diff --git a/deps_install/bin/pcl_add_gaussian_noise b/deps_install/bin/pcl_add_gaussian_noise new file mode 100755 index 0000000..bdc215c Binary files /dev/null and b/deps_install/bin/pcl_add_gaussian_noise differ diff --git a/deps_install/bin/pcl_boundary_estimation b/deps_install/bin/pcl_boundary_estimation new file mode 100755 index 0000000..d2f3b70 Binary files /dev/null and b/deps_install/bin/pcl_boundary_estimation differ diff --git a/deps_install/bin/pcl_cluster_extraction b/deps_install/bin/pcl_cluster_extraction new file mode 100755 index 0000000..a4f9e7a Binary files /dev/null and b/deps_install/bin/pcl_cluster_extraction differ diff --git a/deps_install/bin/pcl_compute_cloud_error b/deps_install/bin/pcl_compute_cloud_error new file mode 100755 index 0000000..dfcb30a Binary files /dev/null and b/deps_install/bin/pcl_compute_cloud_error differ diff --git a/deps_install/bin/pcl_compute_hausdorff b/deps_install/bin/pcl_compute_hausdorff new file mode 100755 index 0000000..b5989d3 Binary files /dev/null and b/deps_install/bin/pcl_compute_hausdorff differ diff --git a/deps_install/bin/pcl_compute_hull b/deps_install/bin/pcl_compute_hull new file mode 100755 index 0000000..a19f0c6 Binary files /dev/null and b/deps_install/bin/pcl_compute_hull differ diff --git a/deps_install/bin/pcl_concatenate_points_pcd b/deps_install/bin/pcl_concatenate_points_pcd new file mode 100755 index 0000000..9364ee1 Binary files /dev/null and b/deps_install/bin/pcl_concatenate_points_pcd differ diff --git a/deps_install/bin/pcl_convert_pcd_ascii_binary b/deps_install/bin/pcl_convert_pcd_ascii_binary new file mode 100755 index 0000000..e750869 Binary files /dev/null and b/deps_install/bin/pcl_convert_pcd_ascii_binary differ diff --git a/deps_install/bin/pcl_crf_segmentation b/deps_install/bin/pcl_crf_segmentation new file mode 100755 index 0000000..9f70be6 Binary files /dev/null and b/deps_install/bin/pcl_crf_segmentation differ diff --git a/deps_install/bin/pcl_crop_to_hull b/deps_install/bin/pcl_crop_to_hull new file mode 100755 index 0000000..d47e306 Binary files /dev/null and b/deps_install/bin/pcl_crop_to_hull differ diff --git a/deps_install/bin/pcl_demean_cloud b/deps_install/bin/pcl_demean_cloud new file mode 100755 index 0000000..94dbd6e Binary files /dev/null and b/deps_install/bin/pcl_demean_cloud differ diff --git a/deps_install/bin/pcl_elch b/deps_install/bin/pcl_elch new file mode 100755 index 0000000..84a4472 Binary files /dev/null and b/deps_install/bin/pcl_elch differ diff --git a/deps_install/bin/pcl_extract_feature b/deps_install/bin/pcl_extract_feature new file mode 100755 index 0000000..e824282 Binary files /dev/null and b/deps_install/bin/pcl_extract_feature differ diff --git a/deps_install/bin/pcl_fast_bilateral_filter b/deps_install/bin/pcl_fast_bilateral_filter new file mode 100755 index 0000000..700b717 Binary files /dev/null and b/deps_install/bin/pcl_fast_bilateral_filter differ diff --git a/deps_install/bin/pcl_fpfh_estimation b/deps_install/bin/pcl_fpfh_estimation new file mode 100755 index 0000000..1d20b67 Binary files /dev/null and b/deps_install/bin/pcl_fpfh_estimation differ diff --git a/deps_install/bin/pcl_generate b/deps_install/bin/pcl_generate new file mode 100755 index 0000000..c64d912 Binary files /dev/null and b/deps_install/bin/pcl_generate differ diff --git a/deps_install/bin/pcl_gp3_surface b/deps_install/bin/pcl_gp3_surface new file mode 100755 index 0000000..cde718d Binary files /dev/null and b/deps_install/bin/pcl_gp3_surface differ diff --git a/deps_install/bin/pcl_grid_min b/deps_install/bin/pcl_grid_min new file mode 100755 index 0000000..b0083ce Binary files /dev/null and b/deps_install/bin/pcl_grid_min differ diff --git a/deps_install/bin/pcl_hdl_grabber b/deps_install/bin/pcl_hdl_grabber new file mode 100755 index 0000000..616cf82 Binary files /dev/null and b/deps_install/bin/pcl_hdl_grabber differ diff --git a/deps_install/bin/pcl_icp b/deps_install/bin/pcl_icp new file mode 100755 index 0000000..ade54de Binary files /dev/null and b/deps_install/bin/pcl_icp differ diff --git a/deps_install/bin/pcl_icp2d b/deps_install/bin/pcl_icp2d new file mode 100755 index 0000000..89669c8 Binary files /dev/null and b/deps_install/bin/pcl_icp2d differ diff --git a/deps_install/bin/pcl_linemod_detection b/deps_install/bin/pcl_linemod_detection new file mode 100755 index 0000000..4476f39 Binary files /dev/null and b/deps_install/bin/pcl_linemod_detection differ diff --git a/deps_install/bin/pcl_local_max b/deps_install/bin/pcl_local_max new file mode 100755 index 0000000..dda6237 Binary files /dev/null and b/deps_install/bin/pcl_local_max differ diff --git a/deps_install/bin/pcl_lum b/deps_install/bin/pcl_lum new file mode 100755 index 0000000..ef19aea Binary files /dev/null and b/deps_install/bin/pcl_lum differ diff --git a/deps_install/bin/pcl_marching_cubes_reconstruction b/deps_install/bin/pcl_marching_cubes_reconstruction new file mode 100755 index 0000000..5a31d02 Binary files /dev/null and b/deps_install/bin/pcl_marching_cubes_reconstruction differ diff --git a/deps_install/bin/pcl_match_linemod_template b/deps_install/bin/pcl_match_linemod_template new file mode 100755 index 0000000..c7bebb8 Binary files /dev/null and b/deps_install/bin/pcl_match_linemod_template differ diff --git a/deps_install/bin/pcl_mls_smoothing b/deps_install/bin/pcl_mls_smoothing new file mode 100755 index 0000000..4653942 Binary files /dev/null and b/deps_install/bin/pcl_mls_smoothing differ diff --git a/deps_install/bin/pcl_morph b/deps_install/bin/pcl_morph new file mode 100755 index 0000000..c548e70 Binary files /dev/null and b/deps_install/bin/pcl_morph differ diff --git a/deps_install/bin/pcl_ndt2d b/deps_install/bin/pcl_ndt2d new file mode 100755 index 0000000..a1b26a7 Binary files /dev/null and b/deps_install/bin/pcl_ndt2d differ diff --git a/deps_install/bin/pcl_ndt3d b/deps_install/bin/pcl_ndt3d new file mode 100755 index 0000000..d429160 Binary files /dev/null and b/deps_install/bin/pcl_ndt3d differ diff --git a/deps_install/bin/pcl_normal_estimation b/deps_install/bin/pcl_normal_estimation new file mode 100755 index 0000000..240dd19 Binary files /dev/null and b/deps_install/bin/pcl_normal_estimation differ diff --git a/deps_install/bin/pcl_oni2pcd b/deps_install/bin/pcl_oni2pcd new file mode 100755 index 0000000..4ea51c8 Binary files /dev/null and b/deps_install/bin/pcl_oni2pcd differ diff --git a/deps_install/bin/pcl_openni_grabber_depth_example b/deps_install/bin/pcl_openni_grabber_depth_example new file mode 100755 index 0000000..578e724 Binary files /dev/null and b/deps_install/bin/pcl_openni_grabber_depth_example differ diff --git a/deps_install/bin/pcl_openni_grabber_example b/deps_install/bin/pcl_openni_grabber_example new file mode 100755 index 0000000..8fe8592 Binary files /dev/null and b/deps_install/bin/pcl_openni_grabber_example differ diff --git a/deps_install/bin/pcl_openni_pcd_recorder b/deps_install/bin/pcl_openni_pcd_recorder new file mode 100755 index 0000000..ab2a122 Binary files /dev/null and b/deps_install/bin/pcl_openni_pcd_recorder differ diff --git a/deps_install/bin/pcl_outlier_removal b/deps_install/bin/pcl_outlier_removal new file mode 100755 index 0000000..2821f75 Binary files /dev/null and b/deps_install/bin/pcl_outlier_removal differ diff --git a/deps_install/bin/pcl_passthrough_filter b/deps_install/bin/pcl_passthrough_filter new file mode 100755 index 0000000..cda43a2 Binary files /dev/null and b/deps_install/bin/pcl_passthrough_filter differ diff --git a/deps_install/bin/pcl_pcd2ply b/deps_install/bin/pcl_pcd2ply new file mode 100755 index 0000000..319a079 Binary files /dev/null and b/deps_install/bin/pcl_pcd2ply differ diff --git a/deps_install/bin/pcl_pcd2vtk b/deps_install/bin/pcl_pcd2vtk new file mode 100755 index 0000000..8e46d9a Binary files /dev/null and b/deps_install/bin/pcl_pcd2vtk differ diff --git a/deps_install/bin/pcl_pcd_change_viewpoint b/deps_install/bin/pcl_pcd_change_viewpoint new file mode 100755 index 0000000..bb05ff0 Binary files /dev/null and b/deps_install/bin/pcl_pcd_change_viewpoint differ diff --git a/deps_install/bin/pcl_pcd_convert_NaN_nan b/deps_install/bin/pcl_pcd_convert_NaN_nan new file mode 100755 index 0000000..168402c Binary files /dev/null and b/deps_install/bin/pcl_pcd_convert_NaN_nan differ diff --git a/deps_install/bin/pcl_pcd_introduce_nan b/deps_install/bin/pcl_pcd_introduce_nan new file mode 100755 index 0000000..ce1b517 Binary files /dev/null and b/deps_install/bin/pcl_pcd_introduce_nan differ diff --git a/deps_install/bin/pcl_pclzf2pcd b/deps_install/bin/pcl_pclzf2pcd new file mode 100755 index 0000000..edb97a2 Binary files /dev/null and b/deps_install/bin/pcl_pclzf2pcd differ diff --git a/deps_install/bin/pcl_plane_projection b/deps_install/bin/pcl_plane_projection new file mode 100755 index 0000000..9b14987 Binary files /dev/null and b/deps_install/bin/pcl_plane_projection differ diff --git a/deps_install/bin/pcl_ply2obj b/deps_install/bin/pcl_ply2obj new file mode 100755 index 0000000..6db6574 Binary files /dev/null and b/deps_install/bin/pcl_ply2obj differ diff --git a/deps_install/bin/pcl_ply2pcd b/deps_install/bin/pcl_ply2pcd new file mode 100755 index 0000000..df227be Binary files /dev/null and b/deps_install/bin/pcl_ply2pcd differ diff --git a/deps_install/bin/pcl_ply2ply b/deps_install/bin/pcl_ply2ply new file mode 100755 index 0000000..e790c86 Binary files /dev/null and b/deps_install/bin/pcl_ply2ply differ diff --git a/deps_install/bin/pcl_ply2raw b/deps_install/bin/pcl_ply2raw new file mode 100755 index 0000000..b5ef0ee Binary files /dev/null and b/deps_install/bin/pcl_ply2raw differ diff --git a/deps_install/bin/pcl_plyheader b/deps_install/bin/pcl_plyheader new file mode 100755 index 0000000..a501e06 Binary files /dev/null and b/deps_install/bin/pcl_plyheader differ diff --git a/deps_install/bin/pcl_poisson_reconstruction b/deps_install/bin/pcl_poisson_reconstruction new file mode 100755 index 0000000..e842f13 Binary files /dev/null and b/deps_install/bin/pcl_poisson_reconstruction differ diff --git a/deps_install/bin/pcl_progressive_morphological_filter b/deps_install/bin/pcl_progressive_morphological_filter new file mode 100755 index 0000000..013bc51 Binary files /dev/null and b/deps_install/bin/pcl_progressive_morphological_filter differ diff --git a/deps_install/bin/pcl_radius_filter b/deps_install/bin/pcl_radius_filter new file mode 100755 index 0000000..598cc42 Binary files /dev/null and b/deps_install/bin/pcl_radius_filter differ diff --git a/deps_install/bin/pcl_sac_segmentation_plane b/deps_install/bin/pcl_sac_segmentation_plane new file mode 100755 index 0000000..9fb8688 Binary files /dev/null and b/deps_install/bin/pcl_sac_segmentation_plane differ diff --git a/deps_install/bin/pcl_spin_estimation b/deps_install/bin/pcl_spin_estimation new file mode 100755 index 0000000..239cf4c Binary files /dev/null and b/deps_install/bin/pcl_spin_estimation differ diff --git a/deps_install/bin/pcl_train_linemod_template b/deps_install/bin/pcl_train_linemod_template new file mode 100755 index 0000000..2eb570a Binary files /dev/null and b/deps_install/bin/pcl_train_linemod_template differ diff --git a/deps_install/bin/pcl_train_unary_classifier b/deps_install/bin/pcl_train_unary_classifier new file mode 100755 index 0000000..41caac4 Binary files /dev/null and b/deps_install/bin/pcl_train_unary_classifier differ diff --git a/deps_install/bin/pcl_transform_from_viewpoint b/deps_install/bin/pcl_transform_from_viewpoint new file mode 100755 index 0000000..4ec93e2 Binary files /dev/null and b/deps_install/bin/pcl_transform_from_viewpoint differ diff --git a/deps_install/bin/pcl_transform_point_cloud b/deps_install/bin/pcl_transform_point_cloud new file mode 100755 index 0000000..0b7ed47 Binary files /dev/null and b/deps_install/bin/pcl_transform_point_cloud differ diff --git a/deps_install/bin/pcl_unary_classifier_segment b/deps_install/bin/pcl_unary_classifier_segment new file mode 100755 index 0000000..7e6b76d Binary files /dev/null and b/deps_install/bin/pcl_unary_classifier_segment differ diff --git a/deps_install/bin/pcl_uniform_sampling b/deps_install/bin/pcl_uniform_sampling new file mode 100755 index 0000000..8ab7eba Binary files /dev/null and b/deps_install/bin/pcl_uniform_sampling differ diff --git a/deps_install/bin/pcl_vfh_estimation b/deps_install/bin/pcl_vfh_estimation new file mode 100755 index 0000000..7834c58 Binary files /dev/null and b/deps_install/bin/pcl_vfh_estimation differ diff --git a/deps_install/bin/pcl_voxel_grid b/deps_install/bin/pcl_voxel_grid new file mode 100755 index 0000000..f771d96 Binary files /dev/null and b/deps_install/bin/pcl_voxel_grid differ diff --git a/deps_install/bin/pcl_xyz2pcd b/deps_install/bin/pcl_xyz2pcd new file mode 100755 index 0000000..00332b3 Binary files /dev/null and b/deps_install/bin/pcl_xyz2pcd differ diff --git a/deps_install/include/pcl-1.10/pcl/2d/convolution.h b/deps_install/include/pcl-1.10/pcl/2d/convolution.h new file mode 100755 index 0000000..3ee3c8e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/convolution.h @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl { + +/// Point cloud containing edge information. +struct EIGEN_ALIGN16 PointXYZIEdge { + PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding + float magnitude; + float direction; + float magnitude_x; + float magnitude_y; + PCL_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned +}; // enforce SSE padding for correct memory alignment + +/// A 2D convolution class. +template +class Convolution : public Filter { +public: + using Filter::input_; + + /** + * Extra pixels are added to the input image so that convolution can be performed over + * the entire image. + * + * (kernel_height/2) rows are added before the first row and after the last row + * (kernel_width/2) columns are added before the first column and after the last + * column border options define what values are set for these extra rows and columns + * + * Assume that the three rows of right edge of the image looks like this: + * .. 3 2 1 + * .. 6 5 4 + * .. 9 8 7 + * + * BOUNDARY_OPTION_CLAMP : the extra pixels are set to the pixel value of the boundary + * pixel. This option makes it seem as if it were: + * .. 3 2 1| 1 1 1 .. + * .. 6 5 4| 4 4 4 .. + * .. 9 8 7| 7 7 7 .. + * + * BOUNDARY_OPTION_MIRROR : the input image is mirrored at the boundary. This option + * makes it seem as if it were: + * .. 3 2 1| 1 2 3 .. + * .. 6 5 4| 4 5 6 .. + * .. 9 8 7| 7 8 9 .. + * + * BOUNDARY_OPTION_ZERO_PADDING : the extra pixels are simply set to 0. This option + * makes it seem as if it were: + * .. 3 2 1| 0 0 0 .. + * .. 6 5 4| 0 0 0 .. + * .. 9 8 7| 0 0 0 .. + * + * Note that the input image is not actually extended in size. Instead, based on these + * options, the convolution is performed differently at the border pixels. + */ + enum BOUNDARY_OPTIONS_ENUM { + BOUNDARY_OPTION_CLAMP, + BOUNDARY_OPTION_MIRROR, + BOUNDARY_OPTION_ZERO_PADDING + }; + + Convolution() { boundary_options_ = BOUNDARY_OPTION_CLAMP; } + + /** \brief Sets the kernel to be used for convolution + * \param[in] kernel convolution kernel passed by reference + */ + inline void + setKernel(const pcl::PointCloud& kernel) + { + kernel_ = kernel; + } + + /** + * \param[in] boundary_options enum indicating the boundary options to be used for + * convolution + */ + inline void + setBoundaryOptions(BOUNDARY_OPTIONS_ENUM boundary_options) + { + boundary_options_ = boundary_options; + } + + /** \brief Performs 2D convolution of the input point cloud with the kernel. + * Uses clamp as the default boundary option. + * \param[out] output Output point cloud passed by reference + */ + void + filter(pcl::PointCloud& output); + +protected: + /** \brief This is an over-ride function for the pcl::Filter interface. */ + void + applyFilter(pcl::PointCloud&) override + {} + +private: + BOUNDARY_OPTIONS_ENUM boundary_options_; + pcl::PointCloud kernel_; +}; +} // namespace pcl + +#include + +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointXYZIEdge, // + (float, x, x) // + (float, y, y) // + (float, z, z) // + (float, magnitude, magnitude) // + (float, direction, direction) // + (float, magnitude_x, magnitude_x) // + (float, magnitude_y, magnitude_y)) // diff --git a/deps_install/include/pcl-1.10/pcl/2d/edge.h b/deps_install/include/pcl-1.10/pcl/2d/edge.h new file mode 100755 index 0000000..504e31e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/edge.h @@ -0,0 +1,313 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl { + +template +class Edge { +private: + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + + PointCloudInPtr input_; + pcl::Convolution convolution_; + kernel kernel_; + + /** \brief This function performs edge tracing for Canny Edge detector. + * + * \param[in] rowOffset row offset for direction in which the edge is to be traced + * \param[in] colOffset column offset for direction in which the edge is to be traced + * \param[in] row row location of the edge point + * \param[in] col column location of the edge point + * \param[out] maxima point cloud containing the edge information in the magnitude + * channel + */ + inline void + cannyTraceEdge(int rowOffset, + int colOffset, + int row, + int col, + pcl::PointCloud& maxima); + + /** \brief This function discretizes the edge directions in steps of 22.5 degrees. + * \param thet point cloud containing the edge information in the direction channel + */ + void + discretizeAngles(pcl::PointCloud& thet); + + /** \brief This function suppresses the edges which don't form a local maximum + * in the edge direction. + * \param[in] edges point cloud containing all the edges + * \param[out] maxima point cloud containing the non-max suppressed edges + * \param[in] tLow + */ + void + suppressNonMaxima(const pcl::PointCloud& edges, + pcl::PointCloud& maxima, + float tLow); + +public: + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + enum OUTPUT_TYPE { + OUTPUT_Y, + OUTPUT_X, + OUTPUT_X_Y, + OUTPUT_MAGNITUDE, + OUTPUT_DIRECTION, + OUTPUT_MAGNITUDE_DIRECTION, + OUTPUT_ALL + }; + + enum DETECTOR_KERNEL_TYPE { + CANNY, + SOBEL, + PREWITT, + ROBERTS, + LOG, + DERIVATIVE_CENTRAL, + DERIVATIVE_FORWARD, + DERIVATIVE_BACKWARD + }; + +private: + OUTPUT_TYPE output_type_; + DETECTOR_KERNEL_TYPE detector_kernel_type_; + bool non_maximal_suppression_; + bool hysteresis_thresholding_; + + float hysteresis_threshold_low_; + float hysteresis_threshold_high_; + float non_max_suppression_radius_x_; + float non_max_suppression_radius_y_; + +public: + Edge() + : output_type_(OUTPUT_X) + , detector_kernel_type_(SOBEL) + , non_maximal_suppression_(false) + , hysteresis_thresholding_(false) + , hysteresis_threshold_low_(20) + , hysteresis_threshold_high_(80) + , non_max_suppression_radius_x_(3) + , non_max_suppression_radius_y_(3) + {} + + /** \brief Set the output type. + * \param[in] output_type the output type + */ + void + setOutputType(OUTPUT_TYPE output_type) + { + output_type_ = output_type; + } + + void + setHysteresisThresholdLow(float threshold) + { + hysteresis_threshold_low_ = threshold; + } + + void + setHysteresisThresholdHigh(float threshold) + { + hysteresis_threshold_high_ = threshold; + } + + /** + * \param[in] input_x + * \param[in] input_y + * \param[out] output + */ + void + sobelMagnitudeDirection(const pcl::PointCloud& input_x, + const pcl::PointCloud& input_y, + pcl::PointCloud& output); + + /** Perform Canny edge detection with two separated input images for horizontal and + * vertical derivatives. + * + * All edges of magnitude above t_high are always classified as edges. All edges + * below t_low are discarded. Edge values between t_low and t_high are classified + * as edges only if they are connected to edges having magnitude > t_high and are + * located in a direction perpendicular to that strong edge. + * + * \param[in] input_x Input point cloud passed by reference for the first derivative + * in the horizontal direction + * \param[in] input_y Input point cloud passed by reference for the first derivative + * in the vertical direction + * \param[out] output Output point cloud passed by reference + */ + void + canny(const pcl::PointCloud& input_x, + const pcl::PointCloud& input_y, + pcl::PointCloud& output); + + /** \brief This is a convenience function which performs edge detection based on + * the variable detector_kernel_type_ + * \param[out] output + */ + void + detectEdge(pcl::PointCloud& output); + + /** \brief All edges of magnitude above t_high are always classified as edges. + * All edges below t_low are discarded. + * Edge values between t_low and t_high are classified as edges only if they are + * connected to edges having magnitude > t_high and are located in a direction + * perpendicular to that strong edge. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeCanny(pcl::PointCloud& output); + + /** \brief Uses the Sobel kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeSobel(pcl::PointCloud& output); + + /** \brief Uses the Prewitt kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgePrewitt(pcl::PointCloud& output); + + /** \brief Uses the Roberts kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeRoberts(pcl::PointCloud& output); + + /** \brief Uses the LoG kernel for edge detection. + * Zero crossings of the Laplacian operator applied on an image indicate edges. + * Gaussian kernel is used to smoothen the image prior to the Laplacian. + * This is because Laplacian uses the second order derivative of the image and hence, + * is very sensitive to noise. The implementation is not two-step but rather applies + * the LoG kernel directly. + * + * \param[in] kernel_sigma variance of the LoG kernel used. + * \param[in] kernel_size a LoG kernel of dimensions kernel_size x kernel_size is + * used. + * \param[out] output Output point cloud passed by reference. + */ + void + detectEdgeLoG(const float kernel_sigma, + const float kernel_size, + pcl::PointCloud& output); + + /** \brief Computes the image derivatives in X direction using the kernel + * kernel::derivativeYCentralKernel. This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeXCentral(pcl::PointCloud& output); + + /** \brief Computes the image derivatives in Y direction using the kernel + * kernel::derivativeYCentralKernel. This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYCentral(pcl::PointCloud& output); + + /** \brief Computes the image derivatives in X direction using the kernel + * kernel::derivativeYForwardKernel. This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeXForward(pcl::PointCloud& output); + + /** \brief Computes the image derivatives in Y direction using the kernel + * kernel::derivativeYForwardKernel. This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYForward(pcl::PointCloud& output); + + /** \brief Computes the image derivatives in X direction using the kernel + * kernel::derivativeXBackwardKernel. This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param output Output point cloud passed by reference + */ + void + computeDerivativeXBackward(pcl::PointCloud& output); + + /** \brief Computes the image derivatives in Y direction using the kernel + * kernel::derivativeYBackwardKernel. This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYBackward(pcl::PointCloud& output); + + /** \brief Override function to implement the pcl::Filter interface */ + void + applyFilter(pcl::PointCloud& /*output*/) + {} + + /** \brief Set the input point cloud pointer + * \param[in] input pointer to input point cloud + */ + void + setInputCloud(PointCloudInPtr input) + { + input_ = input; + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/2d/impl/convolution.hpp b/deps_install/include/pcl-1.10/pcl/2d/impl/convolution.hpp new file mode 100755 index 0000000..3319840 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/impl/convolution.hpp @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_CONVOLUTION_IMPL_HPP +#define PCL_2D_CONVOLUTION_IMPL_HPP + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Convolution::filter(pcl::PointCloud& output) +{ + int input_row = 0; + int input_col = 0; + // default boundary option : zero padding + output = *input_; + + int iw = static_cast(input_->width), ih = static_cast(input_->height), + kw = static_cast(kernel_.width), kh = static_cast(kernel_.height); + switch (boundary_options_) { + default: + case BOUNDARY_OPTION_CLAMP: { + for (int i = 0; i < ih; i++) { + for (int j = 0; j < iw; j++) { + float intensity = 0; + for (int k = 0; k < kh; k++) { + for (int l = 0; l < kw; l++) { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0) + input_row = 0; + else if (ikkh >= ih) + input_row = ih - 1; + else + input_row = ikkh; + + if (jlkw < 0) + input_col = 0; + else if (jlkw >= iw) + input_col = iw - 1; + else + input_col = jlkw; + + intensity += + kernel_(l, k).intensity * (*input_)(input_col, input_row).intensity; + } + } + output(j, i).intensity = intensity; + } + } + break; + } + + case BOUNDARY_OPTION_MIRROR: { + for (int i = 0; i < ih; i++) { + for (int j = 0; j < iw; j++) { + float intensity = 0; + for (int k = 0; k < kh; k++) { + for (int l = 0; l < kw; l++) { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0) + input_row = -ikkh - 1; + else if (ikkh >= ih) + input_row = 2 * ih - 1 - ikkh; + else + input_row = ikkh; + + if (jlkw < 0) + input_col = -jlkw - 1; + else if (jlkw >= iw) + input_col = 2 * iw - 1 - jlkw; + else + input_col = jlkw; + + intensity += + kernel_(l, k).intensity * ((*input_)(input_col, input_row).intensity); + } + } + output(j, i).intensity = intensity; + } + } + break; + } + + case BOUNDARY_OPTION_ZERO_PADDING: { + for (int i = 0; i < ih; i++) { + for (int j = 0; j < iw; j++) { + float intensity = 0; + for (int k = 0; k < kh; k++) { + for (int l = 0; l < kw; l++) { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0 || ikkh >= ih || jlkw < 0 || jlkw >= iw) + continue; + intensity += kernel_(l, k).intensity * ((*input_)(jlkw, ikkh).intensity); + } + } + output(j, i).intensity = intensity; + } + } + break; + } + } // switch +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/2d/impl/edge.hpp b/deps_install/include/pcl-1.10/pcl/2d/impl/edge.hpp new file mode 100755 index 0000000..ded9f69 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/impl/edge.hpp @@ -0,0 +1,473 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_EDGE_IMPL_HPP +#define PCL_2D_EDGE_IMPL_HPP + +#include +#include // rad2deg() + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::detectEdgeSobel(pcl::PointCloud& output) +{ + convolution_.setInputCloud(input_); + pcl::PointCloud::Ptr kernel_x(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x(new pcl::PointCloud); + kernel_.setKernelType(kernel::SOBEL_X); + kernel_.fetchKernel(*kernel_x); + convolution_.setKernel(*kernel_x); + convolution_.filter(*magnitude_x); + + pcl::PointCloud::Ptr kernel_y(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y(new pcl::PointCloud); + kernel_.setKernelType(kernel::SOBEL_Y); + kernel_.fetchKernel(*kernel_y); + convolution_.setKernel(*kernel_y); + convolution_.filter(*magnitude_y); + + const int height = input_->height; + const int width = input_->width; + + output.resize(height * width); + output.height = height; + output.width = width; + + for (std::size_t i = 0; i < output.size(); ++i) { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::sobelMagnitudeDirection( + const pcl::PointCloud& input_x, + const pcl::PointCloud& input_y, + pcl::PointCloud& output) +{ + convolution_.setInputCloud(input_x.makeShared()); + pcl::PointCloud::Ptr kernel_x(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x(new pcl::PointCloud); + kernel_.setKernelType(kernel::SOBEL_X); + kernel_.fetchKernel(*kernel_x); + convolution_.setKernel(*kernel_x); + convolution_.filter(*magnitude_x); + + convolution_.setInputCloud(input_y.makeShared()); + pcl::PointCloud::Ptr kernel_y(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y(new pcl::PointCloud); + kernel_.setKernelType(kernel::SOBEL_Y); + kernel_.fetchKernel(*kernel_y); + convolution_.setKernel(*kernel_y); + convolution_.filter(*magnitude_y); + + const int height = input_x.height; + const int width = input_x.width; + + output.resize(height * width); + output.height = height; + output.width = width; + + for (std::size_t i = 0; i < output.size(); ++i) { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::detectEdgePrewitt(pcl::PointCloud& output) +{ + convolution_.setInputCloud(input_); + + pcl::PointCloud::Ptr kernel_x(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x(new pcl::PointCloud); + kernel_.setKernelType(kernel::PREWITT_X); + kernel_.fetchKernel(*kernel_x); + convolution_.setKernel(*kernel_x); + convolution_.filter(*magnitude_x); + + pcl::PointCloud::Ptr kernel_y(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y(new pcl::PointCloud); + kernel_.setKernelType(kernel::PREWITT_Y); + kernel_.fetchKernel(*kernel_y); + convolution_.setKernel(*kernel_y); + convolution_.filter(*magnitude_y); + + const int height = input_->height; + const int width = input_->width; + + output.resize(height * width); + output.height = height; + output.width = width; + + for (std::size_t i = 0; i < output.size(); ++i) { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::detectEdgeRoberts(pcl::PointCloud& output) +{ + convolution_.setInputCloud(input_); + + pcl::PointCloud::Ptr kernel_x(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x(new pcl::PointCloud); + kernel_.setKernelType(kernel::ROBERTS_X); + kernel_.fetchKernel(*kernel_x); + convolution_.setKernel(*kernel_x); + convolution_.filter(*magnitude_x); + + pcl::PointCloud::Ptr kernel_y(new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y(new pcl::PointCloud); + kernel_.setKernelType(kernel::ROBERTS_Y); + kernel_.fetchKernel(*kernel_y); + convolution_.setKernel(*kernel_y); + convolution_.filter(*magnitude_y); + + const int height = input_->height; + const int width = input_->width; + + output.resize(height * width); + output.height = height; + output.width = width; + + for (std::size_t i = 0; i < output.size(); ++i) { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::cannyTraceEdge( + int rowOffset, int colOffset, int row, int col, pcl::PointCloud& maxima) +{ + int newRow = row + rowOffset; + int newCol = col + colOffset; + PointXYZI& pt = maxima(newCol, newRow); + + if (newRow > 0 && newRow < static_cast(maxima.height) && newCol > 0 && + newCol < static_cast(maxima.width)) { + if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits::max()) + return; + + pt.intensity = std::numeric_limits::max(); + cannyTraceEdge(1, 0, newRow, newCol, maxima); + cannyTraceEdge(-1, 0, newRow, newCol, maxima); + cannyTraceEdge(1, 1, newRow, newCol, maxima); + cannyTraceEdge(-1, -1, newRow, newCol, maxima); + cannyTraceEdge(0, -1, newRow, newCol, maxima); + cannyTraceEdge(0, 1, newRow, newCol, maxima); + cannyTraceEdge(-1, 1, newRow, newCol, maxima); + cannyTraceEdge(1, -1, newRow, newCol, maxima); + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::discretizeAngles(pcl::PointCloud& thet) +{ + const int height = thet.height; + const int width = thet.width; + float angle; + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + angle = pcl::rad2deg(thet(j, i).direction); + if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || + (angle <= -157.5)) + thet(j, i).direction = 0; + else if (((angle > 22.5) && (angle < 67.5)) || + ((angle < -112.5) && (angle > -157.5))) + thet(j, i).direction = 45; + else if (((angle >= 67.5) && (angle <= 112.5)) || + ((angle <= -67.5) && (angle >= -112.5))) + thet(j, i).direction = 90; + else if (((angle > 112.5) && (angle < 157.5)) || + ((angle < -22.5) && (angle > -67.5))) + thet(j, i).direction = 135; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::suppressNonMaxima( + const pcl::PointCloud& edges, + pcl::PointCloud& maxima, + float tLow) +{ + const int height = edges.height; + const int width = edges.width; + + maxima.height = height; + maxima.width = width; + maxima.resize(height * width); + + for (auto& point : maxima) + point.intensity = 0.0f; + + // tHigh and non-maximal supression + for (int i = 1; i < height - 1; i++) { + for (int j = 1; j < width - 1; j++) { + const PointXYZIEdge& ptedge = edges(j, i); + PointXYZI& ptmax = maxima(j, i); + + if (ptedge.magnitude < tLow) + continue; + + // maxima (j, i).intensity = 0; + + switch (int(ptedge.direction)) { + case 0: { + if (ptedge.magnitude >= edges(j - 1, i).magnitude && + ptedge.magnitude >= edges(j + 1, i).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 45: { + if (ptedge.magnitude >= edges(j - 1, i - 1).magnitude && + ptedge.magnitude >= edges(j + 1, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 90: { + if (ptedge.magnitude >= edges(j, i - 1).magnitude && + ptedge.magnitude >= edges(j, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 135: { + if (ptedge.magnitude >= edges(j + 1, i - 1).magnitude && + ptedge.magnitude >= edges(j - 1, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::detectEdgeCanny(pcl::PointCloud& output) +{ + float tHigh = hysteresis_threshold_high_; + float tLow = hysteresis_threshold_low_; + const int height = input_->height; + const int width = input_->width; + + output.resize(height * width); + output.height = height; + output.width = width; + + // Noise reduction using gaussian blurring + pcl::PointCloud::Ptr gaussian_kernel(new pcl::PointCloud); + PointCloudInPtr smoothed_cloud(new PointCloudIn); + kernel_.setKernelSize(3); + kernel_.setKernelSigma(1.0); + kernel_.setKernelType(kernel::GAUSSIAN); + kernel_.fetchKernel(*gaussian_kernel); + convolution_.setKernel(*gaussian_kernel); + convolution_.setInputCloud(input_); + convolution_.filter(*smoothed_cloud); + + // Edge detection using Sobel + pcl::PointCloud::Ptr edges(new pcl::PointCloud); + setInputCloud(smoothed_cloud); + detectEdgeSobel(*edges); + + // Edge discretization + discretizeAngles(*edges); + + // tHigh and non-maximal supression + pcl::PointCloud::Ptr maxima(new pcl::PointCloud); + suppressNonMaxima(*edges, *maxima, tLow); + + // Edge tracing + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + if ((*maxima)(j, i).intensity < tHigh || + (*maxima)(j, i).intensity == std::numeric_limits::max()) + continue; + + (*maxima)(j, i).intensity = std::numeric_limits::max(); + cannyTraceEdge(1, 0, i, j, *maxima); + cannyTraceEdge(-1, 0, i, j, *maxima); + cannyTraceEdge(1, 1, i, j, *maxima); + cannyTraceEdge(-1, -1, i, j, *maxima); + cannyTraceEdge(0, -1, i, j, *maxima); + cannyTraceEdge(0, 1, i, j, *maxima); + cannyTraceEdge(-1, 1, i, j, *maxima); + cannyTraceEdge(1, -1, i, j, *maxima); + } + } + + // Final thresholding + for (std::size_t i = 0; i < input_->size(); ++i) { + if ((*maxima)[i].intensity == std::numeric_limits::max()) + output[i].magnitude = 255; + else + output[i].magnitude = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::canny(const pcl::PointCloud& input_x, + const pcl::PointCloud& input_y, + pcl::PointCloud& output) +{ + float tHigh = hysteresis_threshold_high_; + float tLow = hysteresis_threshold_low_; + const int height = input_x.height; + const int width = input_x.width; + + output.resize(height * width); + output.height = height; + output.width = width; + + // Noise reduction using gaussian blurring + pcl::PointCloud::Ptr gaussian_kernel(new pcl::PointCloud); + kernel_.setKernelSize(3); + kernel_.setKernelSigma(1.0); + kernel_.setKernelType(kernel::GAUSSIAN); + kernel_.fetchKernel(*gaussian_kernel); + convolution_.setKernel(*gaussian_kernel); + + PointCloudIn smoothed_cloud_x; + convolution_.setInputCloud(input_x.makeShared()); + convolution_.filter(smoothed_cloud_x); + + PointCloudIn smoothed_cloud_y; + convolution_.setInputCloud(input_y.makeShared()); + convolution_.filter(smoothed_cloud_y); + + // Edge detection using Sobel + pcl::PointCloud::Ptr edges(new pcl::PointCloud); + sobelMagnitudeDirection(smoothed_cloud_x, smoothed_cloud_y, *edges.get()); + + // Edge discretization + discretizeAngles(*edges); + + pcl::PointCloud::Ptr maxima(new pcl::PointCloud); + suppressNonMaxima(*edges, *maxima, tLow); + + // Edge tracing + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + if ((*maxima)(j, i).intensity < tHigh || + (*maxima)(j, i).intensity == std::numeric_limits::max()) + continue; + + (*maxima)(j, i).intensity = std::numeric_limits::max(); + + // clang-format off + cannyTraceEdge( 1, 0, i, j, *maxima); + cannyTraceEdge(-1, 0, i, j, *maxima); + cannyTraceEdge( 1, 1, i, j, *maxima); + cannyTraceEdge(-1, -1, i, j, *maxima); + cannyTraceEdge( 0, -1, i, j, *maxima); + cannyTraceEdge( 0, 1, i, j, *maxima); + cannyTraceEdge(-1, 1, i, j, *maxima); + cannyTraceEdge( 1, -1, i, j, *maxima); + // clang-format on + } + } + + // Final thresholding + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + if ((*maxima)(j, i).intensity == std::numeric_limits::max()) + output(j, i).magnitude = 255; + else + output(j, i).magnitude = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Edge::detectEdgeLoG(const float kernel_sigma, + const float kernel_size, + pcl::PointCloud& output) +{ + convolution_.setInputCloud(input_); + + pcl::PointCloud::Ptr log_kernel(new pcl::PointCloud); + kernel_.setKernelType(kernel::LOG); + kernel_.setKernelSigma(kernel_sigma); + kernel_.setKernelSize(kernel_size); + kernel_.fetchKernel(*log_kernel); + convolution_.setKernel(*log_kernel); + convolution_.filter(output); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/2d/impl/kernel.hpp b/deps_install/include/pcl-1.10/pcl/2d/impl/kernel.hpp new file mode 100755 index 0000000..d85de7e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/impl/kernel.hpp @@ -0,0 +1,353 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_KERNEL_IMPL_HPP +#define PCL_2D_KERNEL_IMPL_HPP + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::fetchKernel(pcl::PointCloud& kernel) +{ + switch (kernel_type_) { + case SOBEL_X: + sobelKernelX(kernel); + break; + case SOBEL_Y: + sobelKernelY(kernel); + break; + case PREWITT_X: + prewittKernelX(kernel); + break; + case PREWITT_Y: + prewittKernelY(kernel); + break; + case ROBERTS_X: + robertsKernelX(kernel); + break; + case ROBERTS_Y: + robertsKernelY(kernel); + break; + case LOG: + loGKernel(kernel); + break; + case DERIVATIVE_CENTRAL_X: + derivativeXCentralKernel(kernel); + break; + case DERIVATIVE_FORWARD_X: + derivativeXForwardKernel(kernel); + break; + case DERIVATIVE_BACKWARD_X: + derivativeXBackwardKernel(kernel); + break; + case DERIVATIVE_CENTRAL_Y: + derivativeYCentralKernel(kernel); + break; + case DERIVATIVE_FORWARD_Y: + derivativeYForwardKernel(kernel); + break; + case DERIVATIVE_BACKWARD_Y: + derivativeYBackwardKernel(kernel); + break; + case GAUSSIAN: + gaussianKernel(kernel); + break; + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::gaussianKernel(pcl::PointCloud& kernel) +{ + float sum = 0; + kernel.resize(kernel_size_ * kernel_size_); + kernel.height = kernel_size_; + kernel.width = kernel_size_; + + double sigma_sqr = 2 * sigma_ * sigma_; + + for (int i = 0; i < kernel_size_; i++) { + for (int j = 0; j < kernel_size_; j++) { + int iks = (i - kernel_size_ / 2); + int jks = (j - kernel_size_ / 2); + kernel(j, i).intensity = + std::exp(float(-double(iks * iks + jks * jks) / sigma_sqr)); + sum += float(kernel(j, i).intensity); + } + } + + // Normalizing the kernel + for (std::size_t i = 0; i < kernel.size(); ++i) + kernel[i].intensity /= sum; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::loGKernel(pcl::PointCloud& kernel) +{ + float sum = 0; + float temp = 0; + kernel.resize(kernel_size_ * kernel_size_); + kernel.height = kernel_size_; + kernel.width = kernel_size_; + + double sigma_sqr = 2 * sigma_ * sigma_; + + for (int i = 0; i < kernel_size_; i++) { + for (int j = 0; j < kernel_size_; j++) { + int iks = (i - kernel_size_ / 2); + int jks = (j - kernel_size_ / 2); + temp = float(double(iks * iks + jks * jks) / sigma_sqr); + kernel(j, i).intensity = (1.0f - temp) * std::exp(-temp); + sum += kernel(j, i).intensity; + } + } + + // Normalizing the kernel + for (std::size_t i = 0; i < kernel.size(); ++i) + kernel[i].intensity /= sum; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::sobelKernelX(pcl::PointCloud& kernel) +{ + kernel.resize(9); + kernel.height = 3; + kernel.width = 3; + kernel(0, 0).intensity = -1; + kernel(1, 0).intensity = 0; + kernel(2, 0).intensity = 1; + kernel(0, 1).intensity = -2; + kernel(1, 1).intensity = 0; + kernel(2, 1).intensity = 2; + kernel(0, 2).intensity = -1; + kernel(1, 2).intensity = 0; + kernel(2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::prewittKernelX(pcl::PointCloud& kernel) +{ + kernel.resize(9); + kernel.height = 3; + kernel.width = 3; + kernel(0, 0).intensity = -1; + kernel(1, 0).intensity = 0; + kernel(2, 0).intensity = 1; + kernel(0, 1).intensity = -1; + kernel(1, 1).intensity = 0; + kernel(2, 1).intensity = 1; + kernel(0, 2).intensity = -1; + kernel(1, 2).intensity = 0; + kernel(2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::robertsKernelX(pcl::PointCloud& kernel) +{ + kernel.resize(4); + kernel.height = 2; + kernel.width = 2; + kernel(0, 0).intensity = 1; + kernel(1, 0).intensity = 0; + kernel(0, 1).intensity = 0; + kernel(1, 1).intensity = -1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::sobelKernelY(pcl::PointCloud& kernel) +{ + kernel.resize(9); + kernel.height = 3; + kernel.width = 3; + kernel(0, 0).intensity = -1; + kernel(1, 0).intensity = -2; + kernel(2, 0).intensity = -1; + kernel(0, 1).intensity = 0; + kernel(1, 1).intensity = 0; + kernel(2, 1).intensity = 0; + kernel(0, 2).intensity = 1; + kernel(1, 2).intensity = 2; + kernel(2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::prewittKernelY(pcl::PointCloud& kernel) +{ + kernel.resize(9); + kernel.height = 3; + kernel.width = 3; + kernel(0, 0).intensity = 1; + kernel(1, 0).intensity = 1; + kernel(2, 0).intensity = 1; + kernel(0, 1).intensity = 0; + kernel(1, 1).intensity = 0; + kernel(2, 1).intensity = 0; + kernel(0, 2).intensity = -1; + kernel(1, 2).intensity = -1; + kernel(2, 2).intensity = -1; +} + +template +void +pcl::kernel::robertsKernelY(pcl::PointCloud& kernel) +{ + kernel.resize(4); + kernel.height = 2; + kernel.width = 2; + kernel(0, 0).intensity = 0; + kernel(1, 0).intensity = 1; + kernel(0, 1).intensity = -1; + kernel(1, 1).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::derivativeXCentralKernel(pcl::PointCloud& kernel) +{ + kernel.resize(3); + kernel.height = 1; + kernel.width = 3; + kernel(0, 0).intensity = -1; + kernel(1, 0).intensity = 0; + kernel(2, 0).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::derivativeXForwardKernel(pcl::PointCloud& kernel) +{ + kernel.resize(3); + kernel.height = 1; + kernel.width = 3; + kernel(0, 0).intensity = 0; + kernel(1, 0).intensity = -1; + kernel(2, 0).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::derivativeXBackwardKernel(pcl::PointCloud& kernel) +{ + kernel.resize(3); + kernel.height = 1; + kernel.width = 3; + kernel(0, 0).intensity = -1; + kernel(1, 0).intensity = 1; + kernel(2, 0).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::derivativeYCentralKernel(pcl::PointCloud& kernel) +{ + kernel.resize(3); + kernel.height = 3; + kernel.width = 1; + kernel(0, 0).intensity = -1; + kernel(0, 1).intensity = 0; + kernel(0, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::derivativeYForwardKernel(pcl::PointCloud& kernel) +{ + kernel.resize(3); + kernel.height = 3; + kernel.width = 1; + kernel(0, 0).intensity = 0; + kernel(0, 1).intensity = -1; + kernel(0, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::derivativeYBackwardKernel(pcl::PointCloud& kernel) +{ + kernel.resize(3); + kernel.height = 3; + kernel.width = 1; + kernel(0, 0).intensity = -1; + kernel(0, 1).intensity = 1; + kernel(0, 2).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::setKernelType(KERNEL_ENUM kernel_type) +{ + kernel_type_ = kernel_type; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::setKernelSize(int kernel_size) +{ + kernel_size_ = kernel_size; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::kernel::setKernelSigma(float kernel_sigma) +{ + sigma_ = kernel_sigma; +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/2d/impl/morphology.hpp b/deps_install/include/pcl-1.10/pcl/2d/impl/morphology.hpp new file mode 100755 index 0000000..4def02d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/impl/morphology.hpp @@ -0,0 +1,379 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_MORPHOLOGY_HPP_ +#define PCL_2D_MORPHOLOGY_HPP_ + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template +void +pcl::Morphology::erosionBinary(pcl::PointCloud& output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + bool mismatch_flag; + + output.width = width; + output.height = height; + output.resize(width * height); + + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + // Operation done only at 1's + if ((*input_)(j, i).intensity == 0) { + output(j, i).intensity = 0; + continue; + } + mismatch_flag = false; + for (int k = 0; k < kernel_height; k++) { + if (mismatch_flag) + break; + for (int l = 0; l < kernel_width; l++) { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || + (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || + (j + l - kernel_width / 2) >= width) { + continue; + } + // If one of the elements of the kernel and image don't match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2) + .intensity != 1) { + output(j, i).intensity = 0; + mismatch_flag = true; + break; + } + } + } + // Assign value according to mismatch flag + output(j, i).intensity = (mismatch_flag) ? 0 : 1; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template +void +pcl::Morphology::dilationBinary(pcl::PointCloud& output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + bool match_flag; + + output.width = width; + output.height = height; + output.resize(width * height); + + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + match_flag = false; + for (int k = 0; k < kernel_height; k++) { + if (match_flag) + break; + for (int l = 0; l < kernel_width; l++) { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || + (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || + (j + l - kernel_width / 2) >= height) { + continue; + } + // If any position where kernel is 1 and image is also one is detected, + // matching occurs + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2) + .intensity == 1) { + match_flag = true; + break; + } + } + } + // Assign value according to match flag + output(j, i).intensity = (match_flag) ? 1 : 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template +void +pcl::Morphology::openingBinary(pcl::PointCloud& output) +{ + PointCloudInPtr intermediate_output(new PointCloudIn); + erosionBinary(*intermediate_output); + this->setInputCloud(intermediate_output); + dilationBinary(output); +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template +void +pcl::Morphology::closingBinary(pcl::PointCloud& output) +{ + PointCloudInPtr intermediate_output(new PointCloudIn); + dilationBinary(*intermediate_output); + this->setInputCloud(intermediate_output); + erosionBinary(output); +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::erosionGray(pcl::PointCloud& output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + float min; + output.resize(width * height); + output.width = width; + output.height = height; + + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + min = -1; + for (int k = 0; k < kernel_height; k++) { + for (int l = 0; l < kernel_width; l++) { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || + (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || + (j + l - kernel_width / 2) >= width) { + continue; + } + // If one of the elements of the kernel and image don't match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity < + min || + min == -1) { + min = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2) + .intensity; + } + } + } + // Assign value according to mismatch flag + output(j, i).intensity = min; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::dilationGray(pcl::PointCloud& output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + float max; + + output.resize(width * height); + output.width = width; + output.height = height; + + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + max = -1; + for (int k = 0; k < kernel_height; k++) { + for (int l = 0; l < kernel_width; l++) { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || + (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || + (j + l - kernel_width / 2) >= width) { + continue; + } + // If one of the elements of the kernel and image don't match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity > + max || + max == -1) { + max = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2) + .intensity; + } + } + } + // Assign value according to mismatch flag + output(j, i).intensity = max; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::openingGray(pcl::PointCloud& output) +{ + PointCloudInPtr intermediate_output(new PointCloudIn); + erosionGray(*intermediate_output); + this->setInputCloud(intermediate_output); + dilationGray(output); +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::closingGray(pcl::PointCloud& output) +{ + PointCloudInPtr intermediate_output(new PointCloudIn); + dilationGray(*intermediate_output); + this->setInputCloud(intermediate_output); + erosionGray(output); +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::subtractionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2) +{ + const int height = (input1.height < input2.height) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize(height * width); + + for (std::size_t i = 0; i < output.size(); ++i) { + if (input1[i].intensity == 1 && input2[i].intensity == 0) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::unionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2) +{ + const int height = (input1.height < input2.height) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize(height * width); + + for (std::size_t i = 0; i < output.size(); ++i) { + if (input1[i].intensity == 1 || input2[i].intensity == 1) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::intersectionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2) +{ + const int height = (input1.height < input2.height) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize(height * width); + + for (std::size_t i = 0; i < output.size(); ++i) { + if (input1[i].intensity == 1 && input2[i].intensity == 1) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::structuringElementCircular(pcl::PointCloud& kernel, + const int radius) +{ + const int dim = 2 * radius; + kernel.height = dim; + kernel.width = dim; + kernel.resize(dim * dim); + + for (int i = 0; i < dim; i++) { + for (int j = 0; j < dim; j++) { + if (((i - radius) * (i - radius) + (j - radius) * (j - radius)) < radius * radius) + kernel(j, i).intensity = 1; + else + kernel(j, i).intensity = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::structuringElementRectangle(pcl::PointCloud& kernel, + const int height, + const int width) +{ + kernel.height = height; + kernel.width = width; + kernel.resize(height * width); + for (std::size_t i = 0; i < kernel.size(); ++i) + kernel[i].intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template +void +pcl::Morphology::setStructuringElement( + const PointCloudInPtr& structuring_element) +{ + structuring_element_ = structuring_element; +} + +#endif // PCL_2D_MORPHOLOGY_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/2d/kernel.h b/deps_install/include/pcl-1.10/pcl/2d/kernel.h new file mode 100755 index 0000000..499d3a3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/kernel.h @@ -0,0 +1,239 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl { + +template +class kernel { +public: + /// Different types of kernels available. + enum KERNEL_ENUM { + SOBEL_X, //!< SOBEL_X + SOBEL_Y, //!< SOBEL_Y + PREWITT_X, //!< PREWITT_X + PREWITT_Y, //!< PREWITT_Y + ROBERTS_X, //!< ROBERTS_X + ROBERTS_Y, //!< ROBERTS_Y + LOG, //!< LOG + DERIVATIVE_CENTRAL_X, //!< DERIVATIVE_CENTRAL_X + DERIVATIVE_FORWARD_X, //!< DERIVATIVE_FORWARD_X + DERIVATIVE_BACKWARD_X, //!< DERIVATIVE_BACKWARD_X + DERIVATIVE_CENTRAL_Y, //!< DERIVATIVE_CENTRAL_Y + DERIVATIVE_FORWARD_Y, //!< DERIVATIVE_FORWARD_Y + DERIVATIVE_BACKWARD_Y, //!< DERIVATIVE_BACKWARD_Y + GAUSSIAN //!< GAUSSIAN + }; + + int kernel_size_; + float sigma_; + KERNEL_ENUM kernel_type_; + + kernel() : kernel_size_(3), sigma_(1.0), kernel_type_(GAUSSIAN) {} + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Helper function which returns the kernel selected by the kernel_type_ enum + */ + void + fetchKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_ + */ + void + gaussianKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Laplacian of Gaussian kernel with size (kernel_size_ x kernel_size_) and variance + * sigma_ + */ + void + loGKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Sobel kernel in the X direction + */ + void + sobelKernelX(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Prewitt kernel in the X direction + */ + void + prewittKernelX(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 2x2 Roberts kernel in the X direction + */ + void + robertsKernelX(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Sobel kernel in the Y direction + */ + void + sobelKernelY(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Prewitt kernel in the Y direction + */ + void + prewittKernelY(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 2x2 Roberts kernel in the Y direction + */ + void + robertsKernelY(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 0 1] + */ + void + derivativeXCentralKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 0 1]' + */ + void + derivativeYCentralKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [0 -1 1] + */ + void + derivativeXForwardKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [0 -1 1]' + */ + void + derivativeYForwardKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 1 0] + */ + void + derivativeXBackwardKernel(pcl::PointCloud& kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 1 0]' + */ + void + derivativeYBackwardKernel(PointCloud& kernel); + + /** + * + * @param kernel_type enum indicating the kernel type wanted + * + * select the kernel type. + */ + void + setKernelType(KERNEL_ENUM kernel_type); + + /** + * + * @param kernel_size kernel of size kernel_size x kernel_size is created(LoG and + * Gaussian only) + * + * Setter function for kernel_size_ + */ + void + setKernelSize(int kernel_size); + + /** + * + * @param kernel_sigma variance of the Gaussian or LoG kernels. + * + * Setter function for kernel_sigma_ + */ + void + setKernelSigma(float kernel_sigma); +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/2d/morphology.h b/deps_install/include/pcl-1.10/pcl/2d/morphology.h new file mode 100755 index 0000000..b4f9563 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/2d/morphology.h @@ -0,0 +1,206 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl { + +template +class Morphology : public PCLBase { +private: + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + + PointCloudInPtr structuring_element_; + +public: + using PCLBase::input_; + + Morphology() {} + + /** \brief This function performs erosion followed by dilation. + * It is useful for removing noise in the form of small blobs and patches. + * \param[out] output Output point cloud passed by reference + */ + void + openingBinary(pcl::PointCloud& output); + + /** \brief This function performs dilation followed by erosion. + * It is useful for filling up (holes/cracks/small discontinuities) in a binary + * segmented region + * \param[out] output Output point cloud passed by reference + */ + void + closingBinary(pcl::PointCloud& output); + + /** \brief Binary dilation is similar to a logical disjunction of sets. + * At each pixel having value 1, if for all pixels in the structuring element having + * value 1, the corresponding pixels in the input image are also 1, the center pixel + * is set to 1. Otherwise, it is set to 0. + * + * \param[out] output Output point cloud passed by reference + */ + void + erosionBinary(pcl::PointCloud& output); + + /** \brief Binary erosion is similar to a logical addition of sets. + * At each pixel having value 1, if at least one pixel in the structuring element is + * 1 and the corresponding point in the input image is 1, the center pixel is set + * to 1. Otherwise, it is set to 0. + * + * \param[out] output Output point cloud passed by reference + */ + void + dilationBinary(pcl::PointCloud& output); + + /** \brief Grayscale erosion followed by dilation. + * This is used to remove small bright artifacts from the image. Large bright objects + * are relatively undisturbed. + * + * \param[out] output Output point cloud passed by reference + */ + void + openingGray(pcl::PointCloud& output); + + /** \brief Grayscale dilation followed by erosion. + * This is used to remove small dark artifacts from the image. Bright features or + * large dark features are relatively undisturbed. + * + * \param[out] output Output point cloud passed by reference + */ + void + closingGray(pcl::PointCloud& output); + + /** \brief Takes the min of the pixels where kernel is 1 + * \param[out] output Output point cloud passed by reference + */ + void + erosionGray(pcl::PointCloud& output); + + /** \brief Takes the max of the pixels where kernel is 1 + * \param[out] output Output point cloud passed by reference + */ + void + dilationGray(pcl::PointCloud& output); + + /** \brief Set operation + * output = input1 - input2 + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + subtractionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2); + + /** \brief Set operation + * \f$output = input1 \cup input2\f$ + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + unionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2); + + /** \brief Set operation \f$ output = input1 \cap input2 \f$ + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + intersectionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2); + + /** \brief Creates a circular structing element. The size of the kernel created is + * 2*radius x 2*radius. Center of the structuring element is the center of the circle. + * All values lying on the circle are 1 and the others are 0. + * + * \param[out] kernel structuring element kernel passed by reference + * \param[in] radius Radius of the circular structuring element. + */ + void + structuringElementCircular(pcl::PointCloud& kernel, const int radius); + + /** \brief Creates a rectangular structing element of size height x width. * + * All values are 1. + * + * \param[out] kernel structuring element kernel passed by reference + * \param[in] height height number of rows in the structuring element + * \param[in] width number of columns in the structuring element + * + */ + void + structuringElementRectangle(pcl::PointCloud& kernel, + const int height, + const int width); + + enum MORPHOLOGICAL_OPERATOR_TYPE { + EROSION_GRAY, + DILATION_GRAY, + OPENING_GRAY, + CLOSING_GRAY, + EROSION_BINARY, + DILATION_BINARY, + OPENING_BINARY, + CLOSING_BINARY + }; + + MORPHOLOGICAL_OPERATOR_TYPE operator_type; + + /** + * \param[out] output Output point cloud passed by reference + */ + void + applyMorphologicalOperation(pcl::PointCloud& output); + + /** + * \param[in] structuring_element The structuring element to be used for the + * morphological operation + */ + void + setStructuringElement(const PointCloudInPtr& structuring_element); +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ModelCoefficients.h b/deps_install/include/pcl-1.10/pcl/ModelCoefficients.h new file mode 100755 index 0000000..23fedba --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ModelCoefficients.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include + +// Include the correct Header path here +#include + +namespace pcl +{ + struct ModelCoefficients + { + ModelCoefficients () + { + } + + ::pcl::PCLHeader header; + + std::vector values; + + public: + using Ptr = shared_ptr< ::pcl::ModelCoefficients>; + using ConstPtr = shared_ptr; + }; // struct ModelCoefficients + + using ModelCoefficientsPtr = ModelCoefficients::Ptr; + using ModelCoefficientsConstPtr = ModelCoefficients::ConstPtr; + + inline std::ostream& operator<<(std::ostream& s, const ::pcl::ModelCoefficients & v) + { + s << "header: " << std::endl; + s << v.header; + s << "values[]" << std::endl; + for (std::size_t i = 0; i < v.values.size (); ++i) + { + s << " values[" << i << "]: "; + s << " " << v.values[i] << std::endl; + } + return (s); + } + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/PCLHeader.h b/deps_install/include/pcl-1.10/pcl/PCLHeader.h new file mode 100755 index 0000000..8a692d7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/PCLHeader.h @@ -0,0 +1,43 @@ +#pragma once + +#include // for string +#include // for ostream + +#include // for shared_ptr + +namespace pcl +{ + struct PCLHeader + { + /** \brief Sequence number */ + std::uint32_t seq = 0; + /** \brief A timestamp associated with the time when the data was acquired + * + * The value represents microseconds since 1970-01-01 00:00:00 (the UNIX epoch). + */ + std::uint64_t stamp = 0; + /** \brief Coordinate frame ID */ + std::string frame_id; + + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + }; // struct PCLHeader + + using HeaderPtr = PCLHeader::Ptr; + using HeaderConstPtr = PCLHeader::ConstPtr; + + inline std::ostream& operator << (std::ostream& out, const PCLHeader &h) + { + out << "seq: " << h.seq; + out << " stamp: " << h.stamp; + out << " frame_id: " << h.frame_id << std::endl; + return (out); + } + + inline bool operator== (const PCLHeader &lhs, const PCLHeader &rhs) + { + return (&lhs == &rhs) || + (lhs.seq == rhs.seq && lhs.stamp == rhs.stamp && lhs.frame_id == rhs.frame_id); + } + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/PCLImage.h b/deps_install/include/pcl-1.10/pcl/PCLImage.h new file mode 100755 index 0000000..1e8886c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/PCLImage.h @@ -0,0 +1,53 @@ +#pragma once + +#include // for string +#include // for vector +#include // for ostream + +#include // for PCLHeader + +namespace pcl +{ + struct PCLImage + { + ::pcl::PCLHeader header; + + std::uint32_t height = 0; + std::uint32_t width = 0; + std::string encoding; + + std::uint8_t is_bigendian = 0; + std::uint32_t step = 0; + + std::vector data; + + using Ptr = shared_ptr< ::pcl::PCLImage>; + using ConstPtr = shared_ptr; + }; // struct PCLImage + + using PCLImagePtr = PCLImage::Ptr; + using PCLImageConstPtr = PCLImage::ConstPtr; + + inline std::ostream& operator<<(std::ostream& s, const ::pcl::PCLImage & v) + { + s << "header: " << std::endl; + s << v.header; + s << "height: "; + s << " " << v.height << std::endl; + s << "width: "; + s << " " << v.width << std::endl; + s << "encoding: "; + s << " " << v.encoding << std::endl; + s << "is_bigendian: "; + s << " " << v.is_bigendian << std::endl; + s << "step: "; + s << " " << v.step << std::endl; + s << "data[]" << std::endl; + for (std::size_t i = 0; i < v.data.size (); ++i) + { + s << " data[" << i << "]: "; + s << " " << v.data[i] << std::endl; + } + return (s); + } +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/PCLPointCloud2.h b/deps_install/include/pcl-1.10/pcl/PCLPointCloud2.h new file mode 100755 index 0000000..f27ad68 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/PCLPointCloud2.h @@ -0,0 +1,124 @@ +#pragma once + +#include +#include + +#include + +#include +#include + +namespace pcl +{ + + struct PCL_EXPORTS PCLPointCloud2 + { + ::pcl::PCLHeader header; + + std::uint32_t height = 0; + std::uint32_t width = 0; + + std::vector<::pcl::PCLPointField> fields; + + static_assert(BOOST_ENDIAN_BIG_BYTE || BOOST_ENDIAN_LITTLE_BYTE, "unable to determine system endianness"); + std::uint8_t is_bigendian = BOOST_ENDIAN_BIG_BYTE; + std::uint32_t point_step = 0; + std::uint32_t row_step = 0; + + std::vector data; + + std::uint8_t is_dense = 0; + + public: + using Ptr = shared_ptr< ::pcl::PCLPointCloud2>; + using ConstPtr = shared_ptr; + + ////////////////////////////////////////////////////////////////////////// + /** \brief Inplace concatenate two pcl::PCLPointCloud2 + * + * IFF the layout of all the fields in both the clouds is the same, this command + * doesn't remove any fields named "_" (aka marked as skip). For comparison of field + * names, "rgb" and "rgba" are considered equivalent + * However, if the order and/or number of non-skip fields is different, the skip fields + * are dropped and non-skip fields copied selectively. + * This function returns an error if + * * the total number of non-skip fields is different + * * the non-skip field names are named differently (excluding "rbg{a}") in serial order + * * the endian-ness of both clouds is different + * \param[in,out] cloud1 the first input and output point cloud dataset + * \param[in] cloud2 the second input point cloud dataset + * \return true if successful, false if failed (e.g., name/number of fields differs) + */ + static bool + concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2); + + /** \brief Concatenate two pcl::PCLPointCloud2 + * \param[in] cloud1 the first input point cloud dataset + * \param[in] cloud2 the second input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \return true if successful, false if failed (e.g., name/number of fields differs) + */ + static bool + concatenate (const PCLPointCloud2 &cloud1, + const PCLPointCloud2 &cloud2, + PCLPointCloud2 &cloud_out) + { + cloud_out = cloud1; + return concatenate(cloud_out, cloud2); + } + + /** \brief Add a point cloud to the current cloud. + * \param[in] rhs the cloud to add to the current cloud + * \return the new cloud as a concatenation of the current cloud and the new given cloud + */ + PCLPointCloud2& + operator += (const PCLPointCloud2& rhs); + + /** \brief Add a point cloud to another cloud. + * \param[in] rhs the cloud to add to the current cloud + * \return the new cloud as a concatenation of the current cloud and the new given cloud + */ + inline PCLPointCloud2 + operator + (const PCLPointCloud2& rhs) + { + return (PCLPointCloud2 (*this) += rhs); + } + }; // struct PCLPointCloud2 + + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + inline std::ostream& operator<<(std::ostream& s, const ::pcl::PCLPointCloud2 &v) + { + s << "header: " << std::endl; + s << v.header; + s << "height: "; + s << " " << v.height << std::endl; + s << "width: "; + s << " " << v.width << std::endl; + s << "fields[]" << std::endl; + for (std::size_t i = 0; i < v.fields.size (); ++i) + { + s << " fields[" << i << "]: "; + s << std::endl; + s << " " << v.fields[i] << std::endl; + } + s << "is_bigendian: "; + s << " " << v.is_bigendian << std::endl; + s << "point_step: "; + s << " " << v.point_step << std::endl; + s << "row_step: "; + s << " " << v.row_step << std::endl; + s << "data[]" << std::endl; + for (std::size_t i = 0; i < v.data.size (); ++i) + { + s << " data[" << i << "]: "; + s << " " << v.data[i] << std::endl; + } + s << "is_dense: "; + s << " " << v.is_dense << std::endl; + + return (s); + } + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/PCLPointField.h b/deps_install/include/pcl-1.10/pcl/PCLPointField.h new file mode 100755 index 0000000..58f0895 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/PCLPointField.h @@ -0,0 +1,47 @@ +#pragma once + +#include // for string +#include // for ostream + +#include // for shared_ptr + +namespace pcl +{ + struct PCLPointField + { + std::string name; + + std::uint32_t offset = 0; + std::uint8_t datatype = 0; + std::uint32_t count = 0; + + enum PointFieldTypes { INT8 = 1, + UINT8 = 2, + INT16 = 3, + UINT16 = 4, + INT32 = 5, + UINT32 = 6, + FLOAT32 = 7, + FLOAT64 = 8 }; + + public: + using Ptr = shared_ptr< ::pcl::PCLPointField>; + using ConstPtr = shared_ptr; + }; // struct PCLPointField + + using PCLPointFieldPtr = PCLPointField::Ptr; + using PCLPointFieldConstPtr = PCLPointField::ConstPtr; + + inline std::ostream& operator<<(std::ostream& s, const ::pcl::PCLPointField & v) + { + s << "name: "; + s << " " << v.name << std::endl; + s << "offset: "; + s << " " << v.offset << std::endl; + s << "datatype: "; + s << " " << v.datatype << std::endl; + s << "count: "; + s << " " << v.count << std::endl; + return (s); + } +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/PointIndices.h b/deps_install/include/pcl-1.10/pcl/PointIndices.h new file mode 100755 index 0000000..031699f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/PointIndices.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include +#include + +// Include the correct Header path here +#include + +namespace pcl +{ + struct PointIndices + { + PointIndices () + {} + + ::pcl::PCLHeader header; + + std::vector indices; + + public: + using Ptr = shared_ptr< ::pcl::PointIndices>; + using ConstPtr = shared_ptr; + }; // struct PointIndices + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + inline std::ostream& operator << (std::ostream& s, const ::pcl::PointIndices &v) + { + s << "header: " << std::endl; + s << " " << v.header; + s << "indices[]" << std::endl; + for (std::size_t i = 0; i < v.indices.size (); ++i) + { + s << " indices[" << i << "]: "; + s << " " << v.indices[i] << std::endl; + } + return (s); + } +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/PolygonMesh.h b/deps_install/include/pcl-1.10/pcl/PolygonMesh.h new file mode 100755 index 0000000..f62d7b3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/PolygonMesh.h @@ -0,0 +1,119 @@ +#pragma once + +#include +#include +#include +#include + +// Include the correct Header path here +#include +#include +#include + +namespace pcl +{ + struct PolygonMesh + { + PolygonMesh () + {} + + ::pcl::PCLHeader header; + + ::pcl::PCLPointCloud2 cloud; + + std::vector< ::pcl::Vertices> polygons; + + /** \brief Inplace concatenate two pcl::PolygonMesh + * \param[in,out] mesh1 the first input and output mesh + * \param[in] mesh2 the second input mesh + * \return true if successful, false otherwise (unexpected error) + */ + static bool + concatenate (pcl::PolygonMesh &mesh1, const pcl::PolygonMesh &mesh2) + { + bool success = pcl::PCLPointCloud2::concatenate(mesh1.cloud, mesh2.cloud); + if (success == false) { + return false; + } + // Make the resultant polygon mesh take the newest stamp + mesh1.header.stamp = std::max(mesh1.header.stamp, mesh2.header.stamp); + + const auto point_offset = mesh1.cloud.width * mesh1.cloud.height; + std::transform(mesh2.polygons.begin (), + mesh2.polygons.end (), + std::back_inserter (mesh1.polygons), + [point_offset](auto polygon) + { + std::transform(polygon.vertices.begin (), + polygon.vertices.end (), + polygon.vertices.begin (), + [point_offset](auto& point_idx) + { + return point_idx + point_offset; + }); + return polygon; + }); + + return true; + } + + /** \brief Concatenate two pcl::PCLPointCloud2 + * \param[in] mesh1 the first input mesh + * \param[in] mesh2 the second input mesh + * \param[out] mesh_out the resultant output mesh + * \return true if successful, false otherwise (unexpected error) + */ + static bool + concatenate (const PolygonMesh &mesh1, + const PolygonMesh &mesh2, + PolygonMesh &mesh_out) + { + mesh_out = mesh1; + return concatenate(mesh_out, mesh2); + } + + /** \brief Add another polygon mesh to the current mesh. + * \param[in] rhs the mesh to add to the current mesh + * \return the new mesh as a concatenation of the current mesh and the new given mesh + */ + inline PolygonMesh& + operator += (const PolygonMesh& rhs) + { + concatenate((*this), rhs); + return (*this); + } + + /** \brief Add a polygon mesh to another mesh. + * \param[in] rhs the mesh to add to the current mesh + * \return the new mesh as a concatenation of the current mesh and the new given mesh + */ + inline const PolygonMesh + operator + (const PolygonMesh& rhs) + { + return (PolygonMesh (*this) += rhs); + } + + public: + using Ptr = shared_ptr< ::pcl::PolygonMesh>; + using ConstPtr = shared_ptr; + }; // struct PolygonMesh + + using PolygonMeshPtr = PolygonMesh::Ptr; + using PolygonMeshConstPtr = PolygonMesh::ConstPtr; + + inline std::ostream& operator<<(std::ostream& s, const ::pcl::PolygonMesh &v) + { + s << "header: " << std::endl; + s << v.header; + s << "cloud: " << std::endl; + s << v.cloud; + s << "polygons[]" << std::endl; + for (std::size_t i = 0; i < v.polygons.size (); ++i) + { + s << " polygons[" << i << "]: " << std::endl; + s << v.polygons[i]; + } + return (s); + } + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/TextureMesh.h b/deps_install/include/pcl-1.10/pcl/TextureMesh.h new file mode 100755 index 0000000..8811990 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/TextureMesh.h @@ -0,0 +1,105 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \author Khai Tran */ + struct TexMaterial + { + struct RGB + { + float r = 0; + float g = 0; + float b = 0; + }; //RGB + + /** \brief Texture name. */ + std::string tex_name; + + /** \brief Texture file. */ + std::string tex_file; + + /** \brief Defines the ambient color of the material to be (r,g,b). */ + RGB tex_Ka; + + /** \brief Defines the diffuse color of the material to be (r,g,b). */ + RGB tex_Kd; + + /** \brief Defines the specular color of the material to be (r,g,b). This color shows up in highlights. */ + RGB tex_Ks; + + /** \brief Defines the transparency of the material to be alpha. */ + float tex_d; + + /** \brief Defines the shininess of the material to be s. */ + float tex_Ns; + + /** \brief Denotes the illumination model used by the material. + * + * illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used. + * illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required. + */ + int tex_illum; + }; // TexMaterial + + /** \author Khai Tran */ + struct TextureMesh + { + pcl::PCLPointCloud2 cloud; + pcl::PCLHeader header; + + + std::vector > tex_polygons; // polygon which is mapped with specific texture defined in TexMaterial + std::vector > > tex_coordinates; // UV coordinates + std::vector tex_materials; // define texture material + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + }; // struct TextureMesh + + using TextureMeshPtr = TextureMesh::Ptr; + using TextureMeshConstPtr = TextureMesh::ConstPtr; +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/Vertices.h b/deps_install/include/pcl-1.10/pcl/Vertices.h new file mode 100755 index 0000000..cf6ab9f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/Vertices.h @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Describes a set of vertices in a polygon mesh, by basically + * storing an array of indices. + */ + struct Vertices + { + Vertices () + {} + + std::vector vertices; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + }; // struct Vertices + + + using VerticesPtr = Vertices::Ptr; + using VerticesConstPtr = Vertices::ConstPtr; + + inline std::ostream& operator<<(std::ostream& s, const ::pcl::Vertices & v) + { + s << "vertices[]" << std::endl; + for (std::size_t i = 0; i < v.vertices.size (); ++i) + { + s << " vertices[" << i << "]: "; + s << " " << v.vertices[i] << std::endl; + } + return (s); + } +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/cloud_iterator.h b/deps_install/include/pcl-1.10/pcl/cloud_iterator.h new file mode 100755 index 0000000..43356d0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/cloud_iterator.h @@ -0,0 +1,190 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Iterator class for point clouds with or without given indices + * \author Suat Gedikli + */ + template + class CloudIterator + { + public: + CloudIterator (PointCloud& cloud); + + CloudIterator (PointCloud& cloud, const std::vector& indices); + + CloudIterator (PointCloud& cloud, const PointIndices& indices); + + CloudIterator (PointCloud& cloud, const Correspondences& corrs, bool source); + + ~CloudIterator (); + + void operator ++ (); + + void operator ++ (int); + + PointT& operator* () const; + + PointT* operator-> () const; + + unsigned getCurrentPointIndex () const; + + unsigned getCurrentIndex () const; + + /** \brief Size of the range the iterator is going through. Depending on how the CloudIterator was constructed this is the size of the cloud or indices/correspondences. */ + std::size_t size () const; + + void reset (); + + bool isValid () const; + + operator bool () const + { + return isValid (); + } + private: + + class Iterator + { + public: + virtual ~Iterator () {} + + virtual void operator ++ () = 0; + + virtual void operator ++ (int) = 0; + + virtual PointT& operator* () const = 0; + + virtual PointT* operator-> () const = 0; + + virtual unsigned getCurrentPointIndex () const = 0; + + virtual unsigned getCurrentIndex () const = 0; + + /** \brief Size of the range the iterator is going through. Depending on how the CloudIterator was constructed this is the size of the cloud or indices/correspondences. */ + virtual std::size_t size () const = 0; + + virtual void reset () = 0; + + virtual bool isValid () const = 0; + }; + Iterator* iterator_; + }; + + /** \brief Iterator class for point clouds with or without given indices + * \author Suat Gedikli + */ + template + class ConstCloudIterator + { + public: + ConstCloudIterator (const PointCloud& cloud); + + ConstCloudIterator (const PointCloud& cloud, const std::vector& indices); + + ConstCloudIterator (const PointCloud& cloud, const PointIndices& indices); + + ConstCloudIterator (const PointCloud& cloud, const Correspondences& corrs, bool source); + + ~ConstCloudIterator (); + + void operator ++ (); + + void operator ++ (int); + + const PointT& operator* () const; + + const PointT* operator-> () const; + + unsigned getCurrentPointIndex () const; + + unsigned getCurrentIndex () const; + + /** \brief Size of the range the iterator is going through. Depending on how the ConstCloudIterator was constructed this is the size of the cloud or indices/correspondences. */ + std::size_t size () const; + + void reset (); + + bool isValid () const; + + operator bool () const + { + return isValid (); + } + private: + + class Iterator + { + public: + virtual ~Iterator () {} + + virtual void operator ++ () = 0; + + virtual void operator ++ (int) = 0; + + virtual const PointT& operator* () const = 0; + + virtual const PointT* operator-> () const = 0; + + virtual unsigned getCurrentPointIndex () const = 0; + + virtual unsigned getCurrentIndex () const = 0; + + /** \brief Size of the range the iterator is going through. Depending on how the ConstCloudIterator was constructed this is the size of the cloud or indices/correspondences. */ + virtual std::size_t size () const = 0; + + virtual void reset () = 0; + + virtual bool isValid () const = 0; + }; + + class DefaultConstIterator; + class ConstIteratorIdx; + Iterator* iterator_; + }; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/angles.h b/deps_install/include/pcl-1.10/pcl/common/angles.h new file mode 100755 index 0000000..da7acd7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/angles.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +/** + * \file pcl/common/angles.h + * Define standard C methods to do angle calculations + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + /** \brief Convert an angle from radians to degrees + * \param alpha the input angle (in radians) + * \ingroup common + */ + inline float + rad2deg (float alpha); + + /** \brief Convert an angle from degrees to radians + * \param alpha the input angle (in degrees) + * \ingroup common + */ + inline float + deg2rad (float alpha); + + /** \brief Convert an angle from radians to degrees + * \param alpha the input angle (in radians) + * \ingroup common + */ + inline double + rad2deg (double alpha); + + /** \brief Convert an angle from degrees to radians + * \param alpha the input angle (in degrees) + * \ingroup common + */ + inline double + deg2rad (double alpha); + + /** \brief Normalize an angle to (-PI, PI] + * \param alpha the input angle (in radians) + * \ingroup common + */ + inline float + normAngle (float alpha); +} +/*@}*/ +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/bivariate_polynomial.h b/deps_install/include/pcl-1.10/pcl/common/bivariate_polynomial.h new file mode 100755 index 0000000..615320b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/bivariate_polynomial.h @@ -0,0 +1,142 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief This represents a bivariate polynomial and provides some functionality for it + * \author Bastian Steder + * \ingroup common + */ + template + class BivariatePolynomialT + { + public: + //-----CONSTRUCTOR&DESTRUCTOR----- + /** Constructor */ + BivariatePolynomialT (int new_degree=0); + /** Copy constructor */ + BivariatePolynomialT (const BivariatePolynomialT& other); + /** Destructor */ + ~BivariatePolynomialT (); + + //-----OPERATORS----- + /** = operator */ + BivariatePolynomialT& + operator= (const BivariatePolynomialT& other) { deepCopy (other); return *this;} + + //-----METHODS----- + /** Initialize members to default values */ + void + setDegree (int new_degree); + + /** How many parameters has a bivariate polynomial with this degree */ + unsigned int + getNoOfParameters () const { return getNoOfParametersFromDegree (degree);} + + /** Calculate the value of the polynomial at the given point */ + real + getValue (real x, real y) const; + + /** Calculate the gradient of this polynomial + * If forceRecalc is false, it will do nothing when the gradient already exists */ + void + calculateGradient (bool forceRecalc=false); + + /** Calculate the value of the gradient at the given point */ + void + getValueOfGradient (real x, real y, real& gradX, real& gradY); + + /** Returns critical points of the polynomial. type can be 0=maximum, 1=minimum, or 2=saddle point + * !!Currently only implemented for degree 2!! */ + void + findCriticalPoints (std::vector& x_values, std::vector& y_values, std::vector& types) const; + + /** write as binary to a stream */ + void + writeBinary (std::ostream& os) const; + + /** write as binary into a file */ + void + writeBinary (const char* filename) const; + + /** read binary from a stream */ + void + readBinary (std::istream& os); + + /** read binary from a file */ + void + readBinary (const char* filename); + + /** How many parameters has a bivariate polynomial of the given degree */ + static unsigned int + getNoOfParametersFromDegree (int n) { return ((n+2)* (n+1))/2;} + + //-----VARIABLES----- + int degree; + real* parameters; + BivariatePolynomialT* gradient_x, * gradient_y; + + protected: + //-----METHODS----- + /** Delete all members */ + void + memoryCleanUp (); + + /** Create a deep copy of the given polynomial */ + void + deepCopy (const BivariatePolynomialT& other); + //-----VARIABLES----- + }; + + template + std::ostream& + operator<< (std::ostream& os, const BivariatePolynomialT& p); + + using BivariatePolynomiald = BivariatePolynomialT; + using BivariatePolynomial = BivariatePolynomialT; + +} // end namespace + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/boost.h b/deps_install/include/pcl-1.10/pcl/common/boost.h new file mode 100755 index 0000000..3c5d68c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/boost.h @@ -0,0 +1,55 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#ifndef Q_MOC_RUN +// Marking all Boost headers as system headers to remove warnings +#include +#include +#include +#include +#include +#include +#include +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/centroid.h b/deps_install/include/pcl-1.10/pcl/common/centroid.h new file mode 100755 index 0000000..8fde539 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/centroid.h @@ -0,0 +1,1098 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * \file pcl/common/centroid.h + * Define methods for centroid estimation and covariance matrix calculus + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector. + * \param[in] cloud_iterator an iterator over the input point cloud + * \param[out] centroid the output centroid + * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. + * \note if return value is 0, the centroid is not changed, thus not valid. + * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices. + * \ingroup common + */ + template inline unsigned int + compute3DCentroid (ConstCloudIterator &cloud_iterator, + Eigen::Matrix ¢roid); + + template inline unsigned int + compute3DCentroid (ConstCloudIterator &cloud_iterator, + Eigen::Vector4f ¢roid) + { + return (compute3DCentroid (cloud_iterator, centroid)); + } + + template inline unsigned int + compute3DCentroid (ConstCloudIterator &cloud_iterator, + Eigen::Vector4d ¢roid) + { + return (compute3DCentroid (cloud_iterator, centroid)); + } + + /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector. + * \param[in] cloud the input point cloud + * \param[out] centroid the output centroid + * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. + * \note if return value is 0, the centroid is not changed, thus not valid. + * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices. + * \ingroup common + */ + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + Eigen::Matrix ¢roid); + + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + Eigen::Vector4f ¢roid) + { + return (compute3DCentroid (cloud, centroid)); + } + + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + Eigen::Vector4d ¢roid) + { + return (compute3DCentroid (cloud, centroid)); + } + + /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and + * return it as a 3D vector. + * \param[in] cloud the input point cloud + * \param[in] indices the point cloud indices that need to be used + * \param[out] centroid the output centroid + * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input indices. + * \note if return value is 0, the centroid is not changed, thus not valid. + * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices. + * \ingroup common + */ + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix ¢roid); + + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Vector4f ¢roid) + { + return (compute3DCentroid (cloud, indices, centroid)); + } + + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Vector4d ¢roid) + { + return (compute3DCentroid (cloud, indices, centroid)); + } + + /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and + * return it as a 3D vector. + * \param[in] cloud the input point cloud + * \param[in] indices the point cloud indices that need to be used + * \param[out] centroid the output centroid + * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input indices. + * \note if return value is 0, the centroid is not changed, thus not valid. + * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices. + * \ingroup common + */ + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix ¢roid); + + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Vector4f ¢roid) + { + return (compute3DCentroid (cloud, indices, centroid)); + } + + template inline unsigned int + compute3DCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Vector4d ¢roid) + { + return (compute3DCentroid (cloud, indices, centroid)); + } + + /** \brief Compute the 3x3 covariance matrix of a given set of points. + * The result is returned as a Eigen::Matrix3f. + * Note: the covariance matrix is not normalized with the number of + * points. For a normalized covariance, please use + * computeCovarianceMatrixNormalized. + * \param[in] cloud the input point cloud + * \param[in] centroid the centroid of the set of points in the cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input cloud. + * \note if return value is 0, the covariance matrix is not changed, thus not valid. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const Eigen::Vector4f ¢roid, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, centroid, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const Eigen::Vector4d ¢roid, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, centroid, covariance_matrix)); + } + + /** \brief Compute normalized the 3x3 covariance matrix of a given set of points. + * The result is returned as a Eigen::Matrix3f. + * Normalized means that every entry has been divided by the number of points in the point cloud. + * For small number of points, or if you want explicitly the sample-variance, use computeCovarianceMatrix + * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate + * the covariance matrix and is returned by the computeCovarianceMatrix function. + * \param[in] cloud the input point cloud + * \param[in] centroid the centroid of the set of points in the cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input cloud. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const Eigen::Vector4f ¢roid, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const Eigen::Vector4d ¢roid, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix)); + } + + /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices. + * The result is returned as a Eigen::Matrix3f. + * Note: the covariance matrix is not normalized with the number of + * points. For a normalized covariance, please use + * computeCovarianceMatrixNormalized. + * \param[in] cloud the input point cloud + * \param[in] indices the point cloud indices that need to be used + * \param[in] centroid the centroid of the set of points in the cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Vector4f ¢roid, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Vector4d ¢roid, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix)); + } + + /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices. + * The result is returned as a Eigen::Matrix3f. + * Note: the covariance matrix is not normalized with the number of + * points. For a normalized covariance, please use + * computeCovarianceMatrixNormalized. + * \param[in] cloud the input point cloud + * \param[in] indices the point cloud indices that need to be used + * \param[in] centroid the centroid of the set of points in the cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Vector4f ¢roid, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Vector4d ¢roid, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix)); + } + + /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using + * their indices. + * The result is returned as a Eigen::Matrix3f. + * Normalized means that every entry has been divided by the number of entries in indices. + * For small number of points, or if you want explicitly the sample-variance, use computeCovarianceMatrix + * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate + * the covariance matrix and is returned by the computeCovarianceMatrix function. + * \param[in] cloud the input point cloud + * \param[in] indices the point cloud indices that need to be used + * \param[in] centroid the centroid of the set of points in the cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Vector4f ¢roid, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Vector4d ¢roid, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix)); + } + + /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using + * their indices. The result is returned as a Eigen::Matrix3f. + * Normalized means that every entry has been divided by the number of entries in indices. + * For small number of points, or if you want explicitly the sample-variance, use computeCovarianceMatrix + * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate + * the covariance matrix and is returned by the computeCovarianceMatrix function. + * \param[in] cloud the input point cloud + * \param[in] indices the point cloud indices that need to be used + * \param[in] centroid the centroid of the set of points in the cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Vector4f ¢roid, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Vector4d ¢roid, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix)); + } + + /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop. + * Normalized means that every entry has been divided by the number of valid entries in the point cloud. + * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix + * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. + * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. + * \param[in] cloud the input point cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \param[out] centroid the centroid of the set of points in the cloud + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input cloud. + * \ingroup common + */ + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix &covariance_matrix, + Eigen::Matrix ¢roid); + + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix3f &covariance_matrix, + Eigen::Vector4f ¢roid) + { + return (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid)); + } + + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix3d &covariance_matrix, + Eigen::Vector4d ¢roid) + { + return (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid)); + } + + /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop. + * Normalized means that every entry has been divided by the number of entries in indices. + * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix + * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. + * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. + * \param[in] cloud the input point cloud + * \param[in] indices subset of points given by their indices + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \param[out] centroid the centroid of the set of points in the cloud + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix &covariance_matrix, + Eigen::Matrix ¢roid); + + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix3f &covariance_matrix, + Eigen::Vector4f ¢roid) + { + return (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid)); + } + + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix3d &covariance_matrix, + Eigen::Vector4d ¢roid) + { + return (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid)); + } + + /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop. + * Normalized means that every entry has been divided by the number of entries in indices. + * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix + * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. + * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. + * \param[in] cloud the input point cloud + * \param[in] indices subset of points given by their indices + * \param[out] centroid the centroid of the set of points in the cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix &covariance_matrix, + Eigen::Matrix ¢roid); + + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix3f &covariance_matrix, + Eigen::Vector4f ¢roid) + { + return (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid)); + } + + template inline unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix3d &covariance_matrix, + Eigen::Vector4d ¢roid) + { + return (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid)); + } + + /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud. + * Normalized means that every entry has been divided by the number of entries in the input point cloud. + * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix + * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. + * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. + * \param[in] cloud the input point cloud + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input cloud. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, covariance_matrix)); + } + + /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud. + * Normalized means that every entry has been divided by the number of entries in indices. + * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix + * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. + * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. + * \param[in] cloud the input point cloud + * \param[in] indices subset of points given by their indices + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, covariance_matrix)); + } + + /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud. + * Normalized means that every entry has been divided by the number of entries in indices. + * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix + * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. + * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. + * \param[in] cloud the input point cloud + * \param[in] indices subset of points given by their indices + * \param[out] covariance_matrix the resultant 3x3 covariance matrix + * \return number of valid points used to determine the covariance matrix. + * In case of dense point clouds, this is the same as the size of input indices. + * \ingroup common + */ + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix &covariance_matrix); + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix3f &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, covariance_matrix)); + } + + template inline unsigned int + computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix3d &covariance_matrix) + { + return (computeCovarianceMatrix (cloud, indices, covariance_matrix)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned representation + * \param[in] cloud_iterator an iterator over the input point cloud + * \param[in] centroid the centroid of the point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] npts the number of samples guaranteed to be left in the input cloud, accessible by the iterator. If not given, it will be calculated. + * \ingroup common + */ + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out, + int npts = 0); + + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Vector4f ¢roid, + pcl::PointCloud &cloud_out, + int npts = 0) + { + return (demeanPointCloud (cloud_iterator, centroid, cloud_out, npts)); + } + + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Vector4d ¢roid, + pcl::PointCloud &cloud_out, + int npts = 0) + { + return (demeanPointCloud (cloud_iterator, centroid, cloud_out, npts)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned representation + * \param[in] cloud_in the input point cloud + * \param[in] centroid the centroid of the point cloud + * \param[out] cloud_out the resultant output point cloud + * \ingroup common + */ + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out); + + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Vector4f ¢roid, + pcl::PointCloud &cloud_out) + { + return (demeanPointCloud (cloud_iterator, centroid, cloud_out)); + } + + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Vector4d ¢roid, + pcl::PointCloud &cloud_out) + { + return (demeanPointCloud (cloud_iterator, centroid, cloud_out)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned representation + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] centroid the centroid of the point cloud + * \param cloud_out the resultant output point cloud + * \ingroup common + */ + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out); + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Vector4f ¢roid, + pcl::PointCloud &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Vector4d ¢roid, + pcl::PointCloud &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned representation + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] centroid the centroid of the point cloud + * \param cloud_out the resultant output point cloud + * \ingroup common + */ + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices& indices, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out); + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices& indices, + const Eigen::Vector4f ¢roid, + pcl::PointCloud &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices& indices, + const Eigen::Vector4d ¢roid, + pcl::PointCloud &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned + * representation as an Eigen matrix + * \param[in] cloud_iterator an iterator over the input point cloud + * \param[in] centroid the centroid of the point cloud + * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as + * an Eigen matrix (4 rows, N pts columns) + * \param[in] npts the number of samples guaranteed to be left in the input cloud, accessible by the iterator. If not given, it will be calculated. + * \ingroup common + */ + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out, + int npts = 0); + + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Vector4f ¢roid, + Eigen::MatrixXf &cloud_out, + int npts = 0) + { + return (demeanPointCloud (cloud_iterator, centroid, cloud_out, npts)); + } + + template void + demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Vector4d ¢roid, + Eigen::MatrixXd &cloud_out, + int npts = 0) + { + return (demeanPointCloud (cloud_iterator, centroid, cloud_out, npts)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned + * representation as an Eigen matrix + * \param[in] cloud_in the input point cloud + * \param[in] centroid the centroid of the point cloud + * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as + * an Eigen matrix (4 rows, N pts columns) + * \ingroup common + */ + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out); + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const Eigen::Vector4f ¢roid, + Eigen::MatrixXf &cloud_out) + { + return (demeanPointCloud (cloud_in, centroid, cloud_out)); + } + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const Eigen::Vector4d ¢roid, + Eigen::MatrixXd &cloud_out) + { + return (demeanPointCloud (cloud_in, centroid, cloud_out)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned + * representation as an Eigen matrix + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[in] centroid the centroid of the point cloud + * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as + * an Eigen matrix (4 rows, N pts columns) + * \ingroup common + */ + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out); + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Vector4f ¢roid, + Eigen::MatrixXf &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Vector4d ¢roid, + Eigen::MatrixXd &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + /** \brief Subtract a centroid from a point cloud and return the de-meaned + * representation as an Eigen matrix + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[in] centroid the centroid of the point cloud + * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as + * an Eigen matrix (4 rows, N pts columns) + * \ingroup common + */ + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices& indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out); + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices& indices, + const Eigen::Vector4f ¢roid, + Eigen::MatrixXf &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + template void + demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices& indices, + const Eigen::Vector4d ¢roid, + Eigen::MatrixXd &cloud_out) + { + return (demeanPointCloud (cloud_in, indices, centroid, cloud_out)); + } + + /** \brief Helper functor structure for n-D centroid estimation. */ + template + struct NdCentroidFunctor + { + using Pod = typename traits::POD::type; + + NdCentroidFunctor (const PointT &p, Eigen::Matrix ¢roid) + : f_idx_ (0), + centroid_ (centroid), + p_ (reinterpret_cast(p)) { } + + template inline void operator() () + { + using T = typename pcl::traits::datatype::type; + const std::uint8_t* raw_ptr = reinterpret_cast(&p_) + pcl::traits::offset::value; + const T* data_ptr = reinterpret_cast(raw_ptr); + + // Check if the value is invalid + if (!std::isfinite (*data_ptr)) + { + f_idx_++; + return; + } + + centroid_[f_idx_++] += *data_ptr; + } + + private: + int f_idx_; + Eigen::Matrix ¢roid_; + const Pod &p_; + }; + + /** \brief General, all purpose nD centroid estimation for a set of points using their + * indices. + * \param cloud the input point cloud + * \param centroid the output centroid + * \ingroup common + */ + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + Eigen::Matrix ¢roid); + + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + Eigen::VectorXf ¢roid) + { + return (computeNDCentroid (cloud, centroid)); + } + + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + Eigen::VectorXd ¢roid) + { + return (computeNDCentroid (cloud, centroid)); + } + + /** \brief General, all purpose nD centroid estimation for a set of points using their + * indices. + * \param cloud the input point cloud + * \param indices the point cloud indices that need to be used + * \param centroid the output centroid + * \ingroup common + */ + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix ¢roid); + + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::VectorXf ¢roid) + { + return (computeNDCentroid (cloud, indices, centroid)); + } + + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::VectorXd ¢roid) + { + return (computeNDCentroid (cloud, indices, centroid)); + } + + /** \brief General, all purpose nD centroid estimation for a set of points using their + * indices. + * \param cloud the input point cloud + * \param indices the point cloud indices that need to be used + * \param centroid the output centroid + * \ingroup common + */ + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix ¢roid); + + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::VectorXf ¢roid) + { + return (computeNDCentroid (cloud, indices, centroid)); + } + + template inline void + computeNDCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::VectorXd ¢roid) + { + return (computeNDCentroid (cloud, indices, centroid)); + } + +} + +#include + +namespace pcl +{ + + /** A generic class that computes the centroid of points fed to it. + * + * Here by "centroid" we denote not just the mean of 3D point coordinates, + * but also mean of values in the other data fields. The general-purpose + * \ref computeNDCentroid() function also implements this sort of + * functionality, however it does it in a "dumb" way, i.e. regardless of the + * semantics of the data inside a field it simply averages the values. In + * certain cases (e.g. for \c x, \c y, \c z, \c intensity fields) this + * behavior is reasonable, however in other cases (e.g. \c rgb, \c rgba, + * \c label fields) this does not lead to meaningful results. + * + * This class is capable of computing the centroid in a "smart" way, i.e. + * taking into account the meaning of the data inside fields. Currently the + * following fields are supported: + * + * Data | Point fields | Algorithm + * --------- | ------------------------------------- | ------------------------------------------------------------------------------------------- + * XYZ | \c x, \c y, \c z | Average (separate for each field) + * Normal | \c normal_x, \c normal_y, \c normal_z | Average (separate for each field), resulting vector is normalized + * Curvature | \c curvature | Average + * Color | \c rgb or \c rgba | Average (separate for R, G, B, and alpha channels) + * Intensity | \c intensity | Average + * Label | \c label | Majority vote; if several labels have the same largest support then the smaller label wins + * + * The template parameter defines the type of points that may be accumulated + * with this class. This may be an arbitrary PCL point type, and centroid + * computation will happen only for the fields that are present in it and are + * supported. + * + * Current centroid may be retrieved at any time using get(). Note that the + * function is templated on point type, so it is possible to fetch the + * centroid into a point type that differs from the type of points that are + * being accumulated. All the "extra" fields for which the centroid is not + * being calculated will be left untouched. + * + * Example usage: + * + * \code + * // Create and accumulate points + * CentroidPoint centroid; + * centroid.add (pcl::PointXYZ (1, 2, 3); + * centroid.add (pcl::PointXYZ (5, 6, 7); + * // Fetch centroid using `get()` + * pcl::PointXYZ c1; + * centroid.get (c1); + * // The expected result is: c1.x == 3, c1.y == 4, c1.z == 5 + * // It is also okay to use `get()` with a different point type + * pcl::PointXYZRGB c2; + * centroid.get (c2); + * // The expected result is: c2.x == 3, c2.y == 4, c2.z == 5, + * // and c2.rgb is left untouched + * \endcode + * + * \note Assumes that the points being inserted are valid. + * + * \note This class template can be successfully instantiated for *any* + * PCL point type. Of course, each of the field averages is computed only if + * the point type has the corresponding field. + * + * \ingroup common + * \author Sergey Alexandrov */ + template + class CentroidPoint + { + + public: + + CentroidPoint () = default; + + /** Add a new point to the centroid computation. + * + * In this function only the accumulators and point counter are updated, + * actual centroid computation does not happen until get() is called. */ + void + add (const PointT& point); + + /** Retrieve the current centroid. + * + * Computation (division of accumulated values by the number of points + * and normalization where applicable) happens here. The result is not + * cached, so any subsequent call to this function will trigger + * re-computation. + * + * If the number of accumulated points is zero, then the point will be + * left untouched. */ + template void + get (PointOutT& point) const; + + /** Get the total number of points that were added. */ + inline std::size_t + getSize () const + { + return (num_points_); + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + private: + + std::size_t num_points_ = 0; + typename pcl::detail::Accumulators::type accumulators_; + + }; + + /** Compute the centroid of a set of points and return it as a point. + * + * Implementation leverages \ref CentroidPoint class and therefore behaves + * differently from \ref compute3DCentroid() and \ref computeNDCentroid(). + * See \ref CentroidPoint documentation for explanation. + * + * \param[in] cloud input point cloud + * \param[out] centroid output centroid + * + * \return number of valid points used to determine the centroid (will be the + * same as the size of the cloud if it is dense) + * + * \note If return value is \c 0, then the centroid is not changed, thus is + * not valid. + * + * \ingroup common */ + template std::size_t + computeCentroid (const pcl::PointCloud& cloud, + PointOutT& centroid); + + /** Compute the centroid of a set of points and return it as a point. + * \param[in] cloud + * \param[in] indices point cloud indices that need to be used + * \param[out] centroid + * This is an overloaded function provided for convenience. See the + * documentation for computeCentroid(). + * + * \ingroup common */ + template std::size_t + computeCentroid (const pcl::PointCloud& cloud, + const std::vector& indices, + PointOutT& centroid); + +} +/*@}*/ +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/colors.h b/deps_install/include/pcl-1.10/pcl/common/colors.h new file mode 100755 index 0000000..5490c06 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/colors.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + + PCL_EXPORTS RGB + getRandomColor (double min = 0.2, double max = 2.8); + + enum ColorLUTName + { + /** Color lookup table consisting of 256 colors structured in a maximally + * discontinuous manner. Generated using the method of Glasbey et al. + * (see https://github.com/taketwo/glasbey) */ + LUT_GLASBEY, + /** A perceptually uniform colormap created by Stéfan van der Walt and + * Nathaniel Smith for the Python matplotlib library. + * (see https://youtu.be/xAoljeRJ3lU for background and overview) */ + LUT_VIRIDIS, + }; + + template + class ColorLUT + { + + public: + + /** Get a color from the lookup table with a given id. + * + * The id should be less than the size of the LUT (see size()). */ + static RGB at (std::size_t color_id); + + /** Get the number of colors in the lookup table. + * + * Note: the number of colors is different from the number of elements + * in the lookup table (each color is defined by three bytes). */ + static std::size_t size (); + + /** Get a raw pointer to the lookup table. */ + static const unsigned char* data (); + + }; + + using GlasbeyLUT = ColorLUT; + using ViridisLUT = ColorLUT; + +} diff --git a/deps_install/include/pcl-1.10/pcl/common/common.h b/deps_install/include/pcl-1.10/pcl/common/common.h new file mode 100755 index 0000000..49b2210 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/common.h @@ -0,0 +1,205 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +/** + * \file pcl/common/common.h + * Define standard C methods and C++ classes that are common to all methods + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + /** \brief Compute the smallest angle between two 3D vectors in radians (default) or degree. + * \param v1 the first 3D vector (represented as a \a Eigen::Vector4f) + * \param v2 the second 3D vector (represented as a \a Eigen::Vector4f) + * \return the angle between v1 and v2 in radians or degrees + * \note Handles rounding error for parallel and anti-parallel vectors + * \ingroup common + */ + inline double + getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree = false); + + /** \brief Compute the smallest angle between two 3D vectors in radians (default) or degree. + * \param v1 the first 3D vector (represented as a \a Eigen::Vector3f) + * \param v2 the second 3D vector (represented as a \a Eigen::Vector3f) + * \param in_degree determine if angle should be in radians or degrees + * \return the angle between v1 and v2 in radians or degrees + * \ingroup common + */ + inline double + getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree = false); + + + /** \brief Compute both the mean and the standard deviation of an array of values + * \param values the array of values + * \param mean the resultant mean of the distribution + * \param stddev the resultant standard deviation of the distribution + * \ingroup common + */ + inline void + getMeanStd (const std::vector &values, double &mean, double &stddev); + + /** \brief Get a set of points residing in a box given its bounds + * \param cloud the point cloud data message + * \param min_pt the minimum bounds + * \param max_pt the maximum bounds + * \param indices the resultant set of point indices residing in the box + * \ingroup common + */ + template inline void + getPointsInBox (const pcl::PointCloud &cloud, Eigen::Vector4f &min_pt, + Eigen::Vector4f &max_pt, std::vector &indices); + + /** \brief Get the point at maximum distance from a given point and a given pointcloud + * \param cloud the point cloud data message + * \param pivot_pt the point from where to compute the distance + * \param max_pt the point in cloud that is the farthest point away from pivot_pt + * \ingroup common + */ + template inline void + getMaxDistance (const pcl::PointCloud &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt); + + /** \brief Get the point at maximum distance from a given point and a given pointcloud + * \param cloud the point cloud data message + * \param indices the vector of point indices to use from \a cloud + * \param pivot_pt the point from where to compute the distance + * \param max_pt the point in cloud that is the farthest point away from pivot_pt + * \ingroup common + */ + template inline void + getMaxDistance (const pcl::PointCloud &cloud, const std::vector &indices, + const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt); + + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + * \param cloud the point cloud data message + * \param min_pt the resultant minimum bounds + * \param max_pt the resultant maximum bounds + * \ingroup common + */ + template inline void + getMinMax3D (const pcl::PointCloud &cloud, PointT &min_pt, PointT &max_pt); + + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + * \param cloud the point cloud data message + * \param min_pt the resultant minimum bounds + * \param max_pt the resultant maximum bounds + * \ingroup common + */ + template inline void + getMinMax3D (const pcl::PointCloud &cloud, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); + + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + * \param cloud the point cloud data message + * \param indices the vector of point indices to use from \a cloud + * \param min_pt the resultant minimum bounds + * \param max_pt the resultant maximum bounds + * \ingroup common + */ + template inline void + getMinMax3D (const pcl::PointCloud &cloud, const std::vector &indices, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); + + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + * \param cloud the point cloud data message + * \param indices the vector of point indices to use from \a cloud + * \param min_pt the resultant minimum bounds + * \param max_pt the resultant maximum bounds + * \ingroup common + */ + template inline void + getMinMax3D (const pcl::PointCloud &cloud, const pcl::PointIndices &indices, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); + + /** \brief Compute the radius of a circumscribed circle for a triangle formed of three points pa, pb, and pc + * \param pa the first point + * \param pb the second point + * \param pc the third point + * \return the radius of the circumscribed circle + * \ingroup common + */ + template inline double + getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc); + + /** \brief Get the minimum and maximum values on a point histogram + * \param histogram the point representing a multi-dimensional histogram + * \param len the length of the histogram + * \param min_p the resultant minimum + * \param max_p the resultant maximum + * \ingroup common + */ + template inline void + getMinMax (const PointT &histogram, int len, float &min_p, float &max_p); + + /** \brief Calculate the area of a polygon given a point cloud that defines the polygon + * \param polygon point cloud that contains those vertices that comprises the polygon. Vertices are stored in counterclockwise. + * \return the polygon area + * \ingroup common + */ + template inline float + calculatePolygonArea (const pcl::PointCloud &polygon); + + /** \brief Get the minimum and maximum values on a point histogram + * \param cloud the cloud containing multi-dimensional histograms + * \param idx point index representing the histogram that we need to compute min/max for + * \param field_name the field name containing the multi-dimensional histogram + * \param min_p the resultant minimum + * \param max_p the resultant maximum + * \ingroup common + */ + PCL_EXPORTS void + getMinMax (const pcl::PCLPointCloud2 &cloud, int idx, const std::string &field_name, + float &min_p, float &max_p); + + /** \brief Compute both the mean and the standard deviation of an array of values + * \param values the array of values + * \param mean the resultant mean of the distribution + * \param stddev the resultant standard deviation of the distribution + * \ingroup common + */ + PCL_EXPORTS void + getMeanStdDev (const std::vector &values, double &mean, double &stddev); + +} +/*@}*/ +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/common_headers.h b/deps_install/include/pcl-1.10/pcl/common/common_headers.h new file mode 100755 index 0000000..bf68765 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/common_headers.h @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/concatenate.h b/deps_install/include/pcl-1.10/pcl/common/concatenate.h new file mode 100755 index 0000000..5abc232 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/concatenate.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + /** \brief Helper functor structure for concatenate. + * \ingroup common + */ + template + struct NdConcatenateFunctor + { + using PodIn = typename traits::POD::type; + using PodOut = typename traits::POD::type; + + NdConcatenateFunctor (const PointInT &p1, PointOutT &p2) + : p1_ (reinterpret_cast (p1)) + , p2_ (reinterpret_cast (p2)) { } + + template inline void + operator () () + { + // This sucks without Fusion :( + //boost::fusion::at_key (p2_) = boost::fusion::at_key (p1_); + using InT = typename pcl::traits::datatype::type; + using OutT = typename pcl::traits::datatype::type; + // Note: don't currently support different types for the same field (e.g. converting double to float) + BOOST_MPL_ASSERT_MSG ((std::is_same::value), + POINT_IN_AND_POINT_OUT_HAVE_DIFFERENT_TYPES_FOR_FIELD, + (Key, PointInT&, InT, PointOutT&, OutT)); + memcpy (reinterpret_cast(&p2_) + pcl::traits::offset::value, + reinterpret_cast(&p1_) + pcl::traits::offset::value, + sizeof (InT)); + } + + private: + const PodIn &p1_; + PodOut &p2_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/common/copy_point.h b/deps_install/include/pcl-1.10/pcl/common/copy_point.h new file mode 100755 index 0000000..2f490ad --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/copy_point.h @@ -0,0 +1,58 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +namespace pcl +{ + + /** \brief Copy the fields of a source point into a target point. + * + * If the source and the target point types are the same, then a complete + * copy is made. Otherwise only those fields that the two point types share + * in common are copied. + * + * \param[in] point_in the source point + * \param[out] point_out the target point + * + * \ingroup common */ + template void + copyPoint (const PointInT& point_in, PointOutT& point_out); + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/distances.h b/deps_install/include/pcl-1.10/pcl/common/distances.h new file mode 100755 index 0000000..6958ff7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/distances.h @@ -0,0 +1,204 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include + +/** + * \file pcl/common/distances.h + * Define standard C methods to do distance calculations + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + /** \brief Get the shortest 3D segment between two 3D lines + * \param line_a the coefficients of the first line (point, direction) + * \param line_b the coefficients of the second line (point, direction) + * \param pt1_seg the first point on the line segment + * \param pt2_seg the second point on the line segment + * \ingroup common + */ + PCL_EXPORTS void + lineToLineSegment (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, + Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg); + + /** \brief Get the square distance from a point to a line (represented by a point and a direction) + * \param pt a point + * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!) + * \param line_dir the line direction + * \ingroup common + */ + double inline + sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1) + return (line_dir.cross3 (line_pt - pt)).squaredNorm () / line_dir.squaredNorm (); + } + + /** \brief Get the square distance from a point to a line (represented by a point and a direction) + * \note This one is useful if one has to compute many distances to a fixed line, so the vector length can be pre-computed + * \param pt a point + * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!) + * \param line_dir the line direction + * \param sqr_length the squared norm of the line direction + * \ingroup common + */ + double inline + sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, const double sqr_length) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1) + return (line_dir.cross3 (line_pt - pt)).squaredNorm () / sqr_length; + } + + /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points. + * \param[in] cloud the point cloud dataset + * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment) + * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment) + * \return the length of segment length + * \ingroup common + */ + template double inline + getMaxSegment (const pcl::PointCloud &cloud, + PointT &pmin, PointT &pmax) + { + double max_dist = std::numeric_limits::min (); + const auto token = std::numeric_limits::max(); + std::size_t i_min = token, i_max = token; + + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + for (std::size_t j = i; j < cloud.points.size (); ++j) + { + // Compute the distance + double dist = (cloud.points[i].getVector4fMap () - + cloud.points[j].getVector4fMap ()).squaredNorm (); + if (dist <= max_dist) + continue; + + max_dist = dist; + i_min = i; + i_max = j; + } + } + + if (i_min == token || i_max == token) + return (max_dist = std::numeric_limits::min ()); + + pmin = cloud.points[i_min]; + pmax = cloud.points[i_max]; + return (std::sqrt (max_dist)); + } + + /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points. + * \param[in] cloud the point cloud dataset + * \param[in] indices a set of point indices to use from \a cloud + * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment) + * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment) + * \return the length of segment length + * \ingroup common + */ + template double inline + getMaxSegment (const pcl::PointCloud &cloud, const std::vector &indices, + PointT &pmin, PointT &pmax) + { + double max_dist = std::numeric_limits::min (); + const auto token = std::numeric_limits::max(); + std::size_t i_min = token, i_max = token; + + for (std::size_t i = 0; i < indices.size (); ++i) + { + for (std::size_t j = i; j < indices.size (); ++j) + { + // Compute the distance + double dist = (cloud.points[indices[i]].getVector4fMap () - + cloud.points[indices[j]].getVector4fMap ()).squaredNorm (); + if (dist <= max_dist) + continue; + + max_dist = dist; + i_min = i; + i_max = j; + } + } + + if (i_min == token || i_max == token) + return (max_dist = std::numeric_limits::min ()); + + pmin = cloud.points[indices[i_min]]; + pmax = cloud.points[indices[i_max]]; + return (std::sqrt (max_dist)); + } + + /** \brief Calculate the squared euclidean distance between the two given points. + * \param[in] p1 the first point + * \param[in] p2 the second point + */ + template inline float + squaredEuclideanDistance (const PointType1& p1, const PointType2& p2) + { + float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z; + return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z); + } + + /** \brief Calculate the squared euclidean distance between the two given points. + * \param[in] p1 the first point + * \param[in] p2 the second point + */ + template<> inline float + squaredEuclideanDistance (const PointXY& p1, const PointXY& p2) + { + float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y; + return (diff_x*diff_x + diff_y*diff_y); + } + + /** \brief Calculate the euclidean distance between the two given points. + * \param[in] p1 the first point + * \param[in] p2 the second point + */ + template inline float + euclideanDistance (const PointType1& p1, const PointType2& p2) + { + return (std::sqrt (squaredEuclideanDistance (p1, p2))); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/common/eigen.h b/deps_install/include/pcl-1.10/pcl/common/eigen.h new file mode 100755 index 0000000..f5219b3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/eigen.h @@ -0,0 +1,720 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (C) 2010 Gael Guennebaud + * Copyright (C) 2009 Hauke Heibel + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifndef NOMINMAX +#define NOMINMAX +#endif + +#if defined __GNUC__ +# pragma GCC system_header +#elif defined __SUNPRO_CC +# pragma disable_warn +#endif + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Compute the roots of a quadratic polynom x^2 + b*x + c = 0 + * \param[in] b linear parameter + * \param[in] c constant parameter + * \param[out] roots solutions of x^2 + b*x + c = 0 + */ + template void + computeRoots2 (const Scalar &b, const Scalar &c, Roots &roots); + + /** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues + * \param[in] m input matrix + * \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues + */ + template void + computeRoots (const Matrix &m, Roots &roots); + + /** \brief determine the smallest eigenvalue and its corresponding eigenvector + * \param[in] mat input matrix that needs to be symmetric and positive semi definite + * \param[out] eigenvalue the smallest eigenvalue of the input matrix + * \param[out] eigenvector the corresponding eigenvector to the smallest eigenvalue of the input matrix + * \ingroup common + */ + template void + eigen22 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector); + + /** \brief determine the smallest eigenvalue and its corresponding eigenvector + * \param[in] mat input matrix that needs to be symmetric and positive semi definite + * \param[out] eigenvectors the corresponding eigenvector to the smallest eigenvalue of the input matrix + * \param[out] eigenvalues the smallest eigenvalue of the input matrix + * \ingroup common + */ + template void + eigen22 (const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues); + + /** \brief determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi definite input matrix + * \param[in] mat symmetric positive semi definite input matrix + * \param[in] eigenvalue the eigenvalue which corresponding eigenvector is to be computed + * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue + * \ingroup common + */ + template void + computeCorrespondingEigenVector (const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector); + + /** \brief determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix + * \param[in] mat symmetric positive semi definite input matrix + * \param[out] eigenvalue smallest eigenvalue of the input matrix + * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue + * \note if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue. + * \ingroup common + */ + template void + eigen33 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector); + + /** \brief determines the eigenvalues of the symmetric positive semi definite input matrix + * \param[in] mat symmetric positive semi definite input matrix + * \param[out] evals resulting eigenvalues in ascending order + * \ingroup common + */ + template void + eigen33 (const Matrix &mat, Vector &evals); + + /** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix + * \param[in] mat symmetric positive semi definite input matrix + * \param[out] evecs corresponding eigenvectors in correct order according to eigenvalues + * \param[out] evals resulting eigenvalues in ascending order + * \ingroup common + */ + template void + eigen33 (const Matrix &mat, Matrix &evecs, Vector &evals); + + /** \brief Calculate the inverse of a 2x2 matrix + * \param[in] matrix matrix to be inverted + * \param[out] inverse the resultant inverted matrix + * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results + * \return determinant of the original matrix => if 0 no inverse exists => result is invalid + * \ingroup common + */ + template typename Matrix::Scalar + invert2x2 (const Matrix &matrix, Matrix &inverse); + + /** \brief Calculate the inverse of a 3x3 symmetric matrix. + * \param[in] matrix matrix to be inverted + * \param[out] inverse the resultant inverted matrix + * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results + * \return determinant of the original matrix => if 0 no inverse exists => result is invalid + * \ingroup common + */ + template typename Matrix::Scalar + invert3x3SymMatrix (const Matrix &matrix, Matrix &inverse); + + /** \brief Calculate the inverse of a general 3x3 matrix. + * \param[in] matrix matrix to be inverted + * \param[out] inverse the resultant inverted matrix + * \return determinant of the original matrix => if 0 no inverse exists => result is invalid + * \ingroup common + */ + template typename Matrix::Scalar + invert3x3Matrix (const Matrix &matrix, Matrix &inverse); + + /** \brief Calculate the determinant of a 3x3 matrix. + * \param[in] matrix matrix + * \return determinant of the matrix + * \ingroup common + */ + template typename Matrix::Scalar + determinant3x3Matrix (const Matrix &matrix); + + /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector + * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) + * \param[in] z_axis the z-axis + * \param[in] y_direction the y direction + * \param[out] transformation the resultant 3D rotation + * \ingroup common + */ + inline void + getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, + const Eigen::Vector3f& y_direction, + Eigen::Affine3f& transformation); + + /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector + * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) + * \param[in] z_axis the z-axis + * \param[in] y_direction the y direction + * \return the resultant 3D rotation + * \ingroup common + */ + inline Eigen::Affine3f + getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, + const Eigen::Vector3f& y_direction); + + /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector + * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) + * \param[in] x_axis the x-axis + * \param[in] y_direction the y direction + * \param[out] transformation the resultant 3D rotation + * \ingroup common + */ + inline void + getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, + const Eigen::Vector3f& y_direction, + Eigen::Affine3f& transformation); + + /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector + * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) + * \param[in] x_axis the x-axis + * \param[in] y_direction the y direction + * \return the resulting 3D rotation + * \ingroup common + */ + inline Eigen::Affine3f + getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, + const Eigen::Vector3f& y_direction); + + /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector + * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) + * \param[in] y_direction the y direction + * \param[in] z_axis the z-axis + * \param[out] transformation the resultant 3D rotation + * \ingroup common + */ + inline void + getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, + const Eigen::Vector3f& z_axis, + Eigen::Affine3f& transformation); + + /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector + * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) + * \param[in] y_direction the y direction + * \param[in] z_axis the z-axis + * \return transformation the resultant 3D rotation + * \ingroup common + */ + inline Eigen::Affine3f + getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, + const Eigen::Vector3f& z_axis); + + /** \brief Get the transformation that will translate \a origin to (0,0,0) and rotate \a z_axis into (0,0,1) + * and \a y_direction into a vector with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) + * \param[in] y_direction the y direction + * \param[in] z_axis the z-axis + * \param[in] origin the origin + * \param[in] transformation the resultant transformation matrix + * \ingroup common + */ + inline void + getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, + const Eigen::Vector3f& z_axis, + const Eigen::Vector3f& origin, + Eigen::Affine3f& transformation); + + /** \brief Extract the Euler angles (XYZ-convention) from the given transformation + * \param[in] t the input transformation matrix + * \param[in] roll the resulting roll angle + * \param[in] pitch the resulting pitch angle + * \param[in] yaw the resulting yaw angle + * \ingroup common + */ + template void + getEulerAngles (const Eigen::Transform &t, Scalar &roll, Scalar &pitch, Scalar &yaw); + + inline void + getEulerAngles (const Eigen::Affine3f &t, float &roll, float &pitch, float &yaw) + { + getEulerAngles (t, roll, pitch, yaw); + } + + inline void + getEulerAngles (const Eigen::Affine3d &t, double &roll, double &pitch, double &yaw) + { + getEulerAngles (t, roll, pitch, yaw); + } + + /** Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation + * \param[in] t the input transformation matrix + * \param[out] x the resulting x translation + * \param[out] y the resulting y translation + * \param[out] z the resulting z translation + * \param[out] roll the resulting roll angle + * \param[out] pitch the resulting pitch angle + * \param[out] yaw the resulting yaw angle + * \ingroup common + */ + template void + getTranslationAndEulerAngles (const Eigen::Transform &t, + Scalar &x, Scalar &y, Scalar &z, + Scalar &roll, Scalar &pitch, Scalar &yaw); + + inline void + getTranslationAndEulerAngles (const Eigen::Affine3f &t, + float &x, float &y, float &z, + float &roll, float &pitch, float &yaw) + { + getTranslationAndEulerAngles (t, x, y, z, roll, pitch, yaw); + } + + inline void + getTranslationAndEulerAngles (const Eigen::Affine3d &t, + double &x, double &y, double &z, + double &roll, double &pitch, double &yaw) + { + getTranslationAndEulerAngles (t, x, y, z, roll, pitch, yaw); + } + + /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention) + * \param[in] x the input x translation + * \param[in] y the input y translation + * \param[in] z the input z translation + * \param[in] roll the input roll angle + * \param[in] pitch the input pitch angle + * \param[in] yaw the input yaw angle + * \param[out] t the resulting transformation matrix + * \ingroup common + */ + template void + getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, + Eigen::Transform &t); + + inline void + getTransformation (float x, float y, float z, float roll, float pitch, float yaw, + Eigen::Affine3f &t) + { + return (getTransformation (x, y, z, roll, pitch, yaw, t)); + } + + inline void + getTransformation (double x, double y, double z, double roll, double pitch, double yaw, + Eigen::Affine3d &t) + { + return (getTransformation (x, y, z, roll, pitch, yaw, t)); + } + + /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention) + * \param[in] x the input x translation + * \param[in] y the input y translation + * \param[in] z the input z translation + * \param[in] roll the input roll angle + * \param[in] pitch the input pitch angle + * \param[in] yaw the input yaw angle + * \return the resulting transformation matrix + * \ingroup common + */ + inline Eigen::Affine3f + getTransformation (float x, float y, float z, float roll, float pitch, float yaw) + { + Eigen::Affine3f t; + getTransformation (x, y, z, roll, pitch, yaw, t); + return (t); + } + + /** \brief Write a matrix to an output stream + * \param[in] matrix the matrix to output + * \param[out] file the output stream + * \ingroup common + */ + template void + saveBinary (const Eigen::MatrixBase& matrix, std::ostream& file); + + /** \brief Read a matrix from an input stream + * \param[out] matrix the resulting matrix, read from the input stream + * \param[in,out] file the input stream + * \ingroup common + */ + template void + loadBinary (Eigen::MatrixBase const& matrix, std::istream& file); + +// PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1, +// followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over +// finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3. +#define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \ + : (int (a) == 1 || int (b) == 1) ? 1 \ + : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \ + : (int (a) <= int (b)) ? int (a) : int (b)) + + /** \brief Returns the transformation between two point sets. + * The algorithm is based on: + * "Least-squares estimation of transformation parameters between two point patterns", + * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573 + * + * It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that + * \f{align*} + * \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2 + * \f} + * is minimized. + * + * The algorithm is based on the analysis of the covariance matrix + * \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$ + * of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where + * \f$d\f$ is corresponding to the dimension (which is typically small). + * The analysis is involving the SVD having a complexity of \f$O(d^3)\f$ + * though the actual computational effort lies in the covariance + * matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when + * the input point sets have dimension \f$d \times m\f$. + * + * \param[in] src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$ + * \param[in] dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$. + * \param[in] with_scaling Sets \f$ c=1 \f$ when false is passed. (default: false) + * \return The homogeneous transformation + * \f{align*} + * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} + * \f} + * minimizing the resudiual above. This transformation is always returned as an + * Eigen::Matrix. + */ + template + typename Eigen::internal::umeyama_transform_matrix_type::type + umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase& dst, bool with_scaling = false); + +/** \brief Transform a point using an affine matrix + * \param[in] point_in the vector to be transformed + * \param[out] point_out the transformed vector + * \param[in] transformation the transformation matrix + * + * \note Can be used with \c point_in = \c point_out + */ + template inline void + transformPoint (const Eigen::Matrix &point_in, + Eigen::Matrix &point_out, + const Eigen::Transform &transformation) + { + Eigen::Matrix point; + point << point_in, 1.0; + point_out = (transformation * point).template head<3> (); + } + + inline void + transformPoint (const Eigen::Vector3f &point_in, + Eigen::Vector3f &point_out, + const Eigen::Affine3f &transformation) + { + transformPoint (point_in, point_out, transformation); + } + + inline void + transformPoint (const Eigen::Vector3d &point_in, + Eigen::Vector3d &point_out, + const Eigen::Affine3d &transformation) + { + transformPoint (point_in, point_out, transformation); + } + +/** \brief Transform a vector using an affine matrix + * \param[in] vector_in the vector to be transformed + * \param[out] vector_out the transformed vector + * \param[in] transformation the transformation matrix + * + * \note Can be used with \c vector_in = \c vector_out + */ + template inline void + transformVector (const Eigen::Matrix &vector_in, + Eigen::Matrix &vector_out, + const Eigen::Transform &transformation) + { + vector_out = transformation.linear () * vector_in; + } + + inline void + transformVector (const Eigen::Vector3f &vector_in, + Eigen::Vector3f &vector_out, + const Eigen::Affine3f &transformation) + { + transformVector (vector_in, vector_out, transformation); + } + + inline void + transformVector (const Eigen::Vector3d &vector_in, + Eigen::Vector3d &vector_out, + const Eigen::Affine3d &transformation) + { + transformVector (vector_in, vector_out, transformation); + } + +/** \brief Transform a line using an affine matrix + * \param[in] line_in the line to be transformed + * \param[out] line_out the transformed line + * \param[in] transformation the transformation matrix + * + * Lines must be filled in this form:\n + * line[0-2] = Origin coordinates of the vector\n + * line[3-5] = Direction vector + * + * \note Can be used with \c line_in = \c line_out + */ + template bool + transformLine (const Eigen::Matrix &line_in, + Eigen::Matrix &line_out, + const Eigen::Transform &transformation); + + inline bool + transformLine (const Eigen::VectorXf &line_in, + Eigen::VectorXf &line_out, + const Eigen::Affine3f &transformation) + { + return (transformLine (line_in, line_out, transformation)); + } + + inline bool + transformLine (const Eigen::VectorXd &line_in, + Eigen::VectorXd &line_out, + const Eigen::Affine3d &transformation) + { + return (transformLine (line_in, line_out, transformation)); + } + +/** \brief Transform plane vectors using an affine matrix + * \param[in] plane_in the plane coefficients to be transformed + * \param[out] plane_out the transformed plane coefficients to fill + * \param[in] transformation the transformation matrix + * + * The plane vectors are filled in the form ax+by+cz+d=0 + * Can be used with non Hessian form planes coefficients + * Can be used with \c plane_in = \c plane_out + */ + template void + transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation); + + inline void + transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + + inline void + transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + +/** \brief Transform plane vectors using an affine matrix + * \param[in] plane_in the plane coefficients to be transformed + * \param[out] plane_out the transformed plane coefficients to fill + * \param[in] transformation the transformation matrix + * + * The plane vectors are filled in the form ax+by+cz+d=0 + * Can be used with non Hessian form planes coefficients + * Can be used with \c plane_in = \c plane_out + * \warning ModelCoefficients stores floats only ! + */ + template void + transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation); + + inline void + transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + + inline void + transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation) + { + transformPlane (plane_in, plane_out, transformation); + } + +/** \brief Check coordinate system integrity + * \param[in] line_x the first axis + * \param[in] line_y the second axis + * \param[in] norm_limit the limit to ignore norm rounding errors + * \param[in] dot_limit the limit to ignore dot product rounding errors + * \return True if the coordinate system is consistent, false otherwise. + * + * Lines must be filled in this form:\n + * line[0-2] = Origin coordinates of the vector\n + * line[3-5] = Direction vector + * + * Can be used like this :\n + * line_x = X axis and line_y = Y axis\n + * line_x = Z axis and line_y = X axis\n + * line_x = Y axis and line_y = Z axis\n + * Because X^Y = Z, Z^X = Y and Y^Z = X. + * Do NOT invert line order ! + * + * Determine whether a coordinate system is consistent or not by checking :\n + * Line origins: They must be the same for the 2 lines\n + * Norm: The 2 lines must be normalized\n + * Dot products: Must be 0 or perpendicular vectors + */ + template bool + checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const Scalar norm_limit = 1e-3, + const Scalar dot_limit = 1e-3); + + inline bool + checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const double norm_limit = 1e-3, + const double dot_limit = 1e-3) + { + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + + inline bool + checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const float norm_limit = 1e-3, + const float dot_limit = 1e-3) + { + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + +/** \brief Check coordinate system integrity + * \param[in] origin the origin of the coordinate system + * \param[in] x_direction the first axis + * \param[in] y_direction the second axis + * \param[in] norm_limit the limit to ignore norm rounding errors + * \param[in] dot_limit the limit to ignore dot product rounding errors + * \return True if the coordinate system is consistent, false otherwise. + * + * Read the other variant for more information + */ + template inline bool + checkCoordinateSystem (const Eigen::Matrix &origin, + const Eigen::Matrix &x_direction, + const Eigen::Matrix &y_direction, + const Scalar norm_limit = 1e-3, + const Scalar dot_limit = 1e-3) + { + Eigen::Matrix line_x; + Eigen::Matrix line_y; + line_x << origin, x_direction; + line_y << origin, y_direction; + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + + inline bool + checkCoordinateSystem (const Eigen::Matrix &origin, + const Eigen::Matrix &x_direction, + const Eigen::Matrix &y_direction, + const double norm_limit = 1e-3, + const double dot_limit = 1e-3) + { + Eigen::Matrix line_x; + Eigen::Matrix line_y; + line_x.resize (6); + line_y.resize (6); + line_x << origin, x_direction; + line_y << origin, y_direction; + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + + inline bool + checkCoordinateSystem (const Eigen::Matrix &origin, + const Eigen::Matrix &x_direction, + const Eigen::Matrix &y_direction, + const float norm_limit = 1e-3, + const float dot_limit = 1e-3) + { + Eigen::Matrix line_x; + Eigen::Matrix line_y; + line_x.resize (6); + line_y.resize (6); + line_x << origin, x_direction; + line_y << origin, y_direction; + return (checkCoordinateSystem (line_x, line_y, norm_limit, dot_limit)); + } + +/** \brief Compute the transformation between two coordinate systems + * \param[in] from_line_x X axis from the origin coordinate system + * \param[in] from_line_y Y axis from the origin coordinate system + * \param[in] to_line_x X axis from the destination coordinate system + * \param[in] to_line_y Y axis from the destination coordinate system + * \param[out] transformation the transformation matrix to fill + * \return true if transformation was filled, false otherwise. + * + * Line must be filled in this form:\n + * line[0-2] = Coordinate system origin coordinates \n + * line[3-5] = Direction vector (norm doesn't matter) + */ + template bool + transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation); + + inline bool + transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation) + { + return (transformBetween2CoordinateSystems (from_line_x, from_line_y, to_line_x, to_line_y, transformation)); + } + + inline bool + transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation) + { + return (transformBetween2CoordinateSystems (from_line_x, from_line_y, to_line_x, to_line_y, transformation)); + } + +} + +#include + +#if defined __SUNPRO_CC +# pragma enable_warn +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/feature_histogram.h b/deps_install/include/pcl-1.10/pcl/common/feature_histogram.h new file mode 100755 index 0000000..332368e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/feature_histogram.h @@ -0,0 +1,123 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + /** \brief Type for histograms for computing mean and variance of some floats. + * + * \author Timur Ibadov (ibadov.timur@gmail.com) + * \ingroup common + */ + class PCL_EXPORTS FeatureHistogram + { + public: + /** \brief Public constructor. + * \param[in] number_of_bins number of bins in the histogram. + * \param[in] min lower threshold. + * \param[in] max upper threshold. + */ + FeatureHistogram (const std::size_t number_of_bins, const float min, + const float max); + + /** \brief Public destructor. */ + virtual ~FeatureHistogram (); + + /** \brief Get the lower threshold. + * \return lower threshold. + */ + float + getThresholdMin () const; + + /** \brief Get the upper threshold. + * \return upper threshold. + */ + float + getThresholdMax () const; + + /** \brief Get the number of elements was added to the histogram. + * \return number of elements in the histogram. + */ + std::size_t + getNumberOfElements () const; + + /** \brief Get number of bins in the histogram. + * \return number of bins in the histogram. + */ + std::size_t + getNumberOfBins () const; + + /** \brief Increase a bin, that corresponds the value. + * \param[in] value new value. + */ + void + addValue (float value); + + /** \brief Get value, corresponds to the greatest bin. + * \return mean value of the greatest bin. + */ + float + getMeanValue (); + + /** \brief Get variance of the value. + * \return variance of the greatest bin. + */ + float + getVariance (float mean); + + protected: + /** \brief Vector, that contain the histogram. */ + std::vector histogram_; + + /** \brief Min threshold. */ + float threshold_min_; + /** \brief Max threshold. */ + float threshold_max_; + /** \brief "Width" of a bin. */ + float step_; + + /** \brief Number of values was added to the histogram. */ + std::size_t number_of_elements_; + + /** \brief Number of bins. */ + std::size_t number_of_bins_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/common/fft/_kiss_fft_guts.h b/deps_install/include/pcl-1.10/pcl/common/fft/_kiss_fft_guts.h new file mode 100755 index 0000000..8e3b56d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/fft/_kiss_fft_guts.h @@ -0,0 +1,164 @@ +/* +Copyright (c) 2003-2010, Mark Borgerding + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +/* kiss_fft.h + defines kiss_fft_scalar as either short or a float type + and defines + struct kiss_fft_cpx { kiss_fft_scalar r; kiss_fft_scalar i; }; */ +#include "kiss_fft.h" +#include + +#define MAXFACTORS 32 +/* e.g. an fft of length 128 has 4 factors + as far as kissfft is concerned + 4*4*4*2 + */ + +struct kiss_fft_state{ + int nfft; + int inverse; + int factors[2*MAXFACTORS]; + kiss_fft_cpx twiddles[1]; +}; + +/* + Explanation of macros dealing with complex math: + + C_MUL(m,a,b) : m = a*b + C_FIXDIV( c , div ) : if a fixed point impl., c /= div. noop otherwise + C_SUB( res, a,b) : res = a - b + C_SUBFROM( res , a) : res -= a + C_ADDTO( res , a) : res += a + * */ +#ifdef FIXED_POINT +#if (FIXED_POINT==32) +# define FRACBITS 31 +# define SAMPPROD int64_t +#define SAMP_MAX 2147483647 +#else +# define FRACBITS 15 +# define SAMPPROD int32_t +#define SAMP_MAX 32767 +#endif + +#define SAMP_MIN -SAMP_MAX + +#if defined(CHECK_OVERFLOW) +# define CHECK_OVERFLOW_OP(a,op,b) \ + if ( (SAMPPROD)(a) op (SAMPPROD)(b) > SAMP_MAX || (SAMPPROD)(a) op (SAMPPROD)(b) < SAMP_MIN ) { \ + fprintf(stderr,"WARNING:overflow @ " __FILE__ "(%d): (%d " #op" %d) = %ld\n",__LINE__,(a),(b),(SAMPPROD)(a) op (SAMPPROD)(b) ); } +#endif + + +# define smul(a,b) ( (SAMPPROD)(a)*(b) ) +# define sround( x ) (kiss_fft_scalar)( ( (x) + (1<<(FRACBITS-1)) ) >> FRACBITS ) + +# define S_MUL(a,b) sround( smul(a,b) ) + +# define C_MUL(m,a,b) \ + do{ (m).r = sround( smul((a).r,(b).r) - smul((a).i,(b).i) ); \ + (m).i = sround( smul((a).r,(b).i) + smul((a).i,(b).r) ); }while(0) + +# define DIVSCALAR(x,k) \ + (x) = sround( smul( x, SAMP_MAX/k ) ) + +# define C_FIXDIV(c,div) \ + do { DIVSCALAR( (c).r , div); \ + DIVSCALAR( (c).i , div); }while (0) + +# define C_MULBYSCALAR( c, s ) \ + do{ (c).r = sround( smul( (c).r , s ) ) ;\ + (c).i = sround( smul( (c).i , s ) ) ; }while(0) + +#else /* not FIXED_POINT*/ + +# define S_MUL(a,b) ( (a)*(b) ) +#define C_MUL(m,a,b) \ + do{ (m).r = (a).r*(b).r - (a).i*(b).i;\ + (m).i = (a).r*(b).i + (a).i*(b).r; }while(0) +# define C_FIXDIV(c,div) /* NOOP */ +# define C_MULBYSCALAR( c, s ) \ + do{ (c).r *= (s);\ + (c).i *= (s); }while(0) +#endif + +#ifndef CHECK_OVERFLOW_OP +# define CHECK_OVERFLOW_OP(a,op,b) /* noop */ +#endif + +#define C_ADD( res, a,b)\ + do { \ + CHECK_OVERFLOW_OP((a).r,+,(b).r)\ + CHECK_OVERFLOW_OP((a).i,+,(b).i)\ + (res).r=(a).r+(b).r; (res).i=(a).i+(b).i; \ + }while(0) +#define C_SUB( res, a,b)\ + do { \ + CHECK_OVERFLOW_OP((a).r,-,(b).r)\ + CHECK_OVERFLOW_OP((a).i,-,(b).i)\ + (res).r=(a).r-(b).r; (res).i=(a).i-(b).i; \ + }while(0) +#define C_ADDTO( res , a)\ + do { \ + CHECK_OVERFLOW_OP((res).r,+,(a).r)\ + CHECK_OVERFLOW_OP((res).i,+,(a).i)\ + (res).r += (a).r; (res).i += (a).i;\ + }while(0) + +#define C_SUBFROM( res , a)\ + do {\ + CHECK_OVERFLOW_OP((res).r,-,(a).r)\ + CHECK_OVERFLOW_OP((res).i,-,(a).i)\ + (res).r -= (a).r; (res).i -= (a).i; \ + }while(0) + + +#ifdef FIXED_POINT +# define KISS_FFT_COS(phase) floor(.5+SAMP_MAX * cos (phase)) +# define KISS_FFT_SIN(phase) floor(.5+SAMP_MAX * sin (phase)) +# define HALF_OF(x) ((x)>>1) +#elif defined(USE_SIMD) +# define KISS_FFT_COS(phase) _mm_set1_ps( cos(phase) ) +# define KISS_FFT_SIN(phase) _mm_set1_ps( sin(phase) ) +# define HALF_OF(x) ((x)*_mm_set1_ps(.5)) +#else +# define KISS_FFT_COS(phase) (kiss_fft_scalar) cos(phase) +# define KISS_FFT_SIN(phase) (kiss_fft_scalar) sin(phase) +# define HALF_OF(x) ((x)*.5) +#endif + +#define kf_cexp(x,phase) \ + do{ \ + (x)->r = KISS_FFT_COS(phase);\ + (x)->i = KISS_FFT_SIN(phase);\ + }while(0) + + +/* a debugging function */ +#define pcpx(c)\ + fprintf(stderr,"%g + %gi\n",(double)((c)->r),(double)((c)->i) ) + + +#ifdef KISS_FFT_USE_ALLOCA +// define this to allow use of alloca instead of malloc for temporary buffers +// Temporary buffers are used in two case: +// 1. FFT sizes that have "bad" factors. i.e. not 2,3 and 5 +// 2. "in-place" FFTs. Notice the quotes, since kissfft does not really do an in-place transform. +#include +#define KISS_FFT_TMP_ALLOC(nbytes) alloca(nbytes) +#define KISS_FFT_TMP_FREE(ptr) +#else +#define KISS_FFT_TMP_ALLOC(nbytes) KISS_FFT_MALLOC(nbytes) +#define KISS_FFT_TMP_FREE(ptr) KISS_FFT_FREE(ptr) +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/fft/kiss_fft.h b/deps_install/include/pcl-1.10/pcl/common/fft/kiss_fft.h new file mode 100755 index 0000000..e7dc9e2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/fft/kiss_fft.h @@ -0,0 +1,127 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* + ATTENTION! + If you would like a : + -- a utility that will handle the caching of fft objects + -- real-only (no imaginary time component ) FFT + -- a multi-dimensional FFT + -- a command-line utility to perform ffts + -- a command-line utility to perform fast-convolution filtering + + Then see kfc.h kiss_fftr.h kiss_fftnd.h fftutil.c kiss_fastfir.c + in the tools/ directory. +*/ + +#ifdef USE_SIMD +# include +# define kiss_fft_scalar __m128 +#define KISS_FFT_MALLOC(nbytes) _mm_malloc(nbytes,16) +#define KISS_FFT_FREE _mm_free +#else +#define KISS_FFT_MALLOC malloc +#define KISS_FFT_FREE free +#endif + + +#ifdef FIXED_POINT +#include +# if (FIXED_POINT == 32) +# define kiss_fft_scalar int32_t +# else +# define kiss_fft_scalar int16_t +# endif +#else +# ifndef kiss_fft_scalar +/* default is float */ +# define kiss_fft_scalar float +# endif +#endif + +typedef struct { + kiss_fft_scalar r; + kiss_fft_scalar i; +}kiss_fft_cpx; + +typedef struct kiss_fft_state* kiss_fft_cfg; + +/* + * kiss_fft_alloc + * + * Initialize a FFT (or IFFT) algorithm's cfg/state buffer. + * + * typical usage: kiss_fft_cfg mycfg=kiss_fft_alloc(1024,0,NULL,NULL); + * + * The return value from fft_alloc is a cfg buffer used internally + * by the fft routine or NULL. + * + * If lenmem is NULL, then kiss_fft_alloc will allocate a cfg buffer using malloc. + * The returned value should be free()d when done to avoid memory leaks. + * + * The state can be placed in a user supplied buffer 'mem': + * If lenmem is not NULL and mem is not NULL and *lenmem is large enough, + * then the function places the cfg in mem and the size used in *lenmem + * and returns mem. + * + * If lenmem is not NULL and ( mem is NULL or *lenmem is not large enough), + * then the function returns NULL and places the minimum cfg + * buffer size in *lenmem. + * */ + +kiss_fft_cfg PCL_EXPORTS +kiss_fft_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem); + +/* + * kiss_fft(cfg,in_out_buf) + * + * Perform an FFT on a complex input buffer. + * for a forward FFT, + * fin should be f[0] , f[1] , ... ,f[nfft-1] + * fout will be F[0] , F[1] , ... ,F[nfft-1] + * Note that each element is complex and can be accessed like + f[k].r and f[k].i + * */ +void PCL_EXPORTS +kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout); + +/* + A more generic version of the above function. It reads its input from every Nth sample. + * */ +void PCL_EXPORTS +kiss_fft_stride(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout,int fin_stride); + +/* If kiss_fft_alloc allocated a buffer, it is one contiguous + buffer and can be simply free()d when no longer needed*/ +#define kiss_fft_free free + +/* + Cleans up some memory that gets managed internally. Not necessary to call, but it might clean up + your compiler output to call this before you exit. +*/ +void PCL_EXPORTS +kiss_fft_cleanup(void); + +/* + * Returns the smallest integer k, such that k>=n and k has only "fast" factors (2,3,5) + */ +int PCL_EXPORTS +kiss_fft_next_fast_size(int n); + +/* for real ffts, we need an even size */ +#define kiss_fftr_next_fast_size_real(n) \ + (kiss_fft_next_fast_size( ((n)+1)>>1)<<1) + +#ifdef __cplusplus +} +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/fft/kiss_fftr.h b/deps_install/include/pcl-1.10/pcl/common/fft/kiss_fftr.h new file mode 100755 index 0000000..e6d1e8d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/fft/kiss_fftr.h @@ -0,0 +1,44 @@ +#pragma once + +#include "kiss_fft.h" +#ifdef __cplusplus +extern "C" { +#endif + + +/* + + Real optimized version can save about 45% cpu time vs. complex fft of a real seq. + + + + */ + +typedef struct kiss_fftr_state *kiss_fftr_cfg; + + +kiss_fftr_cfg PCL_EXPORTS kiss_fftr_alloc(int nfft,int inverse_fft,void * mem, size_t * lenmem); +/* + nfft must be even + + If you don't care to allocate space, use mem = lenmem = NULL +*/ + + +void PCL_EXPORTS kiss_fftr(kiss_fftr_cfg cfg,const kiss_fft_scalar *timedata,kiss_fft_cpx *freqdata); +/* + input timedata has nfft scalar points + output freqdata has nfft/2+1 complex points +*/ + +void PCL_EXPORTS kiss_fftri(kiss_fftr_cfg cfg,const kiss_fft_cpx *freqdata,kiss_fft_scalar *timedata); +/* + input freqdata has nfft/2+1 complex points + output timedata has nfft scalar points +*/ + +#define kiss_fftr_free free + +#ifdef __cplusplus +} +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/file_io.h b/deps_install/include/pcl-1.10/pcl/common/file_io.h new file mode 100755 index 0000000..fe413bb --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/file_io.h @@ -0,0 +1,85 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +/** + * \file pcl/common/file_io.h + * Define some helper functions for reading and writing files + * \ingroup common + * \todo move this to pcl::console + */ + +/*@{*/ +namespace pcl +{ + /** \brief Find all *.pcd files in the directory and return them sorted + * \param directory the directory to be searched + * \param file_names the resulting (sorted) list of .pcd files + */ + inline void + getAllPcdFilesInDirectory (const std::string& directory, std::vector& file_names); + + /** \brief Remove the path from the given string and return only the filename (the remaining string after the + * last '/') + * \param input the input filename (with full path) + * \return the resulting filename, stripped of the path + */ + inline std::string + getFilenameWithoutPath (const std::string& input); + + /** \brief Remove the extension from the given string and return only the filename (everything before the last '.') + * \param input the input filename (with the file extension) + * \return the resulting filename, stripped of its extension + */ + inline std::string + getFilenameWithoutExtension (const std::string& input); + + /** \brief Get the file extension from the given string (the remaining string after the last '.') + * \param input the input filename + * \return \a input 's file extension + */ + inline std::string + getFileExtension (const std::string& input); +} // namespace end +/*@}*/ +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/gaussian.h b/deps_install/include/pcl-1.10/pcl/common/gaussian.h new file mode 100755 index 0000000..2760203 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/gaussian.h @@ -0,0 +1,262 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +#include +#include + +namespace pcl +{ + /** Class GaussianKernel assembles all the method for computing, + * convolving, smoothing, gradients computing an image using + * a gaussian kernel. The image is stored in point cloud elements + * intensity member or rgb or... + * \author Nizar Sallem + * \ingroup common + */ + class PCL_EXPORTS GaussianKernel + { + public: + + static const unsigned MAX_KERNEL_WIDTH = 71; + /** Computes the gaussian kernel and dervative assiociated to sigma. + * The kernel and derivative width are adjusted according. + * \param[in] sigma + * \param[out] kernel the computed gaussian kernel + * \param[in] kernel_width the desired kernel width upper bond + * \throws pcl::KernelWidthTooSmallException + */ + void + compute (float sigma, + Eigen::VectorXf &kernel, + unsigned kernel_width = MAX_KERNEL_WIDTH) const; + + /** Computes the gaussian kernel and dervative assiociated to sigma. + * The kernel and derivative width are adjusted according. + * \param[in] sigma + * \param[out] kernel the computed gaussian kernel + * \param[out] derivative the computed kernel derivative + * \param[in] kernel_width the desired kernel width upper bond + * \throws pcl::KernelWidthTooSmallException + */ + void + compute (float sigma, + Eigen::VectorXf &kernel, + Eigen::VectorXf &derivative, + unsigned kernel_width = MAX_KERNEL_WIDTH) const; + + /** Convolve a float image rows by a given kernel. + * \param[in] kernel convolution kernel + * \param[in] input the image to convolve + * \param[out] output the convolved image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + void + convolveRows (const pcl::PointCloud &input, + const Eigen::VectorXf &kernel, + pcl::PointCloud &output) const; + + /** Convolve a float image rows by a given kernel. + * \param[in] input the image to convolve + * \param[in] field_accessor a field accessor + * \param[in] kernel convolution kernel + * \param[out] output the convolved image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + template void + convolveRows (const pcl::PointCloud &input, + std::function field_accessor, + const Eigen::VectorXf &kernel, + pcl::PointCloud &output) const; + + /** Convolve a float image columns by a given kernel. + * \param[in] input the image to convolve + * \param[in] kernel convolution kernel + * \param[out] output the convolved image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + void + convolveCols (const pcl::PointCloud &input, + const Eigen::VectorXf &kernel, + pcl::PointCloud &output) const; + + /** Convolve a float image columns by a given kernel. + * \param[in] input the image to convolve + * \param[in] field_accessor a field accessor + * \param[in] kernel convolution kernel + * \param[out] output the convolved image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + template void + convolveCols (const pcl::PointCloud &input, + std::function field_accessor, + const Eigen::VectorXf &kernel, + pcl::PointCloud &output) const; + + /** Convolve a float image in the 2 directions + * \param[in] horiz_kernel kernel for convolving rows + * \param[in] vert_kernel kernel for convolving columns + * \param[in] input image to convolve + * \param[out] output the convolved image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + inline void + convolve (const pcl::PointCloud &input, + const Eigen::VectorXf &horiz_kernel, + const Eigen::VectorXf &vert_kernel, + pcl::PointCloud &output) const + { + std::cout << ">>> convolve cpp" << std::endl; + pcl::PointCloud tmp (input.width, input.height) ; + convolveRows (input, horiz_kernel, tmp); + convolveCols (tmp, vert_kernel, output); + std::cout << "<<< convolve cpp" << std::endl; + } + + /** Convolve a float image in the 2 directions + * \param[in] input image to convolve + * \param[in] field_accessor a field accessor + * \param[in] horiz_kernel kernel for convolving rows + * \param[in] vert_kernel kernel for convolving columns + * \param[out] output the convolved image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + template inline void + convolve (const pcl::PointCloud &input, + std::function field_accessor, + const Eigen::VectorXf &horiz_kernel, + const Eigen::VectorXf &vert_kernel, + pcl::PointCloud &output) const + { + std::cout << ">>> convolve hpp" << std::endl; + pcl::PointCloud tmp (input.width, input.height); + convolveRows(input, field_accessor, horiz_kernel, tmp); + convolveCols(tmp, vert_kernel, output); + std::cout << "<<< convolve hpp" << std::endl; + } + + /** Computes float image gradients using a gaussian kernel and gaussian kernel + * derivative. + * \param[in] input image to compute gardients for + * \param[in] gaussian_kernel the gaussian kernel to be used + * \param[in] gaussian_kernel_derivative the associated derivative + * \param[out] grad_x gradient along X direction + * \param[out] grad_y gradient along Y direction + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + inline void + computeGradients (const pcl::PointCloud &input, + const Eigen::VectorXf &gaussian_kernel, + const Eigen::VectorXf &gaussian_kernel_derivative, + pcl::PointCloud &grad_x, + pcl::PointCloud &grad_y) const + { + convolve (input, gaussian_kernel_derivative, gaussian_kernel, grad_x); + convolve (input, gaussian_kernel, gaussian_kernel_derivative, grad_y); + } + + /** Computes float image gradients using a gaussian kernel and gaussian kernel + * derivative. + * \param[in] input image to compute gardients for + * \param[in] field_accessor a field accessor + * \param[in] gaussian_kernel the gaussian kernel to be used + * \param[in] gaussian_kernel_derivative the associated derivative + * \param[out] grad_x gradient along X direction + * \param[out] grad_y gradient along Y direction + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + template inline void + computeGradients (const pcl::PointCloud &input, + std::function field_accessor, + const Eigen::VectorXf &gaussian_kernel, + const Eigen::VectorXf &gaussian_kernel_derivative, + pcl::PointCloud &grad_x, + pcl::PointCloud &grad_y) const + { + convolve (input, field_accessor, gaussian_kernel_derivative, gaussian_kernel, grad_x); + convolve (input, field_accessor, gaussian_kernel, gaussian_kernel_derivative, grad_y); + } + + /** Smooth image using a gaussian kernel. + * \param[in] input image + * \param[in] gaussian_kernel the gaussian kernel to be used + * \param[out] output the smoothed image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + inline void + smooth (const pcl::PointCloud &input, + const Eigen::VectorXf &gaussian_kernel, + pcl::PointCloud &output) const + { + convolve (input, gaussian_kernel, gaussian_kernel, output); + } + + /** Smooth image using a gaussian kernel. + * \param[in] input image + * \param[in] field_accessor a field accessor + * \param[in] gaussian_kernel the gaussian kernel to be used + * \param[out] output the smoothed image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + template inline void + smooth (const pcl::PointCloud &input, + std::function field_accessor, + const Eigen::VectorXf &gaussian_kernel, + pcl::PointCloud &output) const + { + convolve (input, field_accessor, gaussian_kernel, gaussian_kernel, output); + } + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/generate.h b/deps_install/include/pcl-1.10/pcl/common/generate.h new file mode 100755 index 0000000..4e4fb1c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/generate.h @@ -0,0 +1,186 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + + namespace common + { + /** \brief CloudGenerator class generates a point cloud using some randoom number generator. + * Generators can be found in \file common/random.h and easily extensible. + * + * \ingroup common + * \author Nizar Sallem + */ + template + class CloudGenerator + { + public: + using GeneratorParameters = typename GeneratorT::Parameters; + + /// Default constructor + CloudGenerator (); + + /** Constructor with single generator to ensure all X, Y and Z values are within same range + * \param params parameters for X, Y and Z values generation. Uniqueness is ensured through + * seed incrementation + */ + CloudGenerator (const GeneratorParameters& params); + + /** Constructor with independent generators per axis + * \param x_params parameters for x values generation + * \param y_params parameters for y values generation + * \param z_params parameters for z values generation + */ + CloudGenerator (const GeneratorParameters& x_params, + const GeneratorParameters& y_params, + const GeneratorParameters& z_params); + + /** Set parameters for x, y and z values. Uniqueness is ensured through seed incrementation. + * \param params parameteres for X, Y and Z values generation. + */ + void + setParameters (const GeneratorParameters& params); + + /** Set parameters for x values generation + * \param x_params parameters for x values generation + */ + void + setParametersForX (const GeneratorParameters& x_params); + + /** Set parameters for y values generation + * \param y_params parameters for y values generation + */ + void + setParametersForY (const GeneratorParameters& y_params); + + /** Set parameters for z values generation + * \param z_params parameters for z values generation + */ + void + setParametersForZ (const GeneratorParameters& z_params); + + /// \return x values generation parameters + const GeneratorParameters& + getParametersForX () const; + + /// \return y values generation parameters + const GeneratorParameters& + getParametersForY () const; + + /// \return z values generation parameters + const GeneratorParameters& + getParametersForZ () const; + + /// \return a single random generated point + PointT + get (); + + /** Generates a cloud with X Y Z picked within given ranges. This function assumes that + * cloud is properly defined else it raises errors and does nothing. + * \param[out] cloud cloud to generate coordinates for + * \return 0 if generation went well else -1. + */ + int + fill (pcl::PointCloud& cloud); + + /** Generates a cloud of specified dimensions with X Y Z picked within given ranges. + * \param[in] width width of generated cloud + * \param[in] height height of generated cloud + * \param[out] cloud output cloud + * \return 0 if generation went well else -1. + */ + int + fill (int width, int height, pcl::PointCloud& cloud); + + private: + GeneratorT x_generator_, y_generator_, z_generator_; + }; + + template + class CloudGenerator + { + public: + using GeneratorParameters = typename GeneratorT::Parameters; + + CloudGenerator (); + + CloudGenerator (const GeneratorParameters& params); + + CloudGenerator (const GeneratorParameters& x_params, + const GeneratorParameters& y_params); + + void + setParameters (const GeneratorParameters& params); + + void + setParametersForX (const GeneratorParameters& x_params); + + void + setParametersForY (const GeneratorParameters& y_params); + + const GeneratorParameters& + getParametersForX () const; + + const GeneratorParameters& + getParametersForY () const; + + pcl::PointXY + get (); + + int + fill (pcl::PointCloud& cloud); + + int + fill (int width, int height, pcl::PointCloud& cloud); + + private: + GeneratorT x_generator_; + GeneratorT y_generator_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/geometry.h b/deps_install/include/pcl-1.10/pcl/common/geometry.h new file mode 100755 index 0000000..c7b613e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/geometry.h @@ -0,0 +1,161 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +/** + * \file common/geometry.h + * Defines some geometrical functions and utility functions + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + namespace geometry + { + /** @return the euclidean distance between 2 points */ + template inline float + distance (const PointT& p1, const PointT& p2) + { + Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap (); + return (diff.norm ()); + } + + /** @return the squared euclidean distance between 2 points */ + template inline float + squaredDistance (const PointT& p1, const PointT& p2) + { + Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap (); + return (diff.squaredNorm ()); + } + + /** @return the point projection on a plane defined by its origin and normal vector + * \param[in] point Point to be projected + * \param[in] plane_origin The plane origin + * \param[in] plane_normal The plane normal + * \param[out] projected The returned projected point + */ + template inline void + project (const PointT& point, const PointT &plane_origin, + const NormalT& plane_normal, PointT& projected) + { + Eigen::Vector3f po = point - plane_origin; + const Eigen::Vector3f normal = plane_normal.getVector3fMapConst (); + float lambda = normal.dot(po); + projected.getVector3fMap () = point.getVector3fMapConst () - (lambda * normal); + } + + /** @return the point projection on a plane defined by its origin and normal vector + * \param[in] point Point to be projected + * \param[in] plane_origin The plane origin + * \param[in] plane_normal The plane normal + * \param[out] projected The returned projected point + */ + inline void + project (const Eigen::Vector3f& point, const Eigen::Vector3f &plane_origin, + const Eigen::Vector3f& plane_normal, Eigen::Vector3f& projected) + { + Eigen::Vector3f po = point - plane_origin; + float lambda = plane_normal.dot(po); + projected = point - (lambda * plane_normal); + } + + + /** \brief Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_origin to the projection of point on the plane. + * + * \param[in] point Point projected on the plane + * \param[in] plane_origin The plane origin + * \param[in] plane_normal The plane normal + * \return unit vector pointing from plane_origin to the projection of point on the plane. + * \ingroup geometry + */ + inline Eigen::Vector3f + projectedAsUnitVector (Eigen::Vector3f const &point, + Eigen::Vector3f const &plane_origin, + Eigen::Vector3f const &plane_normal) + { + Eigen::Vector3f projection; + project (point, plane_origin, plane_normal, projection); + Eigen::Vector3f projected_as_unit_vector = projection - plane_origin; + projected_as_unit_vector.normalize (); + return projected_as_unit_vector; + } + + + /** \brief Define a random unit vector orthogonal to axis. + * + * \param[in] axis Axis + * \return random unit vector orthogonal to axis + * \ingroup geometry + */ + inline Eigen::Vector3f + randomOrthogonalAxis (Eigen::Vector3f const &axis) + { + Eigen::Vector3f rand_ortho_axis; + rand_ortho_axis.setRandom(); + if (std::abs (axis.z ()) > 1E-8f) + { + rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z (); + } + else if (std::abs (axis.y ()) > 1E-8f) + { + rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y (); + } + else if (std::abs (axis.x ()) > 1E-8f) + { + rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); + } + else + { + PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f"); + } + + rand_ortho_axis.normalize (); + return rand_ortho_axis; + } + + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/accumulators.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/accumulators.hpp new file mode 100755 index 0000000..13804a9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/accumulators.hpp @@ -0,0 +1,289 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP +#define PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace pcl +{ + + namespace detail + { + + /* Below are several helper accumulator structures that are used by the + * `CentroidPoint` class. Each of them is capable of accumulating + * information from a particular field(s) of a point. The points are + * inserted via `add()` and extracted via `get()` functions. Note that the + * accumulators are not templated on point type, so in principle it is + * possible to insert and extract points of different types. It is the + * responsibility of the user to make sure that points have corresponding + * fields. */ + + struct AccumulatorXYZ + { + + // Requires that point type has x, y, and z fields + using IsCompatible = pcl::traits::has_xyz; + + // Storage + Eigen::Vector3f xyz = Eigen::Vector3f::Zero (); + + template void + add (const PointT& t) { xyz += t.getVector3fMap (); } + + template void + get (PointT& t, std::size_t n) const { t.getVector3fMap () = xyz / n; } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + }; + + struct AccumulatorNormal + { + + // Requires that point type has normal_x, normal_y, and normal_z fields + using IsCompatible = pcl::traits::has_normal; + + // Storage + Eigen::Vector4f normal = Eigen::Vector4f::Zero (); + + // Requires that the normal of the given point is normalized, otherwise it + // does not make sense to sum it up with the accumulated value. + template void + add (const PointT& t) { normal += t.getNormalVector4fMap (); } + + template void + get (PointT& t, std::size_t) const + { +#if EIGEN_VERSION_AT_LEAST (3, 3, 0) + t.getNormalVector4fMap () = normal.normalized (); +#else + if (normal.squaredNorm() > 0) + t.getNormalVector4fMap () = normal.normalized (); + else + t.getNormalVector4fMap () = Eigen::Vector4f::Zero (); +#endif + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + }; + + struct AccumulatorCurvature + { + + // Requires that point type has curvature field + using IsCompatible = pcl::traits::has_curvature; + + // Storage + float curvature = 0; + + template void + add (const PointT& t) { curvature += t.curvature; } + + template void + get (PointT& t, std::size_t n) const { t.curvature = curvature / n; } + + }; + + struct AccumulatorRGBA + { + + // Requires that point type has rgb or rgba field + using IsCompatible = pcl::traits::has_color; + + // Storage + float r = 0, g = 0, b = 0, a = 0; + + template void + add (const PointT& t) + { + r += static_cast (t.r); + g += static_cast (t.g); + b += static_cast (t.b); + a += static_cast (t.a); + } + + template void + get (PointT& t, std::size_t n) const + { + t.rgba = static_cast (a / n) << 24 | + static_cast (r / n) << 16 | + static_cast (g / n) << 8 | + static_cast (b / n); + } + + }; + + struct AccumulatorIntensity + { + + // Requires that point type has intensity field + using IsCompatible = pcl::traits::has_intensity; + + // Storage + float intensity = 0; + + template void + add (const PointT& t) { intensity += t.intensity; } + + template void + get (PointT& t, std::size_t n) const { t.intensity = intensity / n; } + + }; + + struct AccumulatorLabel + { + + // Requires that point type has label field + using IsCompatible = pcl::traits::has_label; + + // Storage + // A better performance may be achieved with a heap structure + std::map labels; + + template void + add (const PointT& t) + { + auto itr = labels.find (t.label); + if (itr == labels.end ()) + labels.insert (std::make_pair (t.label, 1)); + else + ++itr->second; + } + + template void + get (PointT& t, std::size_t) const + { + std::size_t max = 0; + for (const auto &label : labels) + if (label.second > max) + { + max = label.second; + t.label = label.first; + } + } + + }; + + /* Meta-function that checks if an accumulator is compatible with given + * point type(s). */ + template + struct IsAccumulatorCompatible { + + template + struct apply : boost::mpl::and_< + boost::mpl::apply, + boost::mpl::apply + > {}; + }; + + /* Meta-function that creates a Fusion vector of accumulator types that are + * compatible with a given point type. */ + template + struct Accumulators + { + using type = + typename boost::fusion::result_of::as_vector< + typename boost::mpl::filter_view< + boost::mpl::vector< + AccumulatorXYZ + , AccumulatorNormal + , AccumulatorCurvature + , AccumulatorRGBA + , AccumulatorIntensity + , AccumulatorLabel + > + , IsAccumulatorCompatible + > + >::type; + }; + + /* Fusion function object to invoke point addition on every accumulator in + * a fusion sequence. */ + template + struct AddPoint + { + + const PointT& p; + + AddPoint (const PointT& point) : p (point) { } + + template void + operator () (AccumulatorT& accumulator) const + { + accumulator.add (p); + } + + }; + + /* Fusion function object to invoke get point on every accumulator in a + * fusion sequence. */ + template + struct GetPoint + { + + PointT& p; + std::size_t n; + + GetPoint (PointT& point, std::size_t num) : p (point), n (num) { } + + template void + operator () (AccumulatorT& accumulator) const + { + accumulator.get (p, n); + } + + }; + + } + +} + +#endif /* PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP */ + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/angles.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/angles.hpp new file mode 100755 index 0000000..0cd8819 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/angles.hpp @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR a PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_ANGLES_IMPL_HPP_ +#define PCL_COMMON_ANGLES_IMPL_HPP_ + +#include +#include + +namespace pcl +{ + inline float + normAngle (float alpha) + { + return (alpha >= 0 ? + std::fmod (alpha + static_cast(M_PI), + 2.0f * static_cast(M_PI)) + - static_cast(M_PI) + : + -(std::fmod (static_cast(M_PI) - alpha, + 2.0f * static_cast(M_PI)) + - static_cast(M_PI))); + } + + inline float + rad2deg (float alpha) + { + return (alpha * 57.29578f); + } + + inline float + deg2rad (float alpha) + { + return (alpha * 0.017453293f); + } + + inline double + rad2deg (double alpha) + { + return (alpha * 57.29578); + } + + inline double + deg2rad (double alpha) + { + return (alpha * 0.017453293); + } +} + +#endif // PCL_COMMON_ANGLES_IMPL_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/bivariate_polynomial.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/bivariate_polynomial.hpp new file mode 100755 index 0000000..381a7c2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/bivariate_polynomial.hpp @@ -0,0 +1,305 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef BIVARIATE_POLYNOMIAL_HPP +#define BIVARIATE_POLYNOMIAL_HPP + +#include +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::BivariatePolynomialT::BivariatePolynomialT (int new_degree) : + degree(0), parameters(nullptr), gradient_x(nullptr), gradient_y(nullptr) +{ + setDegree(new_degree); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::BivariatePolynomialT::BivariatePolynomialT (const BivariatePolynomialT& other) : + degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL) +{ + deepCopy (other); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::BivariatePolynomialT::~BivariatePolynomialT () +{ + memoryCleanUp (); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::setDegree (int newDegree) +{ + if (newDegree <= 0) + { + degree = -1; + memoryCleanUp(); + return; + } + int oldDegree = degree; + degree = newDegree; + if (oldDegree != degree) + { + delete[] parameters; + parameters = new real[getNoOfParameters ()]; + } + delete gradient_x; gradient_x = nullptr; + delete gradient_y; gradient_y = nullptr; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::memoryCleanUp () +{ + delete[] parameters; parameters = nullptr; + delete gradient_x; gradient_x = nullptr; + delete gradient_y; gradient_y = nullptr; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::deepCopy (const pcl::BivariatePolynomialT& other) +{ + if (this == &other) return; + if (degree != other.degree) + { + memoryCleanUp (); + degree = other.degree; + parameters = new real[getNoOfParameters ()]; + } + if (other.gradient_x == NULL) + { + delete gradient_x; gradient_x=NULL; + delete gradient_y; gradient_y=NULL; + } + else if (gradient_x==NULL) + { + gradient_x = new pcl::BivariatePolynomialT (); + gradient_y = new pcl::BivariatePolynomialT (); + } + + std::copy_n(other.parameters, getNoOfParameters (), parameters); + + if (other.gradient_x != NULL) + { + gradient_x->deepCopy (*other.gradient_x); + gradient_y->deepCopy (*other.gradient_y); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::calculateGradient (bool forceRecalc) +{ + if (gradient_x!=NULL && !forceRecalc) return; + + if (gradient_x == NULL) + gradient_x = new pcl::BivariatePolynomialT (degree-1); + if (gradient_y == NULL) + gradient_y = new pcl::BivariatePolynomialT (degree-1); + + unsigned int parameterPosDx=0, parameterPosDy=0; + for (int xDegree=degree; xDegree>=0; xDegree--) + { + for (int yDegree=degree-xDegree; yDegree>=0; yDegree--) + { + if (xDegree > 0) + { + gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx]; + parameterPosDx++; + } + if (yDegree > 0) + { + gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 - + yDegree - 1]; + parameterPosDy++; + } + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template real +pcl::BivariatePolynomialT::getValue (real x, real y) const +{ + unsigned int parametersSize = getNoOfParameters (); + real* tmpParameter = ¶meters[parametersSize-1]; + real tmpX=1.0, tmpY, ret=0; + for (int xDegree=0; xDegree<=degree; xDegree++) + { + tmpY = 1.0; + for (int yDegree=0; yDegree<=degree-xDegree; yDegree++) + { + ret += (*tmpParameter)*tmpX*tmpY; + tmpY *= y; + tmpParameter--; + } + tmpX *= x; + } + return ret; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::getValueOfGradient (real x, real y, real& gradX, real& gradY) +{ + calculateGradient (); + gradX = gradient_x->getValue (x, y); + gradY = gradient_y->getValue (x, y); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::findCriticalPoints (std::vector& x_values, std::vector& y_values, + std::vector& types) const +{ + x_values.clear (); + y_values.clear (); + types.clear (); + + if (degree == 2) + { + real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) / + (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]), + y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1]; + + if (!std::isfinite(x) || !std::isfinite(y)) + return; + + int type = 2; + real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1]; + //std::cout << "det(H) = "< real(0)) // Check Hessian determinant + { + if (parameters[0]+parameters[3] < real(0)) // Check Hessian trace + type = 0; + else + type = 1; + } + x_values.push_back(x); + y_values.push_back(y); + types.push_back(type); + } + else + { + std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "< std::ostream& +pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT& p) +{ + real* tmpParameter = p.parameters; + bool first = true; + real currentParameter; + for (int xDegree=p.degree; xDegree>=0; xDegree--) + { + for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--) + { + currentParameter = *tmpParameter; + if (!first) + { + os << (currentParameter<0.0?" - ":" + "); + currentParameter = std::abs (currentParameter); + } + os << currentParameter; + if (xDegree>0) + { + os << "x"; + if (xDegree>1) + os<<"^"<0) + { + os << "y"; + if (yDegree>1) + os<<"^"< void +pcl::BivariatePolynomialT::writeBinary (std::ostream& os) const +{ + os.write (reinterpret_cast (°ree), sizeof (int)); + unsigned int paramCnt = getNoOfParametersFromDegree (this->degree); + os.write (reinterpret_cast (this->parameters), paramCnt * sizeof (real)); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::writeBinary (const char* filename) const +{ + std::ofstream fout (filename); + writeBinary (fout); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::readBinary (std::istream& os) +{ + memoryCleanUp (); + os.read (reinterpret_cast (&this->degree), sizeof (int)); + unsigned int paramCnt = getNoOfParametersFromDegree (this->degree); + parameters = new real[paramCnt]; + os.read (reinterpret_cast (&(*this->parameters)), paramCnt * sizeof (real)); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BivariatePolynomialT::readBinary (const char* filename) +{ + std::ifstream fin (filename); + readBinary (fin); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/centroid.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/centroid.hpp new file mode 100755 index 0000000..65a8d4c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/centroid.hpp @@ -0,0 +1,915 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-present, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_COMMON_IMPL_CENTROID_H_ +#define PCL_COMMON_IMPL_CENTROID_H_ + +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::compute3DCentroid (ConstCloudIterator &cloud_iterator, + Eigen::Matrix ¢roid) +{ + // Initialize to 0 + centroid.setZero (); + + unsigned cp = 0; + + // For each point in the cloud + // If the data is dense, we don't need to check for NaN + while (cloud_iterator.isValid ()) + { + // Check if the point is invalid + if (pcl::isFinite (*cloud_iterator)) + { + centroid[0] += cloud_iterator->x; + centroid[1] += cloud_iterator->y; + centroid[2] += cloud_iterator->z; + ++cp; + } + ++cloud_iterator; + } + centroid /= static_cast (cp); + centroid[3] = 1; + return (cp); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::compute3DCentroid (const pcl::PointCloud &cloud, + Eigen::Matrix ¢roid) +{ + if (cloud.empty ()) + return (0); + + // Initialize to 0 + centroid.setZero (); + // For each point in the cloud + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (const auto& point: cloud) + { + centroid[0] += point.x; + centroid[1] += point.y; + centroid[2] += point.z; + } + centroid /= static_cast (cloud.size ()); + centroid[3] = 1; + + return (static_cast (cloud.size ())); + } + // NaN or Inf values could exist => check for them + unsigned cp = 0; + for (const auto& point: cloud) + { + // Check if the point is invalid + if (!isFinite (point)) + continue; + + centroid[0] += point.x; + centroid[1] += point.y; + centroid[2] += point.z; + ++cp; + } + centroid /= static_cast (cp); + centroid[3] = 1; + + return (cp); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::compute3DCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix ¢roid) +{ + if (indices.empty ()) + return (0); + + // Initialize to 0 + centroid.setZero (); + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (const int& index : indices) + { + centroid[0] += cloud[index].x; + centroid[1] += cloud[index].y; + centroid[2] += cloud[index].z; + } + centroid /= static_cast (indices.size ()); + centroid[3] = 1; + return (static_cast (indices.size ())); + } + // NaN or Inf values could exist => check for them + unsigned cp = 0; + for (const int& index : indices) + { + // Check if the point is invalid + if (!isFinite (cloud [index])) + continue; + + centroid[0] += cloud[index].x; + centroid[1] += cloud[index].y; + centroid[2] += cloud[index].z; + ++cp; + } + centroid /= static_cast (cp); + centroid[3] = 1; + return (cp); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::compute3DCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix ¢roid) +{ + return (pcl::compute3DCentroid (cloud, indices.indices, centroid)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned +pcl::computeCovarianceMatrix (const pcl::PointCloud &cloud, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix) +{ + if (cloud.empty ()) + return (0); + + // Initialize to 0 + covariance_matrix.setZero (); + + unsigned point_count; + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + point_count = static_cast (cloud.size ()); + // For each point in the cloud + for (const auto& point: cloud) + { + Eigen::Matrix pt; + pt[0] = point.x - centroid[0]; + pt[1] = point.y - centroid[1]; + pt[2] = point.z - centroid[2]; + + covariance_matrix (1, 1) += pt.y () * pt.y (); + covariance_matrix (1, 2) += pt.y () * pt.z (); + + covariance_matrix (2, 2) += pt.z () * pt.z (); + + pt *= pt.x (); + covariance_matrix (0, 0) += pt.x (); + covariance_matrix (0, 1) += pt.y (); + covariance_matrix (0, 2) += pt.z (); + } + } + // NaN or Inf values could exist => check for them + else + { + point_count = 0; + // For each point in the cloud + for (const auto& point: cloud) + { + // Check if the point is invalid + if (!isFinite (point)) + continue; + + Eigen::Matrix pt; + pt[0] = point.x - centroid[0]; + pt[1] = point.y - centroid[1]; + pt[2] = point.z - centroid[2]; + + covariance_matrix (1, 1) += pt.y () * pt.y (); + covariance_matrix (1, 2) += pt.y () * pt.z (); + + covariance_matrix (2, 2) += pt.z () * pt.z (); + + pt *= pt.x (); + covariance_matrix (0, 0) += pt.x (); + covariance_matrix (0, 1) += pt.y (); + covariance_matrix (0, 2) += pt.z (); + ++point_count; + } + } + covariance_matrix (1, 0) = covariance_matrix (0, 1); + covariance_matrix (2, 0) = covariance_matrix (0, 2); + covariance_matrix (2, 1) = covariance_matrix (1, 2); + + return (point_count); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix) +{ + unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix); + if (point_count != 0) + covariance_matrix /= static_cast (point_count); + return (point_count); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix) +{ + if (indices.empty ()) + return (0); + + // Initialize to 0 + covariance_matrix.setZero (); + + std::size_t point_count; + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + point_count = indices.size (); + // For each point in the cloud + for (const auto& idx: indices) + { + Eigen::Matrix pt; + pt[0] = cloud[idx].x - centroid[0]; + pt[1] = cloud[idx].y - centroid[1]; + pt[2] = cloud[idx].z - centroid[2]; + + covariance_matrix (1, 1) += pt.y () * pt.y (); + covariance_matrix (1, 2) += pt.y () * pt.z (); + + covariance_matrix (2, 2) += pt.z () * pt.z (); + + pt *= pt.x (); + covariance_matrix (0, 0) += pt.x (); + covariance_matrix (0, 1) += pt.y (); + covariance_matrix (0, 2) += pt.z (); + } + } + // NaN or Inf values could exist => check for them + else + { + point_count = 0; + // For each point in the cloud + for (const int &index : indices) + { + // Check if the point is invalid + if (!isFinite (cloud[index])) + continue; + + Eigen::Matrix pt; + pt[0] = cloud[index].x - centroid[0]; + pt[1] = cloud[index].y - centroid[1]; + pt[2] = cloud[index].z - centroid[2]; + + covariance_matrix (1, 1) += pt.y () * pt.y (); + covariance_matrix (1, 2) += pt.y () * pt.z (); + + covariance_matrix (2, 2) += pt.z () * pt.z (); + + pt *= pt.x (); + covariance_matrix (0, 0) += pt.x (); + covariance_matrix (0, 1) += pt.y (); + covariance_matrix (0, 2) += pt.z (); + ++point_count; + } + } + covariance_matrix (1, 0) = covariance_matrix (0, 1); + covariance_matrix (2, 0) = covariance_matrix (0, 2); + covariance_matrix (2, 1) = covariance_matrix (1, 2); + return (static_cast (point_count)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix) +{ + return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix) +{ + unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix); + if (point_count != 0) + covariance_matrix /= static_cast (point_count); + + return (point_count); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &covariance_matrix) +{ + unsigned int point_count = pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix); + if (point_count != 0) + covariance_matrix /= static_cast (point_count); + + return point_count; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix &covariance_matrix) +{ + // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer + Eigen::Matrix accu = Eigen::Matrix::Zero (); + + unsigned int point_count; + if (cloud.is_dense) + { + point_count = static_cast (cloud.size ()); + // For each point in the cloud + for (const auto& point: cloud) + { + accu [0] += point.x * point.x; + accu [1] += point.x * point.y; + accu [2] += point.x * point.z; + accu [3] += point.y * point.y; + accu [4] += point.y * point.z; + accu [5] += point.z * point.z; + } + } + else + { + point_count = 0; + for (const auto& point: cloud) + { + if (!isFinite (point)) + continue; + + accu [0] += point.x * point.x; + accu [1] += point.x * point.y; + accu [2] += point.x * point.z; + accu [3] += point.y * point.y; + accu [4] += point.y * point.z; + accu [5] += point.z * point.z; + ++point_count; + } + } + + if (point_count != 0) + { + accu /= static_cast (point_count); + covariance_matrix.coeffRef (0) = accu [0]; + covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1]; + covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2]; + covariance_matrix.coeffRef (4) = accu [3]; + covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4]; + covariance_matrix.coeffRef (8) = accu [5]; + } + return (point_count); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix &covariance_matrix) +{ + // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer + Eigen::Matrix accu = Eigen::Matrix::Zero (); + + unsigned int point_count; + if (cloud.is_dense) + { + point_count = static_cast (indices.size ()); + for (const int &index : indices) + { + //const PointT& point = cloud[*iIt]; + accu [0] += cloud[index].x * cloud[index].x; + accu [1] += cloud[index].x * cloud[index].y; + accu [2] += cloud[index].x * cloud[index].z; + accu [3] += cloud[index].y * cloud[index].y; + accu [4] += cloud[index].y * cloud[index].z; + accu [5] += cloud[index].z * cloud[index].z; + } + } + else + { + point_count = 0; + for (const int &index : indices) + { + if (!isFinite (cloud[index])) + continue; + + ++point_count; + accu [0] += cloud[index].x * cloud[index].x; + accu [1] += cloud[index].x * cloud[index].y; + accu [2] += cloud[index].x * cloud[index].z; + accu [3] += cloud[index].y * cloud[index].y; + accu [4] += cloud[index].y * cloud[index].z; + accu [5] += cloud[index].z * cloud[index].z; + } + } + if (point_count != 0) + { + accu /= static_cast (point_count); + covariance_matrix.coeffRef (0) = accu [0]; + covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1]; + covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2]; + covariance_matrix.coeffRef (4) = accu [3]; + covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4]; + covariance_matrix.coeffRef (8) = accu [5]; + } + return (point_count); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix &covariance_matrix) +{ + return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix &covariance_matrix, + Eigen::Matrix ¢roid) +{ + // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer + Eigen::Matrix accu = Eigen::Matrix::Zero (); + std::size_t point_count; + if (cloud.is_dense) + { + point_count = cloud.size (); + // For each point in the cloud + for (const auto& point: cloud) + { + accu [0] += point.x * point.x; + accu [1] += point.x * point.y; + accu [2] += point.x * point.z; + accu [3] += point.y * point.y; // 4 + accu [4] += point.y * point.z; // 5 + accu [5] += point.z * point.z; // 8 + accu [6] += point.x; + accu [7] += point.y; + accu [8] += point.z; + } + } + else + { + point_count = 0; + for (const auto& point: cloud) + { + if (!isFinite (point)) + continue; + + accu [0] += point.x * point.x; + accu [1] += point.x * point.y; + accu [2] += point.x * point.z; + accu [3] += point.y * point.y; + accu [4] += point.y * point.z; + accu [5] += point.z * point.z; + accu [6] += point.x; + accu [7] += point.y; + accu [8] += point.z; + ++point_count; + } + } + accu /= static_cast (point_count); + if (point_count != 0) + { + //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0 + centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; + centroid[3] = 1; + covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; + covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; + covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; + covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; + covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; + covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; + covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); + covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); + covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); + } + return (static_cast (point_count)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix &covariance_matrix, + Eigen::Matrix ¢roid) +{ + // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer + Eigen::Matrix accu = Eigen::Matrix::Zero (); + std::size_t point_count; + if (cloud.is_dense) + { + point_count = indices.size (); + for (const int &index : indices) + { + //const PointT& point = cloud[*iIt]; + accu [0] += cloud[index].x * cloud[index].x; + accu [1] += cloud[index].x * cloud[index].y; + accu [2] += cloud[index].x * cloud[index].z; + accu [3] += cloud[index].y * cloud[index].y; + accu [4] += cloud[index].y * cloud[index].z; + accu [5] += cloud[index].z * cloud[index].z; + accu [6] += cloud[index].x; + accu [7] += cloud[index].y; + accu [8] += cloud[index].z; + } + } + else + { + point_count = 0; + for (const int &index : indices) + { + if (!isFinite (cloud[index])) + continue; + + ++point_count; + accu [0] += cloud[index].x * cloud[index].x; + accu [1] += cloud[index].x * cloud[index].y; + accu [2] += cloud[index].x * cloud[index].z; + accu [3] += cloud[index].y * cloud[index].y; // 4 + accu [4] += cloud[index].y * cloud[index].z; // 5 + accu [5] += cloud[index].z * cloud[index].z; // 8 + accu [6] += cloud[index].x; + accu [7] += cloud[index].y; + accu [8] += cloud[index].z; + } + } + + accu /= static_cast (point_count); + //Eigen::Vector3f vec = accu.tail<3> (); + //centroid.head<3> () = vec;//= accu.tail<3> (); + //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0 + centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; + centroid[3] = 1; + covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; + covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; + covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; + covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; + covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; + covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; + covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); + covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); + covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); + + return (static_cast (point_count)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix &covariance_matrix, + Eigen::Matrix ¢roid) +{ + return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out, + int npts) +{ + // Calculate the number of points if not given + if (npts == 0) + { + while (cloud_iterator.isValid ()) + { + ++npts; + ++cloud_iterator; + } + cloud_iterator.reset (); + } + + int i = 0; + cloud_out.resize (npts); + // Subtract the centroid from cloud_in + while (cloud_iterator.isValid ()) + { + cloud_out[i].x = cloud_iterator->x - centroid[0]; + cloud_out[i].y = cloud_iterator->y - centroid[1]; + cloud_out[i].z = cloud_iterator->z - centroid[2]; + ++i; + ++cloud_iterator; + } + cloud_out.width = cloud_out.size (); + cloud_out.height = 1; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (const pcl::PointCloud &cloud_in, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out) +{ + cloud_out = cloud_in; + + // Subtract the centroid from cloud_in + for (auto& point: cloud_out) + { + point.x -= static_cast (centroid[0]); + point.y -= static_cast (centroid[1]); + point.z -= static_cast (centroid[2]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out) +{ + cloud_out.header = cloud_in.header; + cloud_out.is_dense = cloud_in.is_dense; + if (indices.size () == cloud_in.points.size ()) + { + cloud_out.width = cloud_in.width; + cloud_out.height = cloud_in.height; + } + else + { + cloud_out.width = static_cast (indices.size ()); + cloud_out.height = 1; + } + cloud_out.resize (indices.size ()); + + // Subtract the centroid from cloud_in + for (std::size_t i = 0; i < indices.size (); ++i) + { + cloud_out[i].x = static_cast (cloud_in[indices[i]].x - centroid[0]); + cloud_out[i].y = static_cast (cloud_in[indices[i]].y - centroid[1]); + cloud_out[i].z = static_cast (cloud_in[indices[i]].z - centroid[2]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices& indices, + const Eigen::Matrix ¢roid, + pcl::PointCloud &cloud_out) +{ + return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (ConstCloudIterator &cloud_iterator, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out, + int npts) +{ + // Calculate the number of points if not given + if (npts == 0) + { + while (cloud_iterator.isValid ()) + { + ++npts; + ++cloud_iterator; + } + cloud_iterator.reset (); + } + + cloud_out = Eigen::Matrix::Zero (4, npts); // keep the data aligned + + int i = 0; + while (cloud_iterator.isValid ()) + { + cloud_out (0, i) = cloud_iterator->x - centroid[0]; + cloud_out (1, i) = cloud_iterator->y - centroid[1]; + cloud_out (2, i) = cloud_iterator->z - centroid[2]; + ++i; + ++cloud_iterator; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (const pcl::PointCloud &cloud_in, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out) +{ + std::size_t npts = cloud_in.size (); + + cloud_out = Eigen::Matrix::Zero (4, npts); // keep the data aligned + + for (std::size_t i = 0; i < npts; ++i) + { + cloud_out (0, i) = cloud_in[i].x - centroid[0]; + cloud_out (1, i) = cloud_in[i].y - centroid[1]; + cloud_out (2, i) = cloud_in[i].z - centroid[2]; + // One column at a time + //cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid; + } + + // Make sure we zero the 4th dimension out (1 row, N columns) + //cloud_out.block (3, 0, 1, npts).setZero (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out) +{ + std::size_t npts = indices.size (); + + cloud_out = Eigen::Matrix::Zero (4, npts); // keep the data aligned + + for (std::size_t i = 0; i < npts; ++i) + { + cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0]; + cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1]; + cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2]; + // One column at a time + //cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid; + } + + // Make sure we zero the 4th dimension out (1 row, N columns) + //cloud_out.block (3, 0, 1, npts).setZero (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::demeanPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + const Eigen::Matrix ¢roid, + Eigen::Matrix &cloud_out) +{ + return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeNDCentroid (const pcl::PointCloud &cloud, + Eigen::Matrix ¢roid) +{ + using FieldList = typename pcl::traits::fieldList::type; + + // Get the size of the fields + centroid.setZero (boost::mpl::size::value); + + if (cloud.empty ()) + return; + + // Iterate over each point + for (const auto& pt: cloud) + { + // Iterate over each dimension + pcl::for_each_type (NdCentroidFunctor (pt, centroid)); + } + centroid /= static_cast (cloud.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeNDCentroid (const pcl::PointCloud &cloud, + const std::vector &indices, + Eigen::Matrix ¢roid) +{ + using FieldList = typename pcl::traits::fieldList::type; + + // Get the size of the fields + centroid.setZero (boost::mpl::size::value); + + if (indices.empty ()) + return; + + // Iterate over each point + for (const auto& index: indices) + { + // Iterate over each dimension + pcl::for_each_type (NdCentroidFunctor (cloud[index], centroid)); + } + centroid /= static_cast (indices.size ()); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeNDCentroid (const pcl::PointCloud &cloud, + const pcl::PointIndices &indices, + Eigen::Matrix ¢roid) +{ + return (pcl::computeNDCentroid (cloud, indices.indices, centroid)); +} + +template void +pcl::CentroidPoint::add (const PointT& point) +{ + // Invoke add point on each accumulator + boost::fusion::for_each (accumulators_, detail::AddPoint (point)); + ++num_points_; +} + +template +template void +pcl::CentroidPoint::get (PointOutT& point) const +{ + if (num_points_ != 0) + { + // Filter accumulators so that only those that are compatible with + // both PointT and requested point type remain + auto ca = boost::fusion::filter_if> (accumulators_); + // Invoke get point on each accumulator in filtered list + boost::fusion::for_each (ca, detail::GetPoint (point, num_points_)); + } +} + +template std::size_t +pcl::computeCentroid (const pcl::PointCloud& cloud, + PointOutT& centroid) +{ + pcl::CentroidPoint cp; + + if (cloud.is_dense) + for (const auto& point: cloud) + cp.add (point); + else + for (const auto& point: cloud) + if (pcl::isFinite (point)) + cp.add (point); + + cp.get (centroid); + return (cp.getSize ()); +} + +template std::size_t +pcl::computeCentroid (const pcl::PointCloud& cloud, + const std::vector& indices, + PointOutT& centroid) +{ + pcl::CentroidPoint cp; + + if (cloud.is_dense) + for (const int &index : indices) + cp.add (cloud[index]); + else + for (const int &index : indices) + if (pcl::isFinite (cloud[index])) + cp.add (cloud[index]); + + cp.get (centroid); + return (cp.getSize ()); +} + +#endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/common.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/common.hpp new file mode 100755 index 0000000..ecf7f5a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/common.hpp @@ -0,0 +1,440 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_COMMON_IMPL_H_ +#define PCL_COMMON_IMPL_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +inline double +pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree) +{ + // Compute the actual angle + double rad = v1.normalized ().dot (v2.normalized ()); + if (rad < -1.0) + rad = -1.0; + else if (rad > 1.0) + rad = 1.0; + return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad)); +} + +inline double +pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree) +{ + // Compute the actual angle + double rad = v1.normalized ().dot (v2.normalized ()); + if (rad < -1.0) + rad = -1.0; + else if (rad > 1.0) + rad = 1.0; + return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +inline void +pcl::getMeanStd (const std::vector &values, double &mean, double &stddev) +{ + // throw an exception when the input array is empty + if (values.empty ()) + { + PCL_THROW_EXCEPTION (BadArgumentException, "Input array must have at least 1 element."); + } + + // when the array has only one element, mean is the number itself and standard dev is 0 + if (values.size () == 1) + { + mean = values.at (0); + stddev = 0; + return; + } + + double sum = 0, sq_sum = 0; + + for (const float &value : values) + { + sum += value; + sq_sum += value * value; + } + mean = sum / static_cast(values.size ()); + double variance = (sq_sum - sum * sum / static_cast(values.size ())) / (static_cast(values.size ()) - 1); + stddev = sqrt (variance); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getPointsInBox (const pcl::PointCloud &cloud, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, + std::vector &indices) +{ + indices.resize (cloud.points.size ()); + int l = 0; + + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + // Check if the point is inside bounds + if (cloud.points[i].x < min_pt[0] || cloud.points[i].y < min_pt[1] || cloud.points[i].z < min_pt[2]) + continue; + if (cloud.points[i].x > max_pt[0] || cloud.points[i].y > max_pt[1] || cloud.points[i].z > max_pt[2]) + continue; + indices[l++] = int (i); + } + } + // NaN or Inf values could exist => check for them + else + { + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + // Check if the point is invalid + if (!std::isfinite (cloud.points[i].x) || + !std::isfinite (cloud.points[i].y) || + !std::isfinite (cloud.points[i].z)) + continue; + // Check if the point is inside bounds + if (cloud.points[i].x < min_pt[0] || cloud.points[i].y < min_pt[1] || cloud.points[i].z < min_pt[2]) + continue; + if (cloud.points[i].x > max_pt[0] || cloud.points[i].y > max_pt[1] || cloud.points[i].z > max_pt[2]) + continue; + indices[l++] = int (i); + } + } + indices.resize (l); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getMaxDistance (const pcl::PointCloud &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt) +{ + float max_dist = -FLT_MAX; + int max_idx = -1; + float dist; + const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> (); + + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap (); + dist = (pivot_pt3 - pt).norm (); + if (dist > max_dist) + { + max_idx = int (i); + max_dist = dist; + } + } + } + // NaN or Inf values could exist => check for them + else + { + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + // Check if the point is invalid + if (!std::isfinite (cloud.points[i].x) || !std::isfinite (cloud.points[i].y) || !std::isfinite (cloud.points[i].z)) + continue; + pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap (); + dist = (pivot_pt3 - pt).norm (); + if (dist > max_dist) + { + max_idx = int (i); + max_dist = dist; + } + } + } + + if(max_idx != -1) + max_pt = cloud.points[max_idx].getVector4fMap (); + else + max_pt = Eigen::Vector4f(std::numeric_limits::quiet_NaN(),std::numeric_limits::quiet_NaN(),std::numeric_limits::quiet_NaN(),std::numeric_limits::quiet_NaN()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getMaxDistance (const pcl::PointCloud &cloud, const std::vector &indices, + const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt) +{ + float max_dist = -FLT_MAX; + int max_idx = -1; + float dist; + const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> (); + + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (std::size_t i = 0; i < indices.size (); ++i) + { + pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap (); + dist = (pivot_pt3 - pt).norm (); + if (dist > max_dist) + { + max_idx = static_cast (i); + max_dist = dist; + } + } + } + // NaN or Inf values could exist => check for them + else + { + for (std::size_t i = 0; i < indices.size (); ++i) + { + // Check if the point is invalid + if (!std::isfinite (cloud.points[indices[i]].x) || !std::isfinite (cloud.points[indices[i]].y) + || + !std::isfinite (cloud.points[indices[i]].z)) + continue; + + pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap (); + dist = (pivot_pt3 - pt).norm (); + if (dist > max_dist) + { + max_idx = static_cast (i); + max_dist = dist; + } + } + } + + if(max_idx != -1) + max_pt = cloud.points[indices[max_idx]].getVector4fMap (); + else + max_pt = Eigen::Vector4f(std::numeric_limits::quiet_NaN(),std::numeric_limits::quiet_NaN(),std::numeric_limits::quiet_NaN(),std::numeric_limits::quiet_NaN()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getMinMax3D (const pcl::PointCloud &cloud, PointT &min_pt, PointT &max_pt) +{ + Eigen::Array4f min_p, max_p; + min_p.setConstant (FLT_MAX); + max_p.setConstant (-FLT_MAX); + + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (const auto& point: cloud.points) + { + const auto pt = point.getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + // NaN or Inf values could exist => check for them + else + { + for (const auto& point: cloud.points) + { + // Check if the point is invalid + if (!std::isfinite (point.x) || + !std::isfinite (point.y) || + !std::isfinite (point.z)) + continue; + const auto pt = point.getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2]; + max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2]; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getMinMax3D (const pcl::PointCloud &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) +{ + Eigen::Array4f min_p, max_p; + min_p.setConstant (FLT_MAX); + max_p.setConstant (-FLT_MAX); + + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (const auto& point: cloud.points) + { + const auto pt = point.getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + // NaN or Inf values could exist => check for them + else + { + for (const auto& point: cloud.points) + { + // Check if the point is invalid + if (!std::isfinite (point.x) || + !std::isfinite (point.y) || + !std::isfinite (point.z)) + continue; + const auto pt = point.getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + min_pt = min_p; + max_pt = max_p; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getMinMax3D (const pcl::PointCloud &cloud, const pcl::PointIndices &indices, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) +{ + Eigen::Array4f min_p, max_p; + min_p.setConstant (FLT_MAX); + max_p.setConstant (-FLT_MAX); + + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (const int &index : indices.indices) + { + pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + // NaN or Inf values could exist => check for them + else + { + for (const int &index : indices.indices) + { + // Check if the point is invalid + if (!std::isfinite (cloud.points[index].x) || + !std::isfinite (cloud.points[index].y) || + !std::isfinite (cloud.points[index].z)) + continue; + pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + min_pt = min_p; + max_pt = max_p; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getMinMax3D (const pcl::PointCloud &cloud, const std::vector &indices, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) +{ + min_pt.setConstant (FLT_MAX); + max_pt.setConstant (-FLT_MAX); + + // If the data is dense, we don't need to check for NaN + if (cloud.is_dense) + { + for (const int &index : indices) + { + pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap (); + min_pt = min_pt.array ().min (pt); + max_pt = max_pt.array ().max (pt); + } + } + // NaN or Inf values could exist => check for them + else + { + for (const int &index : indices) + { + // Check if the point is invalid + if (!std::isfinite (cloud.points[index].x) || + !std::isfinite (cloud.points[index].y) || + !std::isfinite (cloud.points[index].z)) + continue; + pcl::Array4fMapConst pt = cloud.points[index].getArray4fMap (); + min_pt = min_pt.array ().min (pt); + max_pt = max_pt.array ().max (pt); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline double +pcl::getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc) +{ + Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0); + Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0); + Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0); + + double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm (); + // Calculate the area of the triangle using Heron's formula + // (http://en.wikipedia.org/wiki/Heron's_formula) + double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0; + double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3)); + // Compute the radius of the circumscribed circle + return ((p2p1 * p3p2 * p1p3) / (4.0 * area)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::getMinMax (const PointT &histogram, int len, float &min_p, float &max_p) +{ + min_p = FLT_MAX; + max_p = -FLT_MAX; + + for (int i = 0; i < len; ++i) + { + min_p = (histogram[i] > min_p) ? min_p : histogram[i]; + max_p = (histogram[i] < max_p) ? max_p : histogram[i]; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline float +pcl::calculatePolygonArea (const pcl::PointCloud &polygon) +{ + float area = 0.0f; + int num_points = polygon.size (); + Eigen::Vector3f va,vb,res; + + res(0) = res(1) = res(2) = 0.0f; + for (int i = 0; i < num_points; ++i) + { + int j = (i + 1) % num_points; + va = polygon[i].getVector3fMap (); + vb = polygon[j].getVector3fMap (); + res += va.cross (vb); + } + area = res.norm (); + return (area*0.5); +} + +#endif //#ifndef PCL_COMMON_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/copy_point.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/copy_point.hpp new file mode 100755 index 0000000..a168226 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/copy_point.hpp @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_IMPL_COPY_POINT_HPP_ +#define PCL_COMMON_IMPL_COPY_POINT_HPP_ + +#include +#include +#include +#include + +namespace pcl +{ + + namespace detail + { + + /* CopyPointHelper and its specializations copy the contents of a source + * point to a target point. There are three cases: + * + * - Points have the same type. + * In this case a single `memcpy` is used. + * + * - Points have different types and one of the following is true: + * * both have RGB fields; + * * both have RGBA fields; + * * one or both have no RGB/RGBA fields. + * In this case we find the list of common fields and copy their + * contents one by one with `NdConcatenateFunctor`. + * + * - Points have different types and one of these types has RGB field, and + * the other has RGBA field. + * In this case we also find the list of common fields and copy their + * contents. In order to account for the fact that RGB and RGBA do not + * match we have an additional `memcpy` to copy the contents of one into + * another. + * + * An appropriate version of CopyPointHelper is instantiated during + * compilation time automatically, so there is absolutely no run-time + * overhead. */ + + template + struct CopyPointHelper { }; + + template + struct CopyPointHelper::value>> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + memcpy (&point_out, &point_in, sizeof (PointInT)); + } + }; + + template + struct CopyPointHelper>, + boost::mpl::or_>, + boost::mpl::not_>, + boost::mpl::and_, + pcl::traits::has_field>, + boost::mpl::and_, + pcl::traits::has_field>>>::value>> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + using FieldListInT = typename pcl::traits::fieldList::type; + using FieldListOutT = typename pcl::traits::fieldList::type; + using FieldList = typename pcl::intersect::type; + pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, point_out)); + } + }; + + template + struct CopyPointHelper>, + boost::mpl::or_, + pcl::traits::has_field>, + boost::mpl::and_, + pcl::traits::has_field>>>::value>> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + using FieldListInT = typename pcl::traits::fieldList::type; + using FieldListOutT = typename pcl::traits::fieldList::type; + using FieldList = typename pcl::intersect::type; + const std::uint32_t offset_in = boost::mpl::if_, + pcl::traits::offset, + pcl::traits::offset >::type::value; + const std::uint32_t offset_out = boost::mpl::if_, + pcl::traits::offset, + pcl::traits::offset >::type::value; + pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, point_out)); + memcpy (reinterpret_cast (&point_out) + offset_out, + reinterpret_cast (&point_in) + offset_in, + 4); + } + }; + + } + +} + +template void +pcl::copyPoint (const PointInT& point_in, PointOutT& point_out) +{ + detail::CopyPointHelper copy; + copy (point_in, point_out); +} + +#endif //PCL_COMMON_IMPL_COPY_POINT_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/eigen.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/eigen.hpp new file mode 100755 index 0000000..d41f205 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/eigen.hpp @@ -0,0 +1,904 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR a PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_EIGEN_IMPL_HPP_ +#define PCL_COMMON_EIGEN_IMPL_HPP_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots) +{ + roots (0) = Scalar (0); + Scalar d = Scalar (b * b - 4.0 * c); + if (d < 0.0) // no real roots ! THIS SHOULD NOT HAPPEN! + d = 0.0; + + Scalar sd = std::sqrt (d); + + roots (2) = 0.5f * (b + sd); + roots (1) = 0.5f * (b - sd); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeRoots (const Matrix& m, Roots& roots) +{ + using Scalar = typename Matrix::Scalar; + + // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The + // eigenvalues are the roots to this equation, all guaranteed to be + // real-valued, because the matrix is symmetric. + Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2) + + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2) + - m (0, 0) * m (1, 2) * m (1, 2) + - m (1, 1) * m (0, 2) * m (0, 2) + - m (2, 2) * m (0, 1) * m (0, 1); + Scalar c1 = m (0, 0) * m (1, 1) - + m (0, 1) * m (0, 1) + + m (0, 0) * m (2, 2) - + m (0, 2) * m (0, 2) + + m (1, 1) * m (2, 2) - + m (1, 2) * m (1, 2); + Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2); + + if (std::abs (c0) < Eigen::NumTraits < Scalar > ::epsilon ()) // one root is 0 -> quadratic equation + computeRoots2 (c2, c1, roots); + else + { + const Scalar s_inv3 = Scalar (1.0 / 3.0); + const Scalar s_sqrt3 = std::sqrt (Scalar (3.0)); + // Construct the parameters used in classifying the roots of the equation + // and in solving the equation for the roots in closed form. + Scalar c2_over_3 = c2 * s_inv3; + Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3; + if (a_over_3 > Scalar (0)) + a_over_3 = Scalar (0); + + Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1)); + + Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3; + if (q > Scalar (0)) + q = Scalar (0); + + // Compute the eigenvalues by solving for the roots of the polynomial. + Scalar rho = std::sqrt (-a_over_3); + Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3; + Scalar cos_theta = std::cos (theta); + Scalar sin_theta = std::sin (theta); + roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta; + roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta); + roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta); + + // Sort in increasing order. + if (roots (0) >= roots (1)) + std::swap (roots (0), roots (1)); + if (roots (1) >= roots (2)) + { + std::swap (roots (1), roots (2)); + if (roots (0) >= roots (1)) + std::swap (roots (0), roots (1)); + } + + if (roots (0) <= 0) // eigenval for symmetric positive semi-definite matrix can not be negative! Set it to 0 + computeRoots2 (c2, c1, roots); + } +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) +{ + // if diagonal matrix, the eigenvalues are the diagonal elements + // and the eigenvectors are not unique, thus set to Identity + if (std::abs (mat.coeff (1)) <= std::numeric_limits::min ()) + { + if (mat.coeff (0) < mat.coeff (2)) + { + eigenvalue = mat.coeff (0); + eigenvector[0] = 1.0; + eigenvector[1] = 0.0; + } + else + { + eigenvalue = mat.coeff (2); + eigenvector[0] = 0.0; + eigenvector[1] = 1.0; + } + return; + } + + // 0.5 to optimize further calculations + typename Matrix::Scalar trace = static_cast (0.5) * (mat.coeff (0) + mat.coeff (3)); + typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); + + typename Matrix::Scalar temp = trace * trace - determinant; + + if (temp < 0) + temp = 0; + + eigenvalue = trace - std::sqrt (temp); + + eigenvector[0] = -mat.coeff (1); + eigenvector[1] = mat.coeff (0) - eigenvalue; + eigenvector.normalize (); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues) +{ + // if diagonal matrix, the eigenvalues are the diagonal elements + // and the eigenvectors are not unique, thus set to Identity + if (std::abs (mat.coeff (1)) <= std::numeric_limits::min ()) + { + if (mat.coeff (0) < mat.coeff (3)) + { + eigenvalues.coeffRef (0) = mat.coeff (0); + eigenvalues.coeffRef (1) = mat.coeff (3); + eigenvectors.coeffRef (0) = 1.0; + eigenvectors.coeffRef (1) = 0.0; + eigenvectors.coeffRef (2) = 0.0; + eigenvectors.coeffRef (3) = 1.0; + } + else + { + eigenvalues.coeffRef (0) = mat.coeff (3); + eigenvalues.coeffRef (1) = mat.coeff (0); + eigenvectors.coeffRef (0) = 0.0; + eigenvectors.coeffRef (1) = 1.0; + eigenvectors.coeffRef (2) = 1.0; + eigenvectors.coeffRef (3) = 0.0; + } + return; + } + + // 0.5 to optimize further calculations + typename Matrix::Scalar trace = static_cast (0.5) * (mat.coeff (0) + mat.coeff (3)); + typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); + + typename Matrix::Scalar temp = trace * trace - determinant; + + if (temp < 0) + temp = 0; + else + temp = std::sqrt (temp); + + eigenvalues.coeffRef (0) = trace - temp; + eigenvalues.coeffRef (1) = trace + temp; + + // either this is in a row or column depending on RowMajor or ColumnMajor + eigenvectors.coeffRef (0) = -mat.coeff (1); + eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0); + typename Matrix::Scalar norm = static_cast (1.0) + / static_cast (std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2))); + eigenvectors.coeffRef (0) *= norm; + eigenvectors.coeffRef (2) *= norm; + eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2); + eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector) +{ + using Scalar = typename Matrix::Scalar; + // Scale the matrix so its entries are in [-1,1]. The scaling is applied + // only when at least one matrix entry has magnitude larger than 1. + + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + + scaledMat.diagonal ().array () -= eigenvalue / scale; + + Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); + Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); + Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); + + Scalar len1 = vec1.squaredNorm (); + Scalar len2 = vec2.squaredNorm (); + Scalar len3 = vec3.squaredNorm (); + + if (len1 >= len2 && len1 >= len3) + eigenvector = vec1 / std::sqrt (len1); + else if (len2 >= len1 && len2 >= len3) + eigenvector = vec2 / std::sqrt (len2); + else + eigenvector = vec3 / std::sqrt (len3); +} + +namespace pcl { +namespace detail{ +template +struct EigenVector { + Vector vector; + Scalar length; +}; // struct EigenVector + +/** + * @brief returns the unit vector along the largest eigen value as well as the + * length of the largest eigenvector + * @tparam Vector Requested result type, needs to be explicitly provided and has + * to be implicitly constructible from ConstRowExpr + * @tparam Matrix deduced input type providing similar in API as Eigen::Matrix + */ +template static EigenVector +getLargest3x3Eigenvector (const Matrix scaledMatrix) +{ + using Scalar = typename Matrix::Scalar; + using Index = typename Matrix::Index; + + Matrix crossProduct; + crossProduct << scaledMatrix.row (0).cross (scaledMatrix.row (1)), + scaledMatrix.row (0).cross (scaledMatrix.row (2)), + scaledMatrix.row (1).cross (scaledMatrix.row (2)); + + // expression template, no evaluation here + const auto len = crossProduct.rowwise ().norm (); + + Index index; + const Scalar length = len.maxCoeff (&index); // <- first evaluation + return EigenVector {crossProduct.row (index) / length, + length}; +} +} // namespace detail +} // namespace pcl + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) +{ + using Scalar = typename Matrix::Scalar; + // Scale the matrix so its entries are in [-1,1]. The scaling is applied + // only when at least one matrix entry has magnitude larger than 1. + + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + + Vector eigenvalues; + computeRoots (scaledMat, eigenvalues); + + eigenvalue = eigenvalues (0) * scale; + + scaledMat.diagonal ().array () -= eigenvalues (0); + + eigenvector = detail::getLargest3x3Eigenvector (scaledMat).vector; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen33 (const Matrix& mat, Vector& evals) +{ + using Scalar = typename Matrix::Scalar; + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + computeRoots (scaledMat, evals); + evals *= scale; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) +{ + using Scalar = typename Matrix::Scalar; + // Scale the matrix so its entries are in [-1,1]. The scaling is applied + // only when at least one matrix entry has magnitude larger than 1. + + Scalar scale = mat.cwiseAbs ().maxCoeff (); + if (scale <= std::numeric_limits < Scalar > ::min ()) + scale = Scalar (1.0); + + Matrix scaledMat = mat / scale; + + // Compute the eigenvalues + computeRoots (scaledMat, evals); + + if ( (evals (2) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ()) + { + // all three equal + evecs.setIdentity (); + } + else if ( (evals (1) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ()) + { + // first and second equal + Matrix tmp; + tmp = scaledMat; + tmp.diagonal ().array () -= evals (2); + + evecs.col (2) = detail::getLargest3x3Eigenvector (tmp).vector; + evecs.col (1) = evecs.col (2).unitOrthogonal (); + evecs.col (0) = evecs.col (1).cross (evecs.col (2)); + } + else if ( (evals (2) - evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ()) + { + // second and third equal + Matrix tmp; + tmp = scaledMat; + tmp.diagonal ().array () -= evals (0); + + evecs.col (0) = detail::getLargest3x3Eigenvector (tmp).vector; + evecs.col (1) = evecs.col (0).unitOrthogonal (); + evecs.col (2) = evecs.col (0).cross (evecs.col (1)); + } + else + { + std::array eigenVecLen; + + for (int i = 0; i < 3; ++i) + { + Matrix tmp = scaledMat; + tmp.diagonal ().array () -= evals (i); + const auto vec_len = detail::getLargest3x3Eigenvector (tmp); + evecs.col (i) = vec_len.vector; + eigenVecLen[i] = vec_len.length; + } + + // @TODO: might be redundant or over-complicated as per @SergioRAgostinho + // see: https://github.com/PointCloudLibrary/pcl/pull/3441#discussion_r341024181 + const auto minmax_it = std::minmax_element (eigenVecLen.cbegin (), eigenVecLen.cend ()); + int min_idx = std::distance (eigenVecLen.cbegin (), minmax_it.first); + int max_idx = std::distance (eigenVecLen.cbegin (), minmax_it.second); + int mid_idx = 3 - min_idx - max_idx; + + evecs.col (min_idx) = evecs.col ( (min_idx + 1) % 3).cross (evecs.col ( (min_idx + 2) % 3)).normalized (); + evecs.col (mid_idx) = evecs.col ( (mid_idx + 1) % 3).cross (evecs.col ( (mid_idx + 2) % 3)).normalized (); + } + // Rescale back to the original size. + evals *= scale; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::invert2x2 (const Matrix& matrix, Matrix& inverse) +{ + using Scalar = typename Matrix::Scalar; + Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2); + + if (det != 0) + { + //Scalar inv_det = Scalar (1.0) / det; + inverse.coeffRef (0) = matrix.coeff (3); + inverse.coeffRef (1) = -matrix.coeff (1); + inverse.coeffRef (2) = -matrix.coeff (2); + inverse.coeffRef (3) = matrix.coeff (0); + inverse /= det; + } + return det; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse) +{ + using Scalar = typename Matrix::Scalar; + // elements + // a b c + // b d e + // c e f + //| a b c |-1 | fd-ee ce-bf be-cd | + //| b d e | = 1/det * | ce-bf af-cc bc-ae | + //| c e f | | be-cd bc-ae ad-bb | + + //det = a(fd-ee) + b(ec-fb) + c(eb-dc) + + Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5); + Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8); + Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4); + + Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd; + + if (det != 0) + { + //Scalar inv_det = Scalar (1.0) / det; + inverse.coeffRef (0) = fd_ee; + inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf; + inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd; + inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2)); + inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5)); + inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1)); + inverse /= det; + } + return det; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::invert3x3Matrix (const Matrix& matrix, Matrix& inverse) +{ + using Scalar = typename Matrix::Scalar; + + //| a b c |-1 | ie-hf hc-ib fb-ec | + //| d e f | = 1/det * | gf-id ia-gc dc-fa | + //| g h i | | hd-ge gb-ha ea-db | + //det = a(ie-hf) + d(hc-ib) + g(fb-ec) + + Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5); + Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1); + Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2); + Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec); + + if (det != 0) + { + inverse.coeffRef (0) = ie_hf; + inverse.coeffRef (1) = hc_ib; + inverse.coeffRef (2) = fb_ec; + inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3); + inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2); + inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0); + inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4); + inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0); + inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1); + + inverse /= det; + } + return det; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template inline typename Matrix::Scalar +pcl::determinant3x3Matrix (const Matrix& matrix) +{ + // result is independent of Row/Col Major storage! + return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) + + matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) + + matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ; +} + +////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, + const Eigen::Vector3f& y_direction, + Eigen::Affine3f& transformation) +{ + Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized(); + Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized(); + Eigen::Vector3f tmp2 = z_axis.normalized(); + + transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f; + transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f; + transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f; + transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f; +} + +////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Affine3f +pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, + const Eigen::Vector3f& y_direction) +{ + Eigen::Affine3f transformation; + getTransFromUnitVectorsZY (z_axis, y_direction, transformation); + return (transformation); +} + +////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, + const Eigen::Vector3f& y_direction, + Eigen::Affine3f& transformation) +{ + Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized(); + Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized(); + Eigen::Vector3f tmp0 = x_axis.normalized(); + + transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f; + transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f; + transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f; + transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f; +} + +////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Affine3f +pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, + const Eigen::Vector3f& y_direction) +{ + Eigen::Affine3f transformation; + getTransFromUnitVectorsXY (x_axis, y_direction, transformation); + return (transformation); +} + +////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, + const Eigen::Vector3f& z_axis, + Eigen::Affine3f& transformation) +{ + getTransFromUnitVectorsZY (z_axis, y_direction, transformation); +} + +////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Affine3f +pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, + const Eigen::Vector3f& z_axis) +{ + Eigen::Affine3f transformation; + getTransformationFromTwoUnitVectors (y_direction, z_axis, transformation); + return (transformation); +} + +void +pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, + const Eigen::Vector3f& z_axis, + const Eigen::Vector3f& origin, + Eigen::Affine3f& transformation) +{ + getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation); + Eigen::Vector3f translation = transformation*origin; + transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2]; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getEulerAngles (const Eigen::Transform &t, Scalar &roll, Scalar &pitch, Scalar &yaw) +{ + roll = std::atan2 (t (2, 1), t (2, 2)); + pitch = asin (-t (2, 0)); + yaw = std::atan2 (t (1, 0), t (0, 0)); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getTranslationAndEulerAngles (const Eigen::Transform &t, + Scalar &x, Scalar &y, Scalar &z, + Scalar &roll, Scalar &pitch, Scalar &yaw) +{ + x = t (0, 3); + y = t (1, 3); + z = t (2, 3); + roll = std::atan2 (t (2, 1), t (2, 2)); + pitch = asin (-t (2, 0)); + yaw = std::atan2 (t (1, 0), t (0, 0)); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getTransformation (Scalar x, Scalar y, Scalar z, + Scalar roll, Scalar pitch, Scalar yaw, + Eigen::Transform &t) +{ + Scalar A = std::cos (yaw), B = sin (yaw), C = std::cos (pitch), D = sin (pitch), + E = std::cos (roll), F = sin (roll), DE = D*E, DF = D*F; + + t (0, 0) = A*C; t (0, 1) = A*DF - B*E; t (0, 2) = B*F + A*DE; t (0, 3) = x; + t (1, 0) = B*C; t (1, 1) = A*E + B*DF; t (1, 2) = B*DE - A*F; t (1, 3) = y; + t (2, 0) = -D; t (2, 1) = C*F; t (2, 2) = C*E; t (2, 3) = z; + t (3, 0) = 0; t (3, 1) = 0; t (3, 2) = 0; t (3, 3) = 1; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::saveBinary (const Eigen::MatrixBase& matrix, std::ostream& file) +{ + std::uint32_t rows = static_cast (matrix.rows ()), cols = static_cast (matrix.cols ()); + file.write (reinterpret_cast (&rows), sizeof (rows)); + file.write (reinterpret_cast (&cols), sizeof (cols)); + for (std::uint32_t i = 0; i < rows; ++i) + for (std::uint32_t j = 0; j < cols; ++j) + { + typename Derived::Scalar tmp = matrix(i,j); + file.write (reinterpret_cast (&tmp), sizeof (tmp)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::loadBinary (Eigen::MatrixBase const & matrix_, std::istream& file) +{ + Eigen::MatrixBase &matrix = const_cast &> (matrix_); + + std::uint32_t rows, cols; + file.read (reinterpret_cast (&rows), sizeof (rows)); + file.read (reinterpret_cast (&cols), sizeof (cols)); + if (matrix.rows () != static_cast(rows) || matrix.cols () != static_cast(cols)) + matrix.derived().resize(rows, cols); + + for (std::uint32_t i = 0; i < rows; ++i) + for (std::uint32_t j = 0; j < cols; ++j) + { + typename Derived::Scalar tmp; + file.read (reinterpret_cast (&tmp), sizeof (tmp)); + matrix (i, j) = tmp; + } +} + +////////////////////////////////////////////////////////////////////////////////////////// +template +typename Eigen::internal::umeyama_transform_matrix_type::type +pcl::umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase& dst, bool with_scaling) +{ +#if EIGEN_VERSION_AT_LEAST (3, 3, 0) + return Eigen::umeyama (src, dst, with_scaling); +#else + using TransformationMatrixType = typename Eigen::internal::umeyama_transform_matrix_type::type; + using Scalar = typename Eigen::internal::traits::Scalar; + using RealScalar = typename Eigen::NumTraits::Real; + using Index = typename Derived::Index; + + static_assert (!Eigen::NumTraits::IsComplex, "Numeric type must be real."); + static_assert ((Eigen::internal::is_same::Scalar>::value), + "You mixed different numeric types. You need to use the cast method of matrixbase to cast numeric types explicitly."); + + enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) }; + + using VectorType = Eigen::Matrix; + using MatrixType = Eigen::Matrix; + using RowMajorMatrixType = typename Eigen::internal::plain_matrix_type_row_major::type; + + const Index m = src.rows (); // dimension + const Index n = src.cols (); // number of measurements + + // required for demeaning ... + const RealScalar one_over_n = 1 / static_cast (n); + + // computation of mean + const VectorType src_mean = src.rowwise ().sum () * one_over_n; + const VectorType dst_mean = dst.rowwise ().sum () * one_over_n; + + // demeaning of src and dst points + const RowMajorMatrixType src_demean = src.colwise () - src_mean; + const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean; + + // Eq. (36)-(37) + const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n; + + // Eq. (38) + const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ()); + + Eigen::JacobiSVD svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // Initialize the resulting transformation with an identity matrix... + TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1); + + // Eq. (39) + VectorType S = VectorType::Ones (m); + + if ( svd.matrixU ().determinant () * svd.matrixV ().determinant () < 0 ) + S (m - 1) = -1; + + // Eq. (40) and (43) + Rt.block (0,0,m,m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); + + if (with_scaling) + { + // Eq. (42) + const Scalar c = Scalar (1)/ src_var * svd.singularValues ().dot (S); + + // Eq. (41) + Rt.col (m).head (m) = dst_mean; + Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean; + Rt.block (0, 0, m, m) *= c; + } + else + { + Rt.col (m).head (m) = dst_mean; + Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean; + } + + return (Rt); +#endif +} + +////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::transformLine (const Eigen::Matrix &line_in, + Eigen::Matrix &line_out, + const Eigen::Transform &transformation) +{ + if (line_in.innerSize () != 6 || line_out.innerSize () != 6) + { + PCL_DEBUG ("transformLine: lines size != 6\n"); + return (false); + } + + Eigen::Matrix point, vector; + point << line_in.template head<3> (); + vector << line_out.template tail<3> (); + + pcl::transformPoint (point, point, transformation); + pcl::transformVector (vector, vector, transformation); + line_out << point, vector; + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPlane (const Eigen::Matrix &plane_in, + Eigen::Matrix &plane_out, + const Eigen::Transform &transformation) +{ + Eigen::Hyperplane < Scalar, 3 > plane; + plane.coeffs () << plane_in; + plane.transform (transformation); + plane_out << plane.coeffs (); + + // Versions prior to 3.3.2 don't normalize the result + #if !EIGEN_VERSION_AT_LEAST (3, 3, 2) + plane_out /= plane_out.template head<3> ().norm (); + #endif +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPlane (const pcl::ModelCoefficients::Ptr plane_in, + pcl::ModelCoefficients::Ptr plane_out, + const Eigen::Transform &transformation) +{ + std::vector values (plane_in->values.begin (), plane_in->values.end ()); + Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ()); + pcl::transformPlane (v_plane_in, v_plane_in, transformation); + plane_out->values.resize (4); + std::copy_n(v_plane_in.data (), 4, plane_in->values.begin ()); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::checkCoordinateSystem (const Eigen::Matrix &line_x, + const Eigen::Matrix &line_y, + const Scalar norm_limit, + const Scalar dot_limit) +{ + if (line_x.innerSize () != 6 || line_y.innerSize () != 6) + { + PCL_DEBUG ("checkCoordinateSystem: lines size != 6\n"); + return (false); + } + + if (line_x.template head<3> () != line_y.template head<3> ()) + { + PCL_DEBUG ("checkCoorZdinateSystem: vector origins are different !\n"); + return (false); + } + + // Make a copy of vector directions + // X^Y = Z | Y^Z = X | Z^X = Y + Eigen::Matrix v_line_x (line_x.template tail<3> ()), + v_line_y (line_y.template tail<3> ()), + v_line_z (v_line_x.cross (v_line_y)); + + // Check vectors norms + if (v_line_x.norm () < 1 - norm_limit || v_line_x.norm () > 1 + norm_limit) + { + PCL_DEBUG ("checkCoordinateSystem: line_x norm %d != 1\n", v_line_x.norm ()); + return (false); + } + + if (v_line_y.norm () < 1 - norm_limit || v_line_y.norm () > 1 + norm_limit) + { + PCL_DEBUG ("checkCoordinateSystem: line_y norm %d != 1\n", v_line_y.norm ()); + return (false); + } + + if (v_line_z.norm () < 1 - norm_limit || v_line_z.norm () > 1 + norm_limit) + { + PCL_DEBUG ("checkCoordinateSystem: line_z norm %d != 1\n", v_line_z.norm ()); + return (false); + } + + // Check vectors perendicularity + if (std::abs (v_line_x.dot (v_line_y)) > dot_limit) + { + PCL_DEBUG ("checkCSAxis: line_x dot line_y %e = > %e\n", v_line_x.dot (v_line_y), dot_limit); + return (false); + } + + if (std::abs (v_line_x.dot (v_line_z)) > dot_limit) + { + PCL_DEBUG ("checkCSAxis: line_x dot line_z = %e > %e\n", v_line_x.dot (v_line_z), dot_limit); + return (false); + } + + if (std::abs (v_line_y.dot (v_line_z)) > dot_limit) + { + PCL_DEBUG ("checkCSAxis: line_y dot line_z = %e > %e\n", v_line_y.dot (v_line_z), dot_limit); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::transformBetween2CoordinateSystems (const Eigen::Matrix from_line_x, + const Eigen::Matrix from_line_y, + const Eigen::Matrix to_line_x, + const Eigen::Matrix to_line_y, + Eigen::Transform &transformation) +{ + if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6) + { + PCL_DEBUG ("transformBetween2CoordinateSystems: lines size != 6\n"); + return (false); + } + + // Check if coordinate systems are valid + if (!pcl::checkCoordinateSystem (from_line_x, from_line_y) || !pcl::checkCoordinateSystem (to_line_x, to_line_y)) + { + PCL_DEBUG ("transformBetween2CoordinateSystems: coordinate systems invalid !\n"); + return (false); + } + + // Convert lines into Vector3 : + Eigen::Matrix fr0 (from_line_x.template head<3>()), + fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()), + fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()), + + to0 (to_line_x.template head<3>()), + to1 (to_line_x.template head<3>() + to_line_x.template tail<3>()), + to2 (to_line_y.template head<3>() + to_line_y.template tail<3>()); + + // Code is inspired from http://stackoverflow.com/a/15277421/1816078 + // Define matrices and points : + Eigen::Transform T2, T3 = Eigen::Transform::Identity (); + Eigen::Matrix x1, y1, z1, x2, y2, z2; + + // Axes of the coordinate system "fr" + x1 = (fr1 - fr0).normalized (); // the versor (unitary vector) of the (fr1-fr0) axis vector + y1 = (fr2 - fr0).normalized (); + + // Axes of the coordinate system "to" + x2 = (to1 - to0).normalized (); + y2 = (to2 - to0).normalized (); + + // Transform from CS1 to CS2 + // Note: if fr0 == (0,0,0) --> CS1 == CS2 --> T2 = Identity + T2.linear () << x1, y1, x1.cross (y1); + + // Transform from CS1 to CS3 + T3.linear () << x2, y2, x2.cross (y2); + + // Identity matrix = transform to CS2 to CS3 + // Note: if CS1 == CS2 --> transformation = T3 + transformation = Eigen::Transform::Identity (); + transformation.linear () = T3.linear () * T2.linear ().inverse (); + transformation.translation () = to0 - (transformation.linear () * fr0); + return (true); +} + +#endif //PCL_COMMON_EIGEN_IMPL_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/file_io.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/file_io.hpp new file mode 100755 index 0000000..1c1fe9e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/file_io.hpp @@ -0,0 +1,96 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR a PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_FILE_IO_IMPL_HPP_ +#define PCL_COMMON_FILE_IO_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace pcl +{ + +void getAllPcdFilesInDirectory(const std::string& directory, std::vector& file_names) +{ + boost::filesystem::path p(directory); + if(boost::filesystem::is_directory(p)) + { + for(const auto& entry : boost::make_iterator_range(boost::filesystem::directory_iterator(p), {})) + { + if (boost::filesystem::is_regular_file(entry)) + { + if (entry.path().extension() == ".pcd") + file_names.emplace_back(entry.path().filename().string()); + } + } + } + else + { + std::cerr << "Given path is not a directory\n"; + return; + } + std::sort(file_names.begin(), file_names.end()); +} + +std::string getFilenameWithoutPath(const std::string& input) +{ + std::size_t filename_start = input.find_last_of('/', static_cast(-1)) + 1; + return input.substr(filename_start, input.size()-filename_start); +} + +std::string getFilenameWithoutExtension(const std::string& input) +{ + std::size_t dot_position = input.find_last_of('.', input.size()); + return input.substr(0, dot_position); +} + +std::string getFileExtension(const std::string& input) +{ + std::size_t dot_position = input.find_last_of('.', input.size()); + return input.substr(dot_position+1, input.size()); +} + +} // namespace end + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/gaussian.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/gaussian.hpp new file mode 100755 index 0000000..3631a0d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/gaussian.hpp @@ -0,0 +1,113 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_GAUSSIAN_KERNEL_IMPL +#define PCL_GAUSSIAN_KERNEL_IMPL + +#include + +template void +pcl::GaussianKernel::convolveRows(const pcl::PointCloud &input, + std::function field_accessor, + const Eigen::VectorXf& kernel, + pcl::PointCloud &output) const +{ + assert(kernel.size () % 2 == 1); + int kernel_width = kernel.size () -1; + int radius = kernel.size () / 2.0; + if(output.height < input.height || output.width < input.width) + { + output.width = input.width; + output.height = input.height; + output.points.resize (input.height * input.width); + } + + int i; + for(int j = 0; j < input.height; j++) + { + for (i = 0 ; i < radius ; i++) + output (i,j) = 0; + + for ( ; i < input.width - radius ; i++) { + output (i,j) = 0; + for (int k = kernel_width, l = i - radius; k >= 0 ; k--, l++) + output (i,j) += field_accessor (input (l,j)) * kernel[k]; + } + + for ( ; i < input.width ; i++) + output (i,j) = 0; + } +} + +template void +pcl::GaussianKernel::convolveCols(const pcl::PointCloud &input, + std::function field_accessor, + const Eigen::VectorXf& kernel, + pcl::PointCloud &output) const +{ + assert(kernel.size () % 2 == 1); + int kernel_width = kernel.size () -1; + int radius = kernel.size () / 2.0; + if(output.height < input.height || output.width < input.width) + { + output.width = input.width; + output.height = input.height; + output.points.resize (input.height * input.width); + } + + int j; + for(int i = 0; i < input.width; i++) + { + for (j = 0 ; j < radius ; j++) + output (i,j) = 0; + + for ( ; j < input.height - radius ; j++) { + output (i,j) = 0; + for (int k = kernel_width, l = j - radius ; k >= 0 ; k--, l++) + { + output (i,j) += field_accessor (input (i,l)) * kernel[k]; + } + } + + for ( ; j < input.height ; j++) + output (i,j) = 0; + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/generate.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/generate.hpp new file mode 100755 index 0000000..a2d4dd1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/generate.hpp @@ -0,0 +1,287 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_COMMON_GENERATE_HPP_ +#define PCL_COMMON_GENERATE_HPP_ + +#include + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::CloudGenerator::CloudGenerator () + : x_generator_ () + , y_generator_ () + , z_generator_ () +{} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::CloudGenerator::CloudGenerator (const GeneratorParameters& params) +{ + setParameters (params); +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::CloudGenerator::CloudGenerator (const GeneratorParameters& x_params, + const GeneratorParameters& y_params, + const GeneratorParameters& z_params) + : x_generator_ (x_params) + , y_generator_ (y_params) + , z_generator_ (z_params) +{} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::CloudGenerator::setParameters (const GeneratorParameters& params) +{ + GeneratorParameters y_params = params; + y_params.seed += 1; + GeneratorParameters z_params = y_params; + z_params.seed += 1; + x_generator_.setParameters (params); + y_generator_.setParameters (y_params); + z_generator_.setParameters (z_params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::CloudGenerator::setParametersForX (const GeneratorParameters& x_params) +{ + x_generator_.setParameters (x_params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::CloudGenerator::setParametersForY (const GeneratorParameters& y_params) +{ + y_generator_.setParameters (y_params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::CloudGenerator::setParametersForZ (const GeneratorParameters& z_params) +{ + z_generator_.setParameters (z_params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template const typename pcl::common::CloudGenerator::GeneratorParameters& +pcl::common::CloudGenerator::getParametersForX () const +{ + x_generator_.getParameters (); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template const typename pcl::common::CloudGenerator::GeneratorParameters& +pcl::common::CloudGenerator::getParametersForY () const +{ + y_generator_.getParameters (); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template const typename pcl::common::CloudGenerator::GeneratorParameters& +pcl::common::CloudGenerator::getParametersForZ () const +{ + z_generator_.getParameters (); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template PointT +pcl::common::CloudGenerator::get () +{ + PointT p; + p.x = x_generator_.run (); + p.y = y_generator_.run (); + p.z = z_generator_.run (); + return (p); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::common::CloudGenerator::fill (pcl::PointCloud& cloud) +{ + return (fill (cloud.width, cloud.height, cloud)); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::common::CloudGenerator::fill (int width, int height, pcl::PointCloud& 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 (auto& point: cloud) + { + point.x = x_generator_.run (); + point.y = y_generator_.run (); + point.z = z_generator_.run (); + } + return (0); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::CloudGenerator::CloudGenerator () + : x_generator_ () + , y_generator_ () +{} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::CloudGenerator::CloudGenerator (const GeneratorParameters& x_params, + const GeneratorParameters& y_params) + : x_generator_ (x_params) + , y_generator_ (y_params) +{} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::CloudGenerator::CloudGenerator (const GeneratorParameters& params) +{ + setParameters (params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::CloudGenerator::setParameters (const GeneratorParameters& params) +{ + x_generator_.setParameters (params); + GeneratorParameters y_params = params; + y_params.seed += 1; + y_generator_.setParameters (y_params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::CloudGenerator::setParametersForX (const GeneratorParameters& x_params) +{ + x_generator_.setParameters (x_params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::CloudGenerator::setParametersForY (const GeneratorParameters& y_params) +{ + y_generator_.setParameters (y_params); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template const typename pcl::common::CloudGenerator::GeneratorParameters& +pcl::common::CloudGenerator::getParametersForX () const +{ + x_generator_.getParameters (); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template const typename pcl::common::CloudGenerator::GeneratorParameters& +pcl::common::CloudGenerator::getParametersForY () const +{ + y_generator_.getParameters (); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointXY +pcl::common::CloudGenerator::get () +{ + pcl::PointXY p; + p.x = x_generator_.run (); + p.y = y_generator_.run (); + return (p); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::common::CloudGenerator::fill (pcl::PointCloud& cloud) +{ + return (fill (cloud.width, cloud.height, cloud)); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::common::CloudGenerator::fill (int width, int height, pcl::PointCloud& 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 (auto &point : cloud) + { + point.x = x_generator_.run (); + point.y = y_generator_.run (); + } + return (0); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/intensity.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/intensity.hpp new file mode 100755 index 0000000..92c33a5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/intensity.hpp @@ -0,0 +1,554 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_COMMON_INTENSITY_FIELD_ACCESSOR_IMPL_HPP +#define PCL_COMMON_INTENSITY_FIELD_ACCESSOR_IMPL_HPP + +#include +namespace pcl +{ + namespace common + { + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointNormal &p) const + { + return (p.curvature); + } + + inline void + get (const pcl::PointNormal &p, float &intensity) const + { + intensity = p.curvature; + } + + inline void + set (pcl::PointNormal &p, float intensity) const + { + p.curvature = intensity; + } + + inline void + demean (pcl::PointNormal& p, float value) const + { + p.curvature -= value; + } + + inline void + add (pcl::PointNormal& p, float value) const + { + p.curvature += value; + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZ &p) const + { + return (p.z); + } + + inline void + get (const pcl::PointXYZ &p, float &intensity) const + { + intensity = p.z; + } + + inline void + set (pcl::PointXYZ &p, float intensity) const + { + p.z = intensity; + } + + inline void + demean (pcl::PointXYZ& p, float value) const + { + p.z -= value; + } + + inline void + add (pcl::PointXYZ& p, float value) const + { + p.z += value; + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZRGB &p) const + { + return (static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f); + } + + inline void + get (const pcl::PointXYZRGB &p, float& intensity) const + { + intensity = static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f; + } + + inline void + set (pcl::PointXYZRGB &p, float intensity) const + { + p.r = static_cast (intensity * 3.34448160535f); // 1000 / 299 + p.g = static_cast (intensity * 1.70357751278f); // 1000 / 587 + p.b = static_cast (intensity * 8.77192982456f); // 1000 / 114 + } + + inline void + demean (pcl::PointXYZRGB& p, float value) const + { + float intensity = this->operator () (p); + intensity -= value; + set (p, intensity); + } + + inline void + add (pcl::PointXYZRGB& p, float value) const + { + float intensity = this->operator () (p); + intensity += value; + set (p, intensity); + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZRGBA &p) const + { + return (static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f); + } + + inline void + get (const pcl::PointXYZRGBA &p, float& intensity) const + { + intensity = static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f; + } + + inline void + set (pcl::PointXYZRGBA &p, float intensity) const + { + p.r = static_cast (intensity * 3.34448160535f); // 1000 / 299 + p.g = static_cast (intensity * 1.70357751278f); // 1000 / 587 + p.b = static_cast (intensity * 8.77192982456f); // 1000 / 114 + } + + inline void + demean (pcl::PointXYZRGBA& p, float value) const + { + float intensity = this->operator () (p); + intensity -= value; + set (p, intensity); + } + + inline void + add (pcl::PointXYZRGBA& p, float value) const + { + float intensity = this->operator () (p); + intensity += value; + set (p, intensity); + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZRGBNormal &p) const + { + return (static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f); + } + + inline void + get (const pcl::PointXYZRGBNormal &p, float& intensity) const + { + intensity = static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f; + } + + inline void + set (pcl::PointXYZRGBNormal &p, float intensity) const + { + p.r = static_cast (intensity * 3.34448160535f); // 1000 / 299 + p.g = static_cast (intensity * 1.70357751278f); // 1000 / 587 + p.b = static_cast (intensity * 8.77192982456f); // 1000 / 114 + } + + inline void + demean (pcl::PointXYZRGBNormal &p, float value) const + { + float intensity = this->operator () (p); + intensity -= value; + set (p, intensity); + } + + inline void + add (pcl::PointXYZRGBNormal &p, float value) const + { + float intensity = this->operator () (p); + intensity += value; + set (p, intensity); + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZRGBL &p) const + { + return (static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f); + } + + inline void + get (const pcl::PointXYZRGBL &p, float& intensity) const + { + intensity = static_cast (299*p.r + 587*p.g + 114*p.b) * 0.001f; + } + + inline void + set (pcl::PointXYZRGBL &p, float intensity) const + { + p.r = static_cast (intensity * 3.34448160535f); // 1000 / 299 + p.g = static_cast (intensity * 1.70357751278f); // 1000 / 587 + p.b = static_cast (intensity * 8.77192982456f); // 1000 / 114 + } + + inline void + demean (pcl::PointXYZRGBL& p, float value) const + { + float intensity = this->operator () (p); + intensity -= value; + set (p, intensity); + } + + inline void + add (pcl::PointXYZRGBL& p, float value) const + { + float intensity = this->operator () (p); + intensity += value; + set (p, intensity); + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZHSV &p) const + { + return (p.v); + } + + inline void + get (const pcl::PointXYZHSV &p, float &intensity) const + { + intensity = p.v; + } + + inline void + set (pcl::PointXYZHSV &p, float intensity) const + { + p.v = intensity; + p.s = 0.0f; + } + + inline void + demean (pcl::PointXYZHSV& p, float value) const + { + p.v -= value; + } + + inline void + add (pcl::PointXYZHSV& p, float value) const + { + p.v += value; + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZL &p) const + { + return (static_cast(p.label)); + } + + inline void + get (const pcl::PointXYZL &p, float &intensity) const + { + intensity = static_cast(p.label); + } + + inline void + set (pcl::PointXYZL &p, float intensity) const + { + p.label = static_cast(intensity); + + } + + inline void + demean (pcl::PointXYZL& p, float value) const + { + p.label -= static_cast(value); + } + + inline void + add (pcl::PointXYZL& p, float value) const + { + p.label += static_cast(value); + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointXYZLNormal &p) const + { + return (static_cast(p.label)); + } + + inline void + get (const pcl::PointXYZLNormal &p, float &intensity) const + { + intensity = static_cast(p.label); + } + + inline void + set (pcl::PointXYZLNormal &p, float intensity) const + { + p.label = static_cast(intensity); + + } + + inline void + demean (pcl::PointXYZLNormal& p, float value) const + { + p.label -= static_cast(value); + } + + inline void + add (pcl::PointXYZLNormal& p, float value) const + { + p.label += static_cast(value); + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::InterestPoint &p) const + { + return (p.strength); + } + + inline void + get (const pcl::InterestPoint &p, float &intensity) const + { + intensity = p.strength; + } + + inline void + set (pcl::InterestPoint &p, float intensity) const + { + p.strength = intensity; + } + + inline void + demean (pcl::InterestPoint& p, float value) const + { + p.strength -= value; + } + + inline void + add (pcl::InterestPoint& p, float value) const + { + p.strength += value; + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointWithRange &p) const + { + return (p.range); + } + + inline void + get (const pcl::PointWithRange &p, float &intensity) const + { + intensity = p.range; + } + + inline void + set (pcl::PointWithRange &p, float intensity) const + { + p.range = intensity; + } + + inline void + demean (pcl::PointWithRange& p, float value) const + { + p.range -= value; + } + + inline void + add (pcl::PointWithRange& p, float value) const + { + p.range += value; + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointWithScale &p) const + { + return (p.scale); + } + + inline void + get (const pcl::PointWithScale &p, float &intensity) const + { + intensity = p.scale; + } + + inline void + set (pcl::PointWithScale &p, float intensity) const + { + p.scale = intensity; + } + + inline void + demean (pcl::PointWithScale& p, float value) const + { + p.scale -= value; + } + + inline void + add (pcl::PointWithScale& p, float value) const + { + p.scale += value; + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointWithViewpoint &p) const + { + return (p.z); + } + + inline void + get (const pcl::PointWithViewpoint &p, float &intensity) const + { + intensity = p.z; + } + + inline void + set (pcl::PointWithViewpoint &p, float intensity) const + { + p.z = intensity; + } + + inline void + demean (pcl::PointWithViewpoint& p, float value) const + { + p.z -= value; + } + + inline void + add (pcl::PointWithViewpoint& p, float value) const + { + p.z += value; + } + }; + + template<> + struct IntensityFieldAccessor + { + inline float + operator () (const pcl::PointSurfel &p) const + { + return (p.curvature); + } + + inline void + get (const pcl::PointSurfel &p, float &intensity) const + { + intensity = p.curvature; + } + + inline void + set (pcl::PointSurfel &p, float intensity) const + { + p.curvature = intensity; + } + + inline void + demean (pcl::PointSurfel& p, float value) const + { + p.curvature -= value; + } + + inline void + add (pcl::PointSurfel& p, float value) const + { + p.curvature += value; + } + }; + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/intersections.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/intersections.hpp new file mode 100755 index 0000000..cb73739 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/intersections.hpp @@ -0,0 +1,171 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_COMMON_INTERSECTIONS_IMPL_HPP_ +#define PCL_COMMON_INTERSECTIONS_IMPL_HPP_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// + +bool +pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, + const Eigen::VectorXf &line_b, + Eigen::Vector4f &point, double sqr_eps) +{ + Eigen::Vector4f p1, p2; + lineToLineSegment (line_a, line_b, p1, p2); + + // If the segment size is smaller than a pre-given epsilon... + double sqr_dist = (p1 - p2).squaredNorm (); + if (sqr_dist < sqr_eps) + { + point = p1; + return (true); + } + point.setZero (); + return (false); +} + +bool +pcl::lineWithLineIntersection (const pcl::ModelCoefficients &line_a, + const pcl::ModelCoefficients &line_b, + Eigen::Vector4f &point, double sqr_eps) +{ + Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a.values[0], line_a.values.size ()); + Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b.values[0], line_b.values.size ()); + return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps)); +} + +template bool +pcl::planeWithPlaneIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + Eigen::Matrix &line, + double angular_tolerance) +{ + using Vector3 = Eigen::Matrix; + using Vector4 = Eigen::Matrix; + using Vector5 = Eigen::Matrix; + using Matrix5 = Eigen::Matrix; + + // Normalize plane normals + Vector3 plane_a_norm (plane_a.template head<3> ()); + Vector3 plane_b_norm (plane_b.template head<3> ()); + plane_a_norm.normalize (); + plane_b_norm.normalize (); + + // Test if planes are parallel + double test_cos = plane_a_norm.dot (plane_b_norm); + double tolerance_cos = 1 - sin (std::abs (angular_tolerance)); + + if (std::abs (test_cos) > tolerance_cos) + { + PCL_DEBUG ("Plane A and Plane B are parallel.\n"); + return (false); + } + + Vector4 line_direction = plane_a.cross3 (plane_b); + line_direction.normalized(); + + // Construct system of equations using lagrange multipliers with one objective function and two constraints + Matrix5 langrange_coefs; + langrange_coefs << 2,0,0, plane_a[0], plane_b[0], + 0,2,0, plane_a[1], plane_b[1], + 0,0,2, plane_a[2], plane_b[2], + plane_a[0], plane_a[1], plane_a[2], 0, 0, + plane_b[0], plane_b[1], plane_b[2], 0, 0; + + Vector5 b; + b << 0, 0, 0, -plane_a[3], -plane_b[3]; + + line.resize(6); + // Solve for the lagrange multipliers + line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> (); + line.template tail<3>() = line_direction.template head<3>(); + return (true); +} + +template bool +pcl::threePlanesIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + const Eigen::Matrix &plane_c, + Eigen::Matrix &intersection_point, + double determinant_tolerance) +{ + using Vector3 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix; + + // TODO: Using Eigen::HyperPlanes is better to solve this problem + // Check if some planes are parallel + Matrix3 normals_in_lines; + + for (int i = 0; i < 3; i++) + { + normals_in_lines (i, 0) = plane_a[i]; + normals_in_lines (i, 1) = plane_b[i]; + normals_in_lines (i, 2) = plane_c[i]; + } + + Scalar determinant = normals_in_lines.determinant (); + if (std::abs (determinant) < determinant_tolerance) + { + // det ~= 0 + PCL_DEBUG ("At least two planes are parallel.\n"); + return (false); + } + + // Left part of the 3 equations + Matrix3 left_member; + + for (int i = 0; i < 3; i++) + { + left_member (0, i) = plane_a[i]; + left_member (1, i) = plane_b[i]; + left_member (2, i) = plane_c[i]; + } + + // Right side of the 3 equations + Vector3 right_member; + right_member << -plane_a[3], -plane_b[3], -plane_c[3]; + + // Solve the system + intersection_point = left_member.fullPivLu ().solve (right_member); + return (true); +} + +#endif //PCL_COMMON_INTERSECTIONS_IMPL_HPP diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/io.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/io.hpp new file mode 100755 index 0000000..9bb16c6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/io.hpp @@ -0,0 +1,464 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_IO_IMPL_IO_HPP_ +#define PCL_IO_IMPL_IO_HPP_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::getFieldIndex (const pcl::PointCloud &, + const std::string &field_name, + std::vector &fields) +{ + return getFieldIndex(field_name, fields); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::getFieldIndex (const std::string &field_name, + std::vector &fields) +{ + fields = getFields (); + const auto& ref = fields; + return pcl::getFieldIndex (field_name, ref); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::getFieldIndex (const std::string &field_name, + const std::vector &fields) +{ + const auto result = std::find_if(fields.begin (), fields.end (), + [&field_name](const auto& field) { return field.name == field_name; }); + if (result == fields.end ()) + return -1; + return std::distance(fields.begin (), result); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getFields (const pcl::PointCloud &, std::vector &fields) +{ + fields = getFields (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getFields (std::vector &fields) +{ + fields = getFields (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector +pcl::getFields () +{ + std::vector fields; + // Get the fields list + pcl::for_each_type::type>(pcl::detail::FieldAdder(fields)); + return fields; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::string +pcl::getFieldsList (const pcl::PointCloud &) +{ + // Get the fields list + const auto fields = getFields(); + std::string result; + for (std::size_t i = 0; i < fields.size () - 1; ++i) + result += fields[i].name + " "; + result += fields[fields.size () - 1].name; + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out) +{ + // Allocate enough space and copy the basics + cloud_out.header = cloud_in.header; + cloud_out.width = cloud_in.width; + cloud_out.height = cloud_in.height; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + cloud_out.points.resize (cloud_in.points.size ()); + + if (cloud_in.points.empty ()) + return; + + if (isSamePointType ()) + // Copy the whole memory block + memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT)); + else + // Iterate over each point + for (std::size_t i = 0; i < cloud_in.points.size (); ++i) + copyPoint (cloud_in.points[i], cloud_out.points[i]); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out) +{ + // Do we want to copy everything? + if (indices.size () == cloud_in.points.size ()) + { + cloud_out = cloud_in; + return; + } + + // Allocate enough space and copy the basics + cloud_out.points.resize (indices.size ()); + cloud_out.header = cloud_in.header; + cloud_out.width = static_cast(indices.size ()); + cloud_out.height = 1; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + // Iterate over each point + for (std::size_t i = 0; i < indices.size (); ++i) + cloud_out.points[i] = cloud_in.points[indices[i]]; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out) +{ + // Allocate enough space and copy the basics + cloud_out.points.resize (indices.size ()); + cloud_out.header = cloud_in.header; + cloud_out.width = std::uint32_t (indices.size ()); + cloud_out.height = 1; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + // Iterate over each point + for (std::size_t i = 0; i < indices.size (); ++i) + copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out) +{ + // Do we want to copy everything? + if (indices.indices.size () == cloud_in.points.size ()) + { + cloud_out = cloud_in; + return; + } + + // Allocate enough space and copy the basics + cloud_out.points.resize (indices.indices.size ()); + cloud_out.header = cloud_in.header; + cloud_out.width = indices.indices.size (); + cloud_out.height = 1; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + // Iterate over each point + for (std::size_t i = 0; i < indices.indices.size (); ++i) + cloud_out.points[i] = cloud_in.points[indices.indices[i]]; +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out) +{ + copyPointCloud (cloud_in, indices.indices, cloud_out); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out) +{ + int nr_p = 0; + for (const auto &index : indices) + nr_p += index.indices.size (); + + // Do we want to copy everything? Remember we assume UNIQUE indices + if (nr_p == cloud_in.points.size ()) + { + cloud_out = cloud_in; + return; + } + + // Allocate enough space and copy the basics + cloud_out.points.resize (nr_p); + cloud_out.header = cloud_in.header; + cloud_out.width = nr_p; + cloud_out.height = 1; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + // Iterate over each cluster + int cp = 0; + for (const auto &cluster_index : indices) + { + // Iterate over each idx + for (const auto &index : cluster_index.indices) + { + // Iterate over each dimension + cloud_out.points[cp] = cloud_in.points[index]; + cp++; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out) +{ + const auto nr_p = std::accumulate(indices.begin (), indices.end (), 0, + [](const auto& acc, const auto& index) { return index.indices.size() + acc; }); + + // Do we want to copy everything? Remember we assume UNIQUE indices + if (nr_p == cloud_in.points.size ()) + { + copyPointCloud (cloud_in, cloud_out); + return; + } + + // Allocate enough space and copy the basics + cloud_out.points.resize (nr_p); + cloud_out.header = cloud_in.header; + cloud_out.width = nr_p; + cloud_out.height = 1; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + // Iterate over each cluster + std::size_t cp = 0; + for (const auto &cluster_index : indices) + { + // Iterate over each idx + for (const auto &index : cluster_index.indices) + { + copyPoint (cloud_in.points[index], cloud_out.points[cp]); + ++cp; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::concatenateFields (const pcl::PointCloud &cloud1_in, + const pcl::PointCloud &cloud2_in, + pcl::PointCloud &cloud_out) +{ + using FieldList1 = typename pcl::traits::fieldList::type; + using FieldList2 = typename pcl::traits::fieldList::type; + + if (cloud1_in.points.size () != cloud2_in.points.size ()) + { + PCL_ERROR ("[pcl::concatenateFields] The number of points in the two input datasets differs!\n"); + return; + } + + // Resize the output dataset + cloud_out.points.resize (cloud1_in.points.size ()); + cloud_out.header = cloud1_in.header; + cloud_out.width = cloud1_in.width; + cloud_out.height = cloud1_in.height; + if (!cloud1_in.is_dense || !cloud2_in.is_dense) + cloud_out.is_dense = false; + else + cloud_out.is_dense = true; + + // Iterate over each point + for (std::size_t i = 0; i < cloud_out.points.size (); ++i) + { + // Iterate over each dimension + pcl::for_each_type (pcl::NdConcatenateFunctor (cloud1_in.points[i], cloud_out.points[i])); + pcl::for_each_type (pcl::NdConcatenateFunctor (cloud2_in.points[i], cloud_out.points[i])); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, + int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value) +{ + if (top < 0 || left < 0 || bottom < 0 || right < 0) + { + std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right"; + PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!"); + return; + } + + if (top == 0 && left == 0 && bottom == 0 && right == 0) + cloud_out = cloud_in; + else + { + // Allocate enough space and copy the basics + cloud_out.header = cloud_in.header; + cloud_out.width = cloud_in.width + left + right; + cloud_out.height = cloud_in.height + top + bottom; + if (cloud_out.size () != cloud_out.width * cloud_out.height) + cloud_out.resize (cloud_out.width * cloud_out.height); + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + if (border_type == pcl::BORDER_TRANSPARENT) + { + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + } + } + else + { + // Copy the data + if (border_type != pcl::BORDER_CONSTANT) + { + try + { + std::vector padding (cloud_out.width - cloud_in.width); + int right = cloud_out.width - cloud_in.width - left; + int bottom = cloud_out.height - cloud_in.height - top; + + for (int i = 0; i < left; i++) + padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type); + + for (int i = 0; i < right; i++) + padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type); + + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + + for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + + for (int j = 0; j < left; j++) + out_inner[j - left] = in[padding[j]]; + + for (int j = 0; j < right; j++) + out_inner[j + cloud_in.width] = in[padding[j + left]]; + } + + for (int i = 0; i < top; i++) + { + int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type); + memcpy (out + i*cloud_out.width, + out + (j+top) * cloud_out.width, + sizeof (PointT) * cloud_out.width); + } + + for (int i = 0; i < bottom; i++) + { + int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type); + memcpy (out + (i + cloud_in.height + top)*cloud_out.width, + out + (j+top)*cloud_out.width, + cloud_out.width * sizeof (PointT)); + } + } + catch (pcl::BadArgumentException &e) + { + PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type); + } + } + else + { + int right = cloud_out.width - cloud_in.width - left; + int bottom = cloud_out.height - cloud_in.height - top; + std::vector buff (cloud_out.width, value); + PointT* buff_ptr = &(buff[0]); + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + + for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + + memcpy (out_inner - left, buff_ptr, left * sizeof (PointT)); + memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT)); + } + + for (int i = 0; i < top; i++) + { + memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT)); + } + + for (int i = 0; i < bottom; i++) + { + memcpy (out + (i + cloud_in.height + top)*cloud_out.width, + buff_ptr, + cloud_out.width * sizeof (PointT)); + } + } + } + } +} + +#endif // PCL_IO_IMPL_IO_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/norms.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/norms.hpp new file mode 100755 index 0000000..330c147 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/norms.hpp @@ -0,0 +1,242 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR a PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_NORMS_IMPL_HPP_ +#define PCL_COMMON_NORMS_IMPL_HPP_ + +#include +#include + +namespace pcl +{ + +template inline float +selectNorm (FloatVectorT a, FloatVectorT b, int dim, NormType norm_type) +{ + // {L1, L2_SQR, L2, LINF, JM, B, SUBLINEAR, CS, DIV, PF, K, KL, HIK}; + switch (norm_type) + { + case (L1): + return L1_Norm (a, b, dim); + case (L2_SQR): + return L2_Norm_SQR (a, b, dim); + case (L2): + return L2_Norm (a, b, dim); + case (LINF): + return Linf_Norm (a, b, dim); + case (JM): + return JM_Norm (a, b, dim); + case (B): + return B_Norm (a, b, dim); + case (SUBLINEAR): + return Sublinear_Norm (a, b, dim); + case (CS): + return CS_Norm (a, b, dim); + case (DIV): + return Div_Norm (a, b, dim); + case (KL): + return KL_Norm (a, b, dim); + case (HIK): + return HIK_Norm (a, b, dim); + + case (PF): + case (K): + default: + PCL_ERROR ("[pcl::selectNorm] For PF and K norms you have to explicitly call the method, as they need additional parameters\n"); + return -1; + } +} + + +template inline float +L1_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0f; + for (int i = 0; i < dim; ++i) + norm += std::abs(a[i] - b[i]); + return norm; +} + + +template inline float +L2_Norm_SQR (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0; + for (int i = 0; i < dim; ++i) + { + float diff = a[i] - b[i]; + norm += diff*diff; + } + return norm; +} + + +template inline float +L2_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + return std::sqrt (L2_Norm_SQR(a, b, dim)); +} + + +template inline float +Linf_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0; + for (int i = 0; i < dim; ++i) + norm = (std::max)(std::abs(a[i] - b[i]), norm); + return norm; +} + + +template inline float +JM_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0; + + for (int i = 0; i < dim; ++i) + norm += (std::sqrt (a[i]) - std::sqrt (b[i])) * (std::sqrt (a[i]) - std::sqrt (b[i])); + + return std::sqrt (norm); +} + + +template inline float +B_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0, result; + + for (int i = 0; i < dim; ++i) + norm += std::sqrt (a[i] * b[i]); + + if (norm > 0) + result = -std::log (norm); + else + result = 0; + + return result; +} + + +template inline float +Sublinear_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0; + + for (int i = 0; i < dim; ++i) + norm += std::sqrt (std::abs (a[i] - b[i])); + + return norm; +} + + +template inline float +CS_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0; + + for (int i = 0; i < dim; ++i) + if ((a[i] + b[i]) != 0) + norm += (a[i] - b[i]) * (a[i] - b[i]) / (a[i] + b[i]); + else + norm += 0; + return norm; +} + + +template inline float +Div_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0; + + for (int i = 0; i < dim; ++i) + if ((a[i] / b[i]) > 0) + norm += (a[i] - b[i]) * std::log (a[i] / b[i]); + else + norm += 0; + return norm; +} + + +template inline float +PF_Norm (FloatVectorT a, FloatVectorT b, int dim, float P1, float P2) +{ + float norm = 0.0; + + for (int i = 0; i < dim; ++i) + norm += (P1 * a[i] - P2 * b[i]) * (P1 * a[i] - P2 * b[i]); + return std::sqrt (norm); +} + + +template inline float +K_Norm (FloatVectorT a, FloatVectorT b, int dim, float P1, float P2) +{ + float norm = 0.0; + + for (int i = 0; i < dim; ++i) + norm += std::abs (P1 * a[i] - P2 * b[i]); + return norm; +} + + +template inline float +KL_Norm (FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0; + + for (int i = 0; i < dim; ++i) + if ( (b[i] != 0) && ((a[i] / b[i]) > 0) ) + norm += a[i] * std::log (a[i] / b[i]); + else + norm += 0; + return norm; +} + + +template inline float +HIK_Norm(FloatVectorT a, FloatVectorT b, int dim) +{ + float norm = 0.0f; + for (int i = 0; i < dim; ++i) + norm += (std::min)(a[i], b[i]); + return norm; +} + +} +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/pca.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/pca.hpp new file mode 100755 index 0000000..89ff87e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/pca.hpp @@ -0,0 +1,243 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_PCA_IMPL_HPP +#define PCL_PCA_IMPL_HPP + +#include +#include +#include +#include +#include + +///////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PCA::initCompute () +{ + if(!Base::initCompute ()) + { + PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed"); + } + if(indices_->size () < 3) + { + PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3"); + } + + // Compute mean + mean_ = Eigen::Vector4f::Zero (); + compute3DCentroid (*input_, *indices_, mean_); + // Compute demeanished cloud + Eigen::MatrixXf cloud_demean; + demeanPointCloud (*input_, *indices_, mean_, cloud_demean); + assert (cloud_demean.cols () == int (indices_->size ())); + // Compute the product cloud_demean * cloud_demean^T + const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f)) + * cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose (); + + // Compute eigen vectors and values + Eigen::SelfAdjointEigenSolver evd (alpha); + // Organize eigenvectors and eigenvalues in ascendent order + for (int i = 0; i < 3; ++i) + { + eigenvalues_[i] = evd.eigenvalues () [2-i]; + eigenvectors_.col (i) = evd.eigenvectors ().col (2-i); + } + // Enforce right hand rule + eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1)); + // If not basis only then compute the coefficients + if (!basis_only_) + coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> (); + compute_done_ = true; + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::PCA::update (const PointT& input_point, FLAG flag) +{ + if (!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed"); + + Eigen::Vector3f input (input_point.x, input_point.y, input_point.z); + const std::size_t n = eigenvectors_.cols ();// number of eigen vectors + Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1); + Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>()); + Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>(); + Eigen::VectorXf h = y - input; + if (h.norm() > 0) + h.normalize (); + else + h.setZero (); + float gamma = h.dot(input - mean_.head<3>()); + Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1); + D.block(0,0,n,n) = a * a.transpose(); + D /= float(n)/float((n+1) * (n+1)); + for(std::size_t i=0; i < a.size(); i++) { + D(i,i)+= float(n)/float(n+1)*eigenvalues_(i); + D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i); + D(i,D.cols()-1) = D(D.rows()-1,i); + D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma; + } + + Eigen::MatrixXf R(D.rows(), D.cols()); + Eigen::EigenSolver D_evd (D, false); + Eigen::VectorXf alphap = D_evd.eigenvalues().real(); + eigenvalues_.resize(eigenvalues_.size() +1); + for(std::size_t i=0;i() = h; + eigenvectors_ = Up*R; + if (!basis_only_) { + Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp); + coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1); + for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) { + coefficients_(coefficients_.rows()-1,i) = 0; + coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha; + } + a.resize(a.size()+1); + a(a.size()-1) = 0; + coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha; + } + mean_.head<3>() = meanp; + switch (flag) + { + case increase: + if (eigenvectors_.rows() >= eigenvectors_.cols()) + break; + case preserve: + if (!basis_only_) + coefficients_ = coefficients_.topRows(coefficients_.rows() - 1); + eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1); + eigenvalues_.resize(eigenvalues_.size()-1); + break; + default: + PCL_ERROR("[pcl::PCA] unknown flag\n"); + } +} + +///////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::PCA::project (const PointT& input, PointT& projection) +{ + if(!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed"); + + Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> (); + projection.getVector3fMap () = eigenvectors_.transpose() * demean_input; +} + +///////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::PCA::project (const PointCloud& input, PointCloud& projection) +{ + if(!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed"); + if (input.is_dense) + { + projection.resize (input.size ()); + for (std::size_t i = 0; i < input.size (); ++i) + project (input[i], projection[i]); + } + else + { + PointT p; + for (const auto& pt: input) + { + if (!std::isfinite (pt.x) || + !std::isfinite (pt.y) || + !std::isfinite (pt.z)) + continue; + project (pt, p); + projection.push_back (p); + } + } +} + +///////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::PCA::reconstruct (const PointT& projection, PointT& input) +{ + if(!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed"); + + input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap (); + input.getVector3fMap ()+= mean_.head<3> (); +} + +///////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::PCA::reconstruct (const PointCloud& projection, PointCloud& input) +{ + if(!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed"); + if (input.is_dense) + { + input.resize (projection.size ()); + for (std::size_t i = 0; i < projection.size (); ++i) + reconstruct (projection[i], input[i]); + } + else + { + PointT p; + for (std::size_t i = 0; i < input.size (); ++i) + { + if (!std::isfinite (input[i].x) || + !std::isfinite (input[i].y) || + !std::isfinite (input[i].z)) + continue; + reconstruct (projection[i], p); + input.push_back (p); + } + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/piecewise_linear_function.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/piecewise_linear_function.hpp new file mode 100755 index 0000000..4557c74 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/piecewise_linear_function.hpp @@ -0,0 +1,60 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/* \author Bastian Steder */ + +#include +#include +// #include + + +namespace pcl { + +PiecewiseLinearFunction::PiecewiseLinearFunction(float factor, float offset) : factor_(factor), offset_(offset) +{ +} + +inline float PiecewiseLinearFunction::getValue(float point) const +{ + float vector_pos = factor_*point + offset_; + float floored_vector_pos = std::floor(vector_pos); + float interpolation_size = vector_pos-floored_vector_pos; + int data_point_before = (std::max)(0, (std::min)(int(data_points_.size())-2, int(lrint(floored_vector_pos)))); + //std::cout << "Interpolating between "< +inline void + pcl::PolynomialCalculationsT::Parameters::setZeroValue (real new_zero_value) +{ + zero_value = new_zero_value; + sqr_zero_value = zero_value*zero_value; +} + +//////////////////////////////////// + +template +inline void + pcl::PolynomialCalculationsT::solveLinearEquation (real a, real b, std::vector& roots) const +{ + //std::cout << "Trying to solve "< +inline void + pcl::PolynomialCalculationsT::solveQuadraticEquation (real a, real b, real c, std::vector& roots) const +{ + //std::cout << "Trying to solve "< Calling solveLineaqrEquation.\n"; + solveLinearEquation (b, c, roots); + return; + } + + if (isNearlyZero (c)) + { + roots.push_back (0.0); + //std::cout << "Constant element is 0 => Adding root 0 and calling solveLinearEquation.\n"; + std::vector tmpRoots; + solveLinearEquation (a, b, tmpRoots); + for (const auto& tmpRoot: tmpRoots) + if (!isNearlyZero (tmpRoot)) + roots.push_back (tmpRoot); + return; + } + + real tmp = b*b - 4*a*c; + if (tmp>0) + { + tmp = sqrt (tmp); + real tmp2 = 1.0/ (2*a); + roots.push_back ( (-b + tmp)*tmp2); + roots.push_back ( (-b - tmp)*tmp2); + } + else if (sqrtIsNearlyZero (tmp)) + { + roots.push_back (-b/ (2*a)); + } + +#if 0 + std::cout << __PRETTY_FUNCTION__ << ": Found "< +inline void + pcl::PolynomialCalculationsT::solveCubicEquation (real a, real b, real c, real d, std::vector& roots) const +{ + //std::cout << "Trying to solve "< Calling solveQuadraticEquation.\n"; + solveQuadraticEquation (b, c, d, roots); + return; + } + + if (isNearlyZero (d)) + { + roots.push_back (0.0); + //std::cout << "Constant element is 0 => Adding root 0 and calling solveQuadraticEquation.\n"; + std::vector tmpRoots; + solveQuadraticEquation (a, b, c, tmpRoots); + for (const auto& tmpRoot: tmpRoots) + if (!isNearlyZero (tmpRoot)) + roots.push_back (tmpRoot); + return; + } + + double a2 = a*a, + a3 = a2*a, + b2 = b*b, + b3 = b2*b, + alpha = ( (3.0*a*c-b2)/ (3.0*a2)), + beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a), + alpha2 = alpha*alpha, + alpha3 = alpha2*alpha, + beta2 = beta*beta; + + // Value for resubstitution: + double resubValue = b/ (3*a); + + //std::cout << "Trying to solve y^3 + "< 0) + { + double sqrtDiscriminant = sqrt (discriminant); + double d1 = -0.5*beta + sqrtDiscriminant, + d2 = -0.5*beta - sqrtDiscriminant; + if (d1 < 0) + d1 = -pow (-d1, 1.0/3.0); + else + d1 = pow (d1, 1.0/3.0); + + if (d2 < 0) + d2 = -pow (-d2, 1.0/3.0); + else + d2 = pow (d2, 1.0/3.0); + + //std::cout << PVAR (d1)<<", "< 1e-4) + { + std::cout << "Something went wrong:\n"; + //roots.clear (); + } + std::cout << "Root "< +inline void + pcl::PolynomialCalculationsT::solveQuarticEquation (real a, real b, real c, real d, real e, + std::vector& roots) const +{ + //std::cout << "Trying to solve "< Calling solveCubicEquation.\n"; + solveCubicEquation (b, c, d, e, roots); + return; + } + + if (isNearlyZero (e)) + { + roots.push_back (0.0); + //std::cout << "Constant element is 0 => Adding root 0 and calling solveCubicEquation.\n"; + std::vector tmpRoots; + solveCubicEquation (a, b, c, d, tmpRoots); + for (const auto& tmpRoot: tmpRoots) + if (!isNearlyZero (tmpRoot)) + roots.push_back (tmpRoot); + return; + } + + double a2 = a*a, + a3 = a2*a, + a4 = a2*a2, + b2 = b*b, + b3 = b2*b, + b4 = b2*b2, + alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a), + beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a), + gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a), + alpha2 = alpha*alpha; + + // Value for resubstitution: + double resubValue = b/ (4*a); + + //std::cout << "Trying to solve y^4 + "< tmpRoots; + solveQuadraticEquation (1.0, alpha, gamma, tmpRoots); + for (const auto& quadraticRoot: tmpRoots) + { + if (sqrtIsNearlyZero (quadraticRoot)) + { + roots.push_back (-resubValue); + } + else if (quadraticRoot > 0.0) + { + double root1 = sqrt (quadraticRoot); + roots.push_back (root1 - resubValue); + roots.push_back (-root1 - resubValue); + } + } + } + else + { + //std::cout << "beta != 0\n"; + double alpha3 = alpha2*alpha, + beta2 = beta*beta, + p = (-alpha2/12.0)-gamma, + q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0), + q2 = q*q, + p3 = p*p*p, + u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0)); + if (u > 0.0) + u = pow (u, 1.0/3.0); + else if (isNearlyZero (u)) + u = 0.0; + else + u = -pow (-u, 1.0/3.0); + + double y = (-5.0/6.0)*alpha - u; + if (!isNearlyZero (u)) + y += p/ (3.0*u); + + double w = alpha + 2.0*y; + + if (w > 0) + { + w = sqrt (w); + } + else if (isNearlyZero (w)) + { + w = 0; + } + else + { + //std::cout << "Found no roots\n"; + return; + } + + double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)), + tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w)); + + if (tmp1 > 0) + { + tmp1 = sqrt (tmp1); + double root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1); + double root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1); + roots.push_back (root1); + roots.push_back (root2); + } + else if (isNearlyZero (tmp1)) + { + double root1 = - (b/ (4.0*a)) + 0.5*w; + roots.push_back (root1); + } + + if (tmp2 > 0) + { + tmp2 = sqrt (tmp2); + double root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2); + double root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2); + roots.push_back (root3); + roots.push_back (root4); + } + else if (isNearlyZero (tmp2)) + { + double root3 = - (b/ (4.0*a)) - 0.5*w; + roots.push_back (root3); + } + + //std::cout << "Test: " << alpha<<", "< 1e-4) + { + std::cout << "Something went wrong:\n"; + //roots.clear (); + } + std::cout << "Root "< +inline pcl::BivariatePolynomialT + pcl::PolynomialCalculationsT::bivariatePolynomialApproximation ( + std::vector, Eigen::aligned_allocator > >& samplePoints, unsigned int polynomial_degree, bool& error) const +{ + pcl::BivariatePolynomialT ret; + error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret); + return ret; +} + +//////////////////////////////////// + +template +inline bool + pcl::PolynomialCalculationsT::bivariatePolynomialApproximation ( + std::vector, Eigen::aligned_allocator > >& samplePoints, unsigned int polynomial_degree, + pcl::BivariatePolynomialT& ret) const +{ + const auto parameters_size = BivariatePolynomialT::getNoOfParametersFromDegree (polynomial_degree); + + // Too many parameters for this number of equations (points)? + if (parameters_size > samplePoints.size ()) + { + return false; + // Reduce degree of polynomial + //polynomial_degree = (unsigned int) (0.5f* (std::sqrt (8*samplePoints.size ()+1) - 3)); + //parameters_size = BivariatePolynomialT::getNoOfParametersFromDegree (polynomial_degree); + //std::cout << "Not enough points, so degree of polynomial was decreased to "< "< A (parameters_size, parameters_size); + A.setZero(); + Eigen::Matrix b (parameters_size); + b.setZero(); + + { + std::vector C (parameters_size); + for (const auto& point: samplePoints) + { + real currentX = point[0], currentY = point[1], currentZ = point[2]; + + { + auto CRevPtr = C.rbegin (); + real tmpX = 1.0; + for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree) + { + real tmpY = 1.0; + for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree, ++CRevPtr) + { + *CRevPtr = tmpX*tmpY; + tmpY *= currentY; + } + tmpX *= currentX; + } + } + + for (std::size_t i=0; i::outProd (C); + //b += currentZ * C; + } + } + + // The Eigen only solution is slow for small matrices. Maybe file a bug + // A.traingularView = A.transpose(); + // copy upper-right elements to lower-left + for (std::size_t i = 0; i < parameters_size; ++i) + { + for (std::size_t j = 0; j < i; ++j) + { + A (i, j) = A (j, i); + } + } + + Eigen::Matrix parameters; + //double choleskyStartTime=-get_time (); + //parameters = A.choleskySolve (b); + //std::cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n"; + + //double invStartTime=-get_time (); + parameters = A.inverse () * b; + //std::cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n"; + + //std::cout << PVARC (A)< 1e-5) + { + //std::cout << "Inversion result: "<< inversionCheckResult<<" for matrix "< + +/////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl +{ + namespace common + { + namespace internal + { + template void + makeSymmetric (MatrixT& matrix, bool use_upper_triangular = true) + { + if (use_upper_triangular && (MatrixT::Flags & Eigen::RowMajorBit)) + { + matrix.coeffRef (4) = matrix.coeff (1); + matrix.coeffRef (8) = matrix.coeff (2); + matrix.coeffRef (9) = matrix.coeff (6); + matrix.coeffRef (12) = matrix.coeff (3); + matrix.coeffRef (13) = matrix.coeff (7); + matrix.coeffRef (14) = matrix.coeff (11); + } + else + { + matrix.coeffRef (1) = matrix.coeff (4); + matrix.coeffRef (2) = matrix.coeff (8); + matrix.coeffRef (6) = matrix.coeff (9); + matrix.coeffRef (3) = matrix.coeff (12); + matrix.coeffRef (7) = matrix.coeff (13); + matrix.coeffRef (11) = matrix.coeff (14); + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template double +pcl::estimateProjectionMatrix ( + typename pcl::PointCloud::ConstPtr cloud, + Eigen::Matrix& projection_matrix, + const std::vector& indices) +{ + // internally we calculate with double but store the result into float matrices. + using Scalar = double; + projection_matrix.setZero (); + if (cloud->height == 1 || cloud->width == 1) + { + PCL_ERROR ("[pcl::estimateProjectionMatrix] Input dataset is not organized!\n"); + return (-1.0); + } + + Eigen::Matrix A = Eigen::Matrix::Zero (); + Eigen::Matrix B = Eigen::Matrix::Zero (); + Eigen::Matrix C = Eigen::Matrix::Zero (); + Eigen::Matrix D = Eigen::Matrix::Zero (); + + pcl::ConstCloudIterator pointIt (*cloud, indices); + + while (pointIt) + { + unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width; + unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width; + + const PointT& point = *pointIt; + if (std::isfinite (point.x)) + { + Scalar xx = point.x * point.x; + Scalar xy = point.x * point.y; + Scalar xz = point.x * point.z; + Scalar yy = point.y * point.y; + Scalar yz = point.y * point.z; + Scalar zz = point.z * point.z; + Scalar xx_yy = xIdx * xIdx + yIdx * yIdx; + + A.coeffRef (0) += xx; + A.coeffRef (1) += xy; + A.coeffRef (2) += xz; + A.coeffRef (3) += point.x; + + A.coeffRef (5) += yy; + A.coeffRef (6) += yz; + A.coeffRef (7) += point.y; + + A.coeffRef (10) += zz; + A.coeffRef (11) += point.z; + A.coeffRef (15) += 1.0; + + B.coeffRef (0) -= xx * xIdx; + B.coeffRef (1) -= xy * xIdx; + B.coeffRef (2) -= xz * xIdx; + B.coeffRef (3) -= point.x * static_cast(xIdx); + + B.coeffRef (5) -= yy * xIdx; + B.coeffRef (6) -= yz * xIdx; + B.coeffRef (7) -= point.y * static_cast(xIdx); + + B.coeffRef (10) -= zz * xIdx; + B.coeffRef (11) -= point.z * static_cast(xIdx); + + B.coeffRef (15) -= xIdx; + + C.coeffRef (0) -= xx * yIdx; + C.coeffRef (1) -= xy * yIdx; + C.coeffRef (2) -= xz * yIdx; + C.coeffRef (3) -= point.x * static_cast(yIdx); + + C.coeffRef (5) -= yy * yIdx; + C.coeffRef (6) -= yz * yIdx; + C.coeffRef (7) -= point.y * static_cast(yIdx); + + C.coeffRef (10) -= zz * yIdx; + C.coeffRef (11) -= point.z * static_cast(yIdx); + + C.coeffRef (15) -= yIdx; + + D.coeffRef (0) += xx * xx_yy; + D.coeffRef (1) += xy * xx_yy; + D.coeffRef (2) += xz * xx_yy; + D.coeffRef (3) += point.x * xx_yy; + + D.coeffRef (5) += yy * xx_yy; + D.coeffRef (6) += yz * xx_yy; + D.coeffRef (7) += point.y * xx_yy; + + D.coeffRef (10) += zz * xx_yy; + D.coeffRef (11) += point.z * xx_yy; + + D.coeffRef (15) += xx_yy; + } + + ++pointIt; + } // while + + pcl::common::internal::makeSymmetric (A); + pcl::common::internal::makeSymmetric (B); + pcl::common::internal::makeSymmetric (C); + pcl::common::internal::makeSymmetric (D); + + Eigen::Matrix X = Eigen::Matrix::Zero (); + X.topLeftCorner<4,4> ().matrix () = A; + X.block<4,4> (0, 8).matrix () = B; + X.block<4,4> (8, 0).matrix () = B; + X.block<4,4> (4, 4).matrix () = A; + X.block<4,4> (4, 8).matrix () = C; + X.block<4,4> (8, 4).matrix () = C; + X.block<4,4> (8, 8).matrix () = D; + + Eigen::SelfAdjointEigenSolver > ei_symm (X); + Eigen::Matrix eigen_vectors = ei_symm.eigenvectors (); + + // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device. + Eigen::Matrix residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0); + + double residual = residual_sqr.coeff (0); + + projection_matrix.coeffRef (0) = static_cast (eigen_vectors.coeff (0)); + projection_matrix.coeffRef (1) = static_cast (eigen_vectors.coeff (12)); + projection_matrix.coeffRef (2) = static_cast (eigen_vectors.coeff (24)); + projection_matrix.coeffRef (3) = static_cast (eigen_vectors.coeff (36)); + projection_matrix.coeffRef (4) = static_cast (eigen_vectors.coeff (48)); + projection_matrix.coeffRef (5) = static_cast (eigen_vectors.coeff (60)); + projection_matrix.coeffRef (6) = static_cast (eigen_vectors.coeff (72)); + projection_matrix.coeffRef (7) = static_cast (eigen_vectors.coeff (84)); + projection_matrix.coeffRef (8) = static_cast (eigen_vectors.coeff (96)); + projection_matrix.coeffRef (9) = static_cast (eigen_vectors.coeff (108)); + projection_matrix.coeffRef (10) = static_cast (eigen_vectors.coeff (120)); + projection_matrix.coeffRef (11) = static_cast (eigen_vectors.coeff (132)); + + if (projection_matrix.coeff (0) < 0) + projection_matrix *= -1.0; + + return (residual); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/random.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/random.hpp new file mode 100755 index 0000000..3babfc2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/random.hpp @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_COMMON_RANDOM_HPP_ +#define PCL_COMMON_RANDOM_HPP_ + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::UniformGenerator::UniformGenerator(T min, T max, std::uint32_t seed) + : distribution_ (min, max) +{ + parameters_ = Parameters (min, max, seed); + if(parameters_.seed != static_cast (-1)) + rng_.seed (seed); +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::UniformGenerator::UniformGenerator(const Parameters& parameters) + : parameters_ (parameters) + , distribution_ (parameters_.min, parameters_.max) +{ + if(parameters_.seed != static_cast (-1)) + rng_.seed (parameters_.seed); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::UniformGenerator::setSeed (std::uint32_t seed) +{ + if (seed != static_cast (-1)) + { + parameters_.seed = seed; + rng_.seed(parameters_.seed); + } +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::UniformGenerator::setParameters (T min, T max, std::uint32_t seed) +{ + parameters_.min = min; + parameters_.max = max; + parameters_.seed = seed; + typename DistributionType::param_type params (parameters_.min, parameters_.max); + distribution_.param (params); + distribution_.reset (); + if (seed != static_cast (-1)) + { + parameters_.seed = seed; + rng_.seed (parameters_.seed); + } +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::UniformGenerator::setParameters (const Parameters& parameters) +{ + parameters_ = parameters; + typename DistributionType::param_type params (parameters_.min, parameters_.max); + distribution_.param (params); + distribution_.reset (); + if (parameters_.seed != static_cast (-1)) + rng_.seed (parameters_.seed); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::NormalGenerator::NormalGenerator(T mean, T sigma, std::uint32_t seed) + : distribution_ (mean, sigma) +{ + parameters_ = Parameters (mean, sigma, seed); + if(parameters_.seed != static_cast (-1)) + rng_.seed (seed); +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::common::NormalGenerator::NormalGenerator(const Parameters& parameters) + : parameters_ (parameters) + , distribution_ (parameters_.mean, parameters_.sigma) +{ + if(parameters_.seed != static_cast (-1)) + rng_.seed (parameters_.seed); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::NormalGenerator::setSeed (std::uint32_t seed) +{ + if (seed != static_cast (-1)) + { + parameters_.seed = seed; + rng_.seed(seed); + } +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::NormalGenerator::setParameters (T mean, T sigma, std::uint32_t seed) +{ + parameters_.mean = mean; + parameters_.sigma = sigma; + parameters_.seed = seed; + typename DistributionType::param_type params (parameters_.mean, parameters_.sigma); + distribution_.param (params); + distribution_.reset (); + if (seed != static_cast (-1)) + rng_.seed (parameters_.seed); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::common::NormalGenerator::setParameters (const Parameters& parameters) +{ + parameters_ = parameters; + typename DistributionType::param_type params (parameters_.mean, parameters_.sigma); + distribution_.param (params); + distribution_.reset (); + if (parameters_.seed != static_cast (-1)) + rng_.seed (parameters_.seed); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/spring.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/spring.hpp new file mode 100755 index 0000000..c8a549f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/spring.hpp @@ -0,0 +1,258 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_POINT_CLOUD_SPRING_IMPL_HPP_ +#define PCL_POINT_CLOUD_SPRING_IMPL_HPP_ + +template void +pcl::common::expandColumns (const PointCloud& input, PointCloud& output, + const PointT& val, const std::size_t& amount) +{ + if (amount <= 0) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::expandColumns] error: amount must be ]0.." + << (input.width/2) << "] !"); + + if (!input.isOrganized () || amount > (input.width/2)) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::expandColumns] error: " + << "columns expansion requires organised point cloud"); + + std::uint32_t old_height = input.height; + std::uint32_t old_width = input.width; + std::uint32_t new_width = old_width + 2*amount; + if (&input != &output) + output = input; + output.reserve (new_width * old_height); + for (int j = 0; j < output.height; ++j) + { + typename PointCloud::iterator start = output.begin() + (j * new_width); + output.insert (start, amount, val); + start = output.begin() + (j * new_width) + old_width + amount; + output.insert (start, amount, val); + output.height = old_height; + } + output.width = new_width; + output.height = old_height; +} + +template void +pcl::common::expandRows (const PointCloud& input, PointCloud& output, + const PointT& val, const std::size_t& amount) +{ + if (amount <= 0) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::expandRows] error: amount must be ]0.." + << (input.height/2) << "] !"); + + std::uint32_t old_height = input.height; + std::uint32_t new_height = old_height + 2*amount; + std::uint32_t old_width = input.width; + if (&input != &output) + output = input; + output.reserve (new_height * old_width); + output.insert (output.begin (), amount * old_width, val); + output.insert (output.end (), amount * old_width, val); + output.width = old_width; + output.height = new_height; +} + +template void +pcl::common::duplicateColumns (const PointCloud& input, PointCloud& output, + const std::size_t& amount) +{ + if (amount <= 0) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::duplicateColumns] error: amount must be ]0.." + << (input.width/2) << "] !"); + + if (!input.isOrganized () || amount > (input.width/2)) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::duplicateColumns] error: " + << "columns expansion requires organised point cloud"); + + std::size_t old_height = input.height; + std::size_t old_width = input.width; + std::size_t new_width = old_width + 2*amount; + if (&input != &output) + output = input; + output.reserve (new_width * old_height); + for (std::size_t j = 0; j < old_height; ++j) + for(std::size_t i = 0; i < amount; ++i) + { + typename PointCloud::iterator start = output.begin () + (j * new_width); + output.insert (start, *start); + start = output.begin () + (j * new_width) + old_width + i; + output.insert (start, *start); + } + + output.width = new_width; + output.height = old_height; +} + +template void +pcl::common::duplicateRows (const PointCloud& input, PointCloud& output, + const std::size_t& amount) +{ + if (amount <= 0 || amount > (input.height/2)) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::duplicateRows] error: amount must be ]0.." + << (input.height/2) << "] !"); + + std::uint32_t old_height = input.height; + std::uint32_t new_height = old_height + 2*amount; + std::uint32_t old_width = input.width; + if (&input != &output) + output = input; + output.reserve (new_height * old_width); + for(std::size_t i = 0; i < amount; ++i) + { + output.insert (output.begin (), output.begin (), output.begin () + old_width); + output.insert (output.end (), output.end () - old_width, output.end ()); + } + + output.width = old_width; + output.height = new_height; +} + +template void +pcl::common::mirrorColumns (const PointCloud& input, PointCloud& output, + const std::size_t& amount) +{ + if (amount <= 0) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::mirrorColumns] error: amount must be ]0.." + << (input.width/2) << "] !"); + + if (!input.isOrganized () || amount > (input.width/2)) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::mirrorColumns] error: " + << "columns expansion requires organised point cloud"); + + std::size_t old_height = input.height; + std::size_t old_width = input.width; + std::size_t new_width = old_width + 2*amount; + if (&input != &output) + output = input; + output.reserve (new_width * old_height); + for (std::size_t j = 0; j < old_height; ++j) + for(std::size_t i = 0; i < amount; ++i) + { + typename PointCloud::iterator start = output.begin () + (j * new_width); + output.insert (start, *(start + 2*i)); + start = output.begin () + (j * new_width) + old_width + 2*i; + output.insert (start+1, *(start - 2*i)); + } + output.width = new_width; + output.height = old_height; +} + +template void +pcl::common::mirrorRows (const PointCloud& input, PointCloud& output, + const std::size_t& amount) +{ + if (amount <= 0 || amount > (input.height/2)) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::mirrorRows] error: amount must be ]0.." + << (input.height/2) << "] !"); + + std::uint32_t old_height = input.height; + std::uint32_t new_height = old_height + 2*amount; + std::uint32_t old_width = input.width; + if (&input != &output) + output = input; + output.reserve (new_height * old_width); + for(std::size_t i = 0; i < amount; i++) + { + const auto extra_odd = output.height % 2; + auto up = output.begin () + (2*i + extra_odd) * old_width; + output.insert (output.begin (), up, up + old_width); + auto bottom = output.end () - (2*i+1) * old_width; + output.insert (output.end (), bottom, bottom + old_width); + } + output.width = old_width; + output.height = new_height; +} + +template void +pcl::common::deleteRows (const PointCloud& input, PointCloud& output, + const std::size_t& amount) +{ + if (amount <= 0 || amount > (input.height/2)) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::deleteRows] error: amount must be ]0.." + << (input.height/2) << "] !"); + + std::uint32_t old_height = input.height; + std::uint32_t old_width = input.width; + output.erase (output.begin (), output.begin () + amount * old_width); + output.erase (output.end () - amount * old_width, output.end ()); + output.height = old_height - 2*amount; + output.width = old_width; +} + +template void +pcl::common::deleteCols (const PointCloud& input, PointCloud& output, + const std::size_t& amount) +{ + if (amount <= 0 || amount > (input.width/2)) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::deleteCols] error: amount must be in ]0.." + << (input.width/2) << "] !"); + + if (!input.isOrganized ()) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::common::deleteCols] error: " + << "columns delete requires organised point cloud"); + + std::uint32_t old_height = input.height; + std::uint32_t old_width = input.width; + std::uint32_t new_width = old_width - 2 * amount; + for(std::size_t j = 0; j < old_height; j++) + { + typename PointCloud::iterator start = output.begin () + j * new_width; + output.erase (start, start + amount); + start = output.begin () + (j+1) * new_width; + output.erase (start, start + amount); + } + output.height = old_height; + output.width = new_width; +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/transformation_from_correspondences.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/transformation_from_correspondences.hpp new file mode 100755 index 0000000..4067a5b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/transformation_from_correspondences.hpp @@ -0,0 +1,93 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +inline void +pcl::TransformationFromCorrespondences::reset () +{ + no_of_samples_ = 0; + accumulated_weight_ = 0.0; + mean1_.fill(0); + mean2_.fill(0); + covariance_.fill(0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +inline void +pcl::TransformationFromCorrespondences::add (const Eigen::Vector3f& point, const Eigen::Vector3f& corresponding_point, + float weight) +{ + if (weight==0.0f) + return; + + ++no_of_samples_; + accumulated_weight_ += weight; + float alpha = weight/accumulated_weight_; + + Eigen::Vector3f diff1 = point - mean1_, diff2 = corresponding_point - mean2_; + covariance_ = (1.0f-alpha)*(covariance_ + alpha * (diff2 * diff1.transpose())); + + mean1_ += alpha*(diff1); + mean2_ += alpha*(diff2); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +inline Eigen::Affine3f +pcl::TransformationFromCorrespondences::getTransformation () +{ + //Eigen::JacobiSVD > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::JacobiSVD > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV); + const Eigen::Matrix& u = svd.matrixU(), + & v = svd.matrixV(); + Eigen::Matrix s; + s.setIdentity(); + if (u.determinant()*v.determinant() < 0.0f) + s(2,2) = -1.0f; + + Eigen::Matrix r = u * s * v.transpose(); + Eigen::Vector3f t = mean2_ - r*mean1_; + + Eigen::Affine3f ret; + ret(0,0)=r(0,0); ret(0,1)=r(0,1); ret(0,2)=r(0,2); ret(0,3)=t(0); + ret(1,0)=r(1,0); ret(1,1)=r(1,1); ret(1,2)=r(1,2); ret(1,3)=t(1); + ret(2,0)=r(2,0); ret(2,1)=r(2,1); ret(2,2)=r(2,2); ret(2,3)=t(2); + ret(3,0)=0.0f; ret(3,1)=0.0f; ret(3,2)=0.0f; ret(3,3)=1.0f; + + return (ret); +} diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/transforms.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/transforms.hpp new file mode 100755 index 0000000..eff1bdd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/transforms.hpp @@ -0,0 +1,481 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#if defined(__SSE2__) +#include +#endif + +#if defined(__AVX__) +#include +#endif + +#include +#include +#include +#include + +namespace pcl +{ + + namespace detail + { + + /** A helper struct to apply an SO3 or SE3 transform to a 3D point. + * Supports single and double precision transform matrices. */ + template + struct Transformer + { + const Eigen::Matrix& tf; + + /** Construct a transformer object. + * The transform matrix is captured by const reference. Make sure that it does not go out of scope before this + * object does. */ + Transformer (const Eigen::Matrix& transform) : tf (transform) { }; + + /** Apply SO3 transform (top-left corner of the transform matrix). + * \param[in] src input 3D point (pointer to 3 floats) + * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 0. */ + void so3 (const float* src, float* tgt) const + { + const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt + tgt[0] = static_cast (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2]); + tgt[1] = static_cast (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2]); + tgt[2] = static_cast (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2]); + tgt[3] = 0; + } + + /** Apply SE3 transform. + * \param[in] src input 3D point (pointer to 3 floats) + * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 1. */ + void se3 (const float* src, float* tgt) const + { + const Scalar p[3] = { src[0], src[1], src[2] }; // need this when src == tgt + tgt[0] = static_cast (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2] + tf (0, 3)); + tgt[1] = static_cast (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2] + tf (1, 3)); + tgt[2] = static_cast (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2] + tf (2, 3)); + tgt[3] = 1; + } + }; + +#if defined(__SSE2__) + + /** Optimized version for single-precision transforms using SSE2 intrinsics. */ + template<> + struct Transformer + { + /// Columns of the transform matrix stored in XMM registers. + __m128 c[4]; + + Transformer(const Eigen::Matrix4f& tf) + { + for (std::size_t i = 0; i < 4; ++i) + c[i] = _mm_load_ps (tf.col (i).data ()); + } + + void so3 (const float* src, float* tgt) const + { + __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]); + __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]); + __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]); + _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, p2))); + } + + void se3 (const float* src, float* tgt) const + { + __m128 p0 = _mm_mul_ps (_mm_load_ps1 (&src[0]), c[0]); + __m128 p1 = _mm_mul_ps (_mm_load_ps1 (&src[1]), c[1]); + __m128 p2 = _mm_mul_ps (_mm_load_ps1 (&src[2]), c[2]); + _mm_store_ps (tgt, _mm_add_ps(p0, _mm_add_ps(p1, _mm_add_ps(p2, c[3])))); + } + }; + +#if !defined(__AVX__) + + /** Optimized version for double-precision transform using SSE2 intrinsics. */ + template<> + struct Transformer + { + /// Columns of the transform matrix stored in XMM registers. + __m128d c[4][2]; + + Transformer(const Eigen::Matrix4d& tf) + { + for (std::size_t i = 0; i < 4; ++i) + { + c[i][0] = _mm_load_pd (tf.col (i).data () + 0); + c[i][1] = _mm_load_pd (tf.col (i).data () + 2); + } + } + + void so3 (const float* src, float* tgt) const + { + __m128d xx = _mm_cvtps_pd (_mm_load_ps1 (&src[0])); + __m128d p0 = _mm_mul_pd (xx, c[0][0]); + __m128d p1 = _mm_mul_pd (xx, c[0][1]); + + for (std::size_t i = 1; i < 3; ++i) + { + __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i])); + p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0); + p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1); + } + + _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1))); + } + + void se3 (const float* src, float* tgt) const + { + __m128d p0 = c[3][0]; + __m128d p1 = c[3][1]; + + for (std::size_t i = 0; i < 3; ++i) + { + __m128d vv = _mm_cvtps_pd (_mm_load_ps1 (&src[i])); + p0 = _mm_add_pd (_mm_mul_pd (vv, c[i][0]), p0); + p1 = _mm_add_pd (_mm_mul_pd (vv, c[i][1]), p1); + } + + _mm_store_ps (tgt, _mm_movelh_ps (_mm_cvtpd_ps (p0), _mm_cvtpd_ps (p1))); + } + + }; + +#else + + /** Optimized version for double-precision transform using AVX intrinsics. */ + template<> + struct Transformer + { + __m256d c[4]; + + Transformer(const Eigen::Matrix4d& tf) + { + for (std::size_t i = 0; i < 4; ++i) + c[i] = _mm256_load_pd (tf.col (i).data ()); + } + + void so3 (const float* src, float* tgt) const + { + __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]); + __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]); + __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]); + _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, p2)))); + } + + void se3 (const float* src, float* tgt) const + { + __m256d p0 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[0])), c[0]); + __m256d p1 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[1])), c[1]); + __m256d p2 = _mm256_mul_pd (_mm256_cvtps_pd (_mm_load_ps1 (&src[2])), c[2]); + _mm_store_ps (tgt, _mm256_cvtpd_ps (_mm256_add_pd(p0, _mm256_add_pd(p1, _mm256_add_pd(p2, c[3]))))); + } + + }; + +#endif +#endif + + } + +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields) +{ + if (&cloud_in != &cloud_out) + { + cloud_out.header = cloud_in.header; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.width = cloud_in.width; + cloud_out.height = cloud_in.height; + cloud_out.points.reserve (cloud_in.points.size ()); + if (copy_all_fields) + cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); + else + cloud_out.points.resize (cloud_in.points.size ()); + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + } + + pcl::detail::Transformer tf (transform.matrix ()); + if (cloud_in.is_dense) + { + // If the dataset is dense, simply transform it! + for (std::size_t i = 0; i < cloud_out.points.size (); ++i) + tf.se3 (cloud_in[i].data, cloud_out[i].data); + } + else + { + // Dataset might contain NaNs and Infs, so check for them first, + // otherwise we get errors during the multiplication (?) + for (std::size_t i = 0; i < cloud_out.points.size (); ++i) + { + if (!std::isfinite (cloud_in.points[i].x) || + !std::isfinite (cloud_in.points[i].y) || + !std::isfinite (cloud_in.points[i].z)) + continue; + tf.se3 (cloud_in[i].data, cloud_out[i].data); + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields) +{ + std::size_t npts = indices.size (); + // In order to transform the data, we need to remove NaNs + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.header = cloud_in.header; + cloud_out.width = static_cast (npts); + cloud_out.height = 1; + cloud_out.points.resize (npts); + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + pcl::detail::Transformer tf (transform.matrix ()); + if (cloud_in.is_dense) + { + // If the dataset is dense, simply transform it! + for (std::size_t i = 0; i < npts; ++i) + { + // Copy fields first, then transform xyz data + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; + tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data); + } + } + else + { + // Dataset might contain NaNs and Infs, so check for them first, + // otherwise we get errors during the multiplication (?) + for (std::size_t i = 0; i < npts; ++i) + { + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; + if (!std::isfinite (cloud_in.points[indices[i]].x) || + !std::isfinite (cloud_in.points[indices[i]].y) || + !std::isfinite (cloud_in.points[indices[i]].z)) + continue; + tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data); + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields) +{ + if (&cloud_in != &cloud_out) + { + // Note: could be replaced by cloud_out = cloud_in + cloud_out.header = cloud_in.header; + cloud_out.width = cloud_in.width; + cloud_out.height = cloud_in.height; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.points.reserve (cloud_out.points.size ()); + if (copy_all_fields) + cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); + else + cloud_out.points.resize (cloud_in.points.size ()); + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + } + + pcl::detail::Transformer tf (transform.matrix ()); + // If the data is dense, we don't need to check for NaN + if (cloud_in.is_dense) + { + for (std::size_t i = 0; i < cloud_out.points.size (); ++i) + { + tf.se3 (cloud_in[i].data, cloud_out[i].data); + tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n); + } + } + // Dataset might contain NaNs and Infs, so check for them first. + else + { + for (std::size_t i = 0; i < cloud_out.points.size (); ++i) + { + if (!std::isfinite (cloud_in.points[i].x) || + !std::isfinite (cloud_in.points[i].y) || + !std::isfinite (cloud_in.points[i].z)) + continue; + tf.se3 (cloud_in[i].data, cloud_out[i].data); + tf.so3 (cloud_in[i].data_n, cloud_out[i].data_n); + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields) +{ + std::size_t npts = indices.size (); + // In order to transform the data, we need to remove NaNs + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.header = cloud_in.header; + cloud_out.width = static_cast (npts); + cloud_out.height = 1; + cloud_out.points.resize (npts); + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + pcl::detail::Transformer tf (transform.matrix ()); + // If the data is dense, we don't need to check for NaN + if (cloud_in.is_dense) + { + for (std::size_t i = 0; i < cloud_out.points.size (); ++i) + { + // Copy fields first, then transform + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; + tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data); + tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n); + } + } + // Dataset might contain NaNs and Infs, so check for them first. + else + { + for (std::size_t i = 0; i < cloud_out.points.size (); ++i) + { + // Copy fields first, then transform + if (copy_all_fields) + cloud_out.points[i] = cloud_in.points[indices[i]]; + + if (!std::isfinite (cloud_in.points[indices[i]].x) || + !std::isfinite (cloud_in.points[indices[i]].y) || + !std::isfinite (cloud_in.points[indices[i]].z)) + continue; + + tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data); + tf.so3 (cloud_in[indices[i]].data_n, cloud_out[i].data_n); + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &offset, + const Eigen::Quaternion &rotation, + bool copy_all_fields) +{ + Eigen::Translation translation (offset); + // Assemble an Eigen Transform + Eigen::Transform t (translation * rotation); + transformPointCloud (cloud_in, cloud_out, t, copy_all_fields); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &offset, + const Eigen::Quaternion &rotation, + bool copy_all_fields) +{ + Eigen::Translation translation (offset); + // Assemble an Eigen Transform + Eigen::Transform t (translation * rotation); + transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline PointT +pcl::transformPoint (const PointT &point, + const Eigen::Transform &transform) +{ + PointT ret = point; + pcl::detail::Transformer tf (transform.matrix ()); + tf.se3 (point.data, ret.data); + return (ret); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline PointT +pcl::transformPointWithNormal (const PointT &point, + const Eigen::Transform &transform) +{ + PointT ret = point; + pcl::detail::Transformer tf (transform.matrix ()); + tf.se3 (point.data, ret.data); + tf.so3 (point.data_n, ret.data_n); + return (ret); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::getPrincipalTransformation (const pcl::PointCloud &cloud, + Eigen::Transform &transform) +{ + EIGEN_ALIGN16 Eigen::Matrix covariance_matrix; + Eigen::Matrix centroid; + + pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid); + + EIGEN_ALIGN16 Eigen::Matrix eigen_vects; + Eigen::Matrix eigen_vals; + pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals); + + double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1); + double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2); + + transform.translation () = centroid.head (3); + transform.linear () = eigen_vects; + + return (std::min (rel1, rel2)); +} + diff --git a/deps_install/include/pcl-1.10/pcl/common/impl/vector_average.hpp b/deps_install/include/pcl-1.10/pcl/common/impl/vector_average.hpp new file mode 100755 index 0000000..fa8d931 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/impl/vector_average.hpp @@ -0,0 +1,201 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_COMMON_VECTOR_AVERAGE_IMPL_HPP_ +#define PCL_COMMON_VECTOR_AVERAGE_IMPL_HPP_ + +namespace pcl +{ + template + VectorAverage::VectorAverage () + { + reset(); + } + + template + inline void VectorAverage::reset() + { + noOfSamples_ = 0; + accumulatedWeight_ = 0.0; + mean_.fill(0); + covariance_.fill(0); + } + + template + inline void VectorAverage::add(const Eigen::Matrix& sample, real weight) { + if (weight == 0.0f) + return; + + ++noOfSamples_; + accumulatedWeight_ += weight; + real alpha = weight/accumulatedWeight_; + + Eigen::Matrix diff = sample - mean_; + covariance_ = (covariance_ + (diff * diff.transpose())*alpha)*(1.0f-alpha); + + mean_ += (diff)*alpha; + + //if (std::isnan(covariance_(0,0))) + //{ + //std::cout << PVARN(weight); + //exit(0); + //} + } + + template + inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values, Eigen::Matrix& eigen_vector1, + Eigen::Matrix& eigen_vector2, Eigen::Matrix& eigen_vector3) const + { + // The following step is necessary for cases where the values in the covariance matrix are small + // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors. + //Eigen::Matrix tmp_covariance = covariance_.template cast(); + //Eigen::SelfAdjointEigenSolver > ei_symm(tmp_covariance); + //eigen_values = ei_symm.eigenvalues().template cast(); + //Eigen::Matrix eigen_vectors = ei_symm.eigenvectors().template cast(); + + //std::cout << "My covariance is \n"< > ei_symm(covariance_); + eigen_values = ei_symm.eigenvalues(); + Eigen::Matrix eigen_vectors = ei_symm.eigenvectors(); + + eigen_vector1 = eigen_vectors.col(0); + eigen_vector2 = eigen_vectors.col(1); + eigen_vector3 = eigen_vectors.col(2); + } + + template + inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values) const + { + // The following step is necessary for cases where the values in the covariance matrix are small + // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors. + //Eigen::Matrix tmp_covariance = covariance_.template cast(); + //Eigen::SelfAdjointEigenSolver > ei_symm(tmp_covariance, false); + //eigen_values = ei_symm.eigenvalues().template cast(); + + Eigen::SelfAdjointEigenSolver > ei_symm(covariance_, false); + eigen_values = ei_symm.eigenvalues(); + } + + template + inline void VectorAverage::getEigenVector1(Eigen::Matrix& eigen_vector1) const + { + // The following step is necessary for cases where the values in the covariance matrix are small + // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors. + //Eigen::Matrix tmp_covariance = covariance_.template cast(); + //Eigen::SelfAdjointEigenSolver > ei_symm(tmp_covariance); + //eigen_values = ei_symm.eigenvalues().template cast(); + //Eigen::Matrix eigen_vectors = ei_symm.eigenvectors().template cast(); + + //std::cout << "My covariance is \n"< > ei_symm(covariance_); + Eigen::Matrix eigen_vectors = ei_symm.eigenvectors(); + eigen_vector1 = eigen_vectors.col(0); + } + + + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Special cases for real=float & dimension=3 -> Partial specialization does not work with class templates. :( // + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////// + // float // + /////////// + template <> + inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values, Eigen::Matrix& eigen_vector1, + Eigen::Matrix& eigen_vector2, Eigen::Matrix& eigen_vector3) const + { + //std::cout << "Using specialized 3x3 version of doPCA!\n"; + Eigen::Matrix eigen_vectors; + eigen33(covariance_, eigen_vectors, eigen_values); + eigen_vector1 = eigen_vectors.col(0); + eigen_vector2 = eigen_vectors.col(1); + eigen_vector3 = eigen_vectors.col(2); + } + template <> + inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values) const + { + //std::cout << "Using specialized 3x3 version of doPCA!\n"; + computeRoots (covariance_, eigen_values); + } + template <> + inline void VectorAverage::getEigenVector1(Eigen::Matrix& eigen_vector1) const + { + //std::cout << "Using specialized 3x3 version of doPCA!\n"; + Eigen::Vector3f::Scalar eigen_value; + Eigen::Vector3f eigen_vector; + eigen33(covariance_, eigen_value, eigen_vector); + eigen_vector1 = eigen_vector; + } + + //////////// + // double // + //////////// + template <> + inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values, Eigen::Matrix& eigen_vector1, + Eigen::Matrix& eigen_vector2, Eigen::Matrix& eigen_vector3) const + { + //std::cout << "Using specialized 3x3 version of doPCA!\n"; + Eigen::Matrix eigen_vectors; + eigen33(covariance_, eigen_vectors, eigen_values); + eigen_vector1 = eigen_vectors.col(0); + eigen_vector2 = eigen_vectors.col(1); + eigen_vector3 = eigen_vectors.col(2); + } + template <> + inline void VectorAverage::doPCA(Eigen::Matrix& eigen_values) const + { + //std::cout << "Using specialized 3x3 version of doPCA!\n"; + computeRoots (covariance_, eigen_values); + } + template <> + inline void VectorAverage::getEigenVector1(Eigen::Matrix& eigen_vector1) const + { + //std::cout << "Using specialized 3x3 version of doPCA!\n"; + Eigen::Vector3d::Scalar eigen_value; + Eigen::Vector3d eigen_vector; + eigen33(covariance_, eigen_value, eigen_vector); + eigen_vector1 = eigen_vector; + } +} // END namespace + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/common/intensity.h b/deps_install/include/pcl-1.10/pcl/common/intensity.h new file mode 100755 index 0000000..cd38fc4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/intensity.h @@ -0,0 +1,101 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +namespace pcl +{ + namespace common + { + /** \brief Intensity field accessor provides access to the intensity filed of a PoinT + * implementation for specific types should be done in \file pcl/common/impl/intensity.hpp + */ + template + struct IntensityFieldAccessor + { + /** \brief get intensity field + * \param[in] p point + * \return p.intensity + */ + inline float + operator () (const PointT &p) const + { + return p.intensity; + } + /** \brief gets the intensity value of a point + * \param p point for which intensity to be get + * \param[in] intensity value of the intensity field + */ + inline void + get (const PointT &p, float &intensity) const + { + intensity = p.intensity; + } + /** \brief sets the intensity value of a point + * \param p point for which intensity to be set + * \param[in] intensity value of the intensity field + */ + inline void + set (PointT &p, float intensity) const + { + p.intensity = intensity; + } + /** \brief subtract value from intensity field + * \param p point for which to modify intensity + * \param[in] value value to be subtracted from point intensity + */ + inline void + demean (PointT& p, float value) const + { + p.intensity -= value; + } + /** \brief add value to intensity field + * \param p point for which to modify intensity + * \param[in] value value to be added to point intensity + */ + inline void + add (PointT& p, float value) const + { + p.intensity += value; + } + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/intersections.h b/deps_install/include/pcl-1.10/pcl/common/intersections.h new file mode 100755 index 0000000..28a3f80 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/intersections.h @@ -0,0 +1,157 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +/** + * \file pcl/common/intersections.h + * Define line with line intersection functions + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + /** \brief Get the intersection of a two 3D lines in space as a 3D point + * \param[in] line_a the coefficients of the first line (point, direction) + * \param[in] line_b the coefficients of the second line (point, direction) + * \param[out] point holder for the computed 3D point + * \param[in] sqr_eps maximum allowable squared distance to the true solution + * \ingroup common + */ + PCL_EXPORTS inline bool + lineWithLineIntersection (const Eigen::VectorXf &line_a, + const Eigen::VectorXf &line_b, + Eigen::Vector4f &point, + double sqr_eps = 1e-4); + + /** \brief Get the intersection of a two 3D lines in space as a 3D point + * \param[in] line_a the coefficients of the first line (point, direction) + * \param[in] line_b the coefficients of the second line (point, direction) + * \param[out] point holder for the computed 3D point + * \param[in] sqr_eps maximum allowable squared distance to the true solution + * \ingroup common + */ + + PCL_EXPORTS inline bool + lineWithLineIntersection (const pcl::ModelCoefficients &line_a, + const pcl::ModelCoefficients &line_b, + Eigen::Vector4f &point, + double sqr_eps = 1e-4); + + /** \brief Determine the line of intersection of two non-parallel planes using lagrange multipliers + * \note Described in: "Intersection of Two Planes, John Krumm, Microsoft Research, Redmond, WA, USA" + * \param[in] plane_a coefficients of plane A and plane B in the form ax + by + cz + d = 0 + * \param[in] plane_b coefficients of line where line.tail<3>() = direction vector and + * line.head<3>() the point on the line clossest to (0, 0, 0) + * \param[out] line the intersected line to be filled + * \param[in] angular_tolerance tolerance in radians + * \return true if succeeded/planes aren't parallel + */ + PCL_EXPORTS template bool + planeWithPlaneIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + Eigen::Matrix &line, + double angular_tolerance = 0.1); + + PCL_EXPORTS inline bool + planeWithPlaneIntersection (const Eigen::Vector4f &plane_a, + const Eigen::Vector4f &plane_b, + Eigen::VectorXf &line, + double angular_tolerance = 0.1) + { + return (planeWithPlaneIntersection (plane_a, plane_b, line, angular_tolerance)); + } + + PCL_EXPORTS inline bool + planeWithPlaneIntersection (const Eigen::Vector4d &plane_a, + const Eigen::Vector4d &plane_b, + Eigen::VectorXd &line, + double angular_tolerance = 0.1) + { + return (planeWithPlaneIntersection (plane_a, plane_b, line, angular_tolerance)); + } + + /** \brief Determine the point of intersection of three non-parallel planes by solving the equations. + * \note If using nearly parallel planes you can lower the determinant_tolerance value. This can + * lead to inconsistent results. + * If the three planes intersects in a line the point will be anywhere on the line. + * \param[in] plane_a are the coefficients of the first plane in the form ax + by + cz + d = 0 + * \param[in] plane_b are the coefficients of the second plane + * \param[in] plane_c are the coefficients of the third plane + * \param[in] determinant_tolerance is a limit to determine whether planes are parallel or not + * \param[out] intersection_point the three coordinates x, y, z of the intersection point + * \return true if succeeded/planes aren't parallel + */ + PCL_EXPORTS template bool + threePlanesIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + const Eigen::Matrix &plane_c, + Eigen::Matrix &intersection_point, + double determinant_tolerance = 1e-6); + + + PCL_EXPORTS inline bool + threePlanesIntersection (const Eigen::Vector4f &plane_a, + const Eigen::Vector4f &plane_b, + const Eigen::Vector4f &plane_c, + Eigen::Vector3f &intersection_point, + double determinant_tolerance = 1e-6) + { + return (threePlanesIntersection (plane_a, plane_b, plane_c, + intersection_point, determinant_tolerance)); + } + + PCL_EXPORTS inline bool + threePlanesIntersection (const Eigen::Vector4d &plane_a, + const Eigen::Vector4d &plane_b, + const Eigen::Vector4d &plane_c, + Eigen::Vector3d &intersection_point, + double determinant_tolerance = 1e-6) + { + return (threePlanesIntersection (plane_a, plane_b, plane_c, + intersection_point, determinant_tolerance)); + } + +} +/*@}*/ + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/io.h b/deps_install/include/pcl-1.10/pcl/common/io.h new file mode 100755 index 0000000..eba6eaf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/io.h @@ -0,0 +1,587 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Get the index of a specified field (i.e., dimension/channel) + * \param[in] cloud the point cloud message + * \param[in] field_name the string defining the field name + * \ingroup common + */ + inline int + getFieldIndex (const pcl::PCLPointCloud2 &cloud, const std::string &field_name) + { + // Get the index we need + const auto result = std::find_if(cloud.fields.begin (), cloud.fields.end (), + [&field_name](const auto field) { return field.name == field_name; }); + if (result == cloud.fields.end ()) + return -1; + return std::distance(cloud.fields.begin (), result); + } + + /** \brief Get the index of a specified field (i.e., dimension/channel) + * \param[in] cloud the point cloud message + * \param[in] field_name the string defining the field name + * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains + * \ingroup common + */ + template + PCL_DEPRECATED("use getFieldIndex (field_name, fields) instead") + inline int + getFieldIndex (const pcl::PointCloud &cloud, const std::string &field_name, + std::vector &fields); + + /** \brief Get the index of a specified field (i.e., dimension/channel) + * \tparam PointT datatype for which fields is being queries + * \param[in] field_name the string defining the field name + * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains + * \ingroup common + */ + template inline int + getFieldIndex (const std::string &field_name, + std::vector &fields); + /** \brief Get the index of a specified field (i.e., dimension/channel) + * \tparam PointT datatype for which fields is being queries + * \param[in] field_name the string defining the field name + * \param[in] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains + * \ingroup common + */ + template inline int + getFieldIndex (const std::string &field_name, + const std::vector &fields); + + /** \brief Get the list of available fields (i.e., dimension/channel) + * \param[in] cloud the point cloud message + * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains + * \ingroup common + */ + template + PCL_DEPRECATED("use getFields () with return value instead") + inline void + getFields (const pcl::PointCloud &cloud, std::vector &fields); + + /** \brief Get the list of available fields (i.e., dimension/channel) + * \tparam PointT datatype whose details are requested + * \param[out] fields a vector to the original \a PCLPointField vector that the raw PointCloud message contains + * \ingroup common + */ + template + PCL_DEPRECATED("use getFields () with return value instead") + inline void + getFields (std::vector &fields); + + /** \brief Get the list of available fields (i.e., dimension/channel) + * \tparam PointT datatype whose details are requested + * \ingroup common + */ + template inline std::vector + getFields (); + + /** \brief Get the list of all fields available in a given cloud + * \param[in] cloud the point cloud message + * \ingroup common + */ + template inline std::string + getFieldsList (const pcl::PointCloud &cloud); + + /** \brief Get the available point cloud fields as a space separated string + * \param[in] cloud a pointer to the PointCloud message + * \ingroup common + */ + inline std::string + getFieldsList (const pcl::PCLPointCloud2 &cloud) + { + return std::accumulate(std::next (cloud.fields.begin ()), cloud.fields.end (), cloud.fields[0].name, + [](const auto& acc, const auto& field) { return acc + " " + field.name; }); + } + + /** \brief Obtains the size of a specific field data type in bytes + * \param[in] datatype the field data type (see PCLPointField.h) + * \ingroup common + */ + inline int + getFieldSize (const int datatype) + { + switch (datatype) + { + case pcl::PCLPointField::INT8: + case pcl::PCLPointField::UINT8: + return (1); + + case pcl::PCLPointField::INT16: + case pcl::PCLPointField::UINT16: + return (2); + + case pcl::PCLPointField::INT32: + case pcl::PCLPointField::UINT32: + case pcl::PCLPointField::FLOAT32: + return (4); + + case pcl::PCLPointField::FLOAT64: + return (8); + + default: + return (0); + } + } + + /** \brief Obtain a vector with the sizes of all valid fields (e.g., not "_") + * \param[in] fields the input vector containing the fields + * \param[out] field_sizes the resultant field sizes in bytes + */ + PCL_EXPORTS void + getFieldsSizes (const std::vector &fields, + std::vector &field_sizes); + + /** \brief Obtains the type of the PCLPointField from a specific size and type + * \param[in] size the size in bytes of the data field + * \param[in] type a char describing the type of the field ('F' = float, 'I' = signed, 'U' = unsigned) + * \ingroup common + */ + inline int + getFieldType (const int size, char type) + { + type = std::toupper (type, std::locale::classic ()); + switch (size) + { + case 1: + if (type == 'I') + return (pcl::PCLPointField::INT8); + if (type == 'U') + return (pcl::PCLPointField::UINT8); + break; + + case 2: + if (type == 'I') + return (pcl::PCLPointField::INT16); + if (type == 'U') + return (pcl::PCLPointField::UINT16); + break; + + case 4: + if (type == 'I') + return (pcl::PCLPointField::INT32); + if (type == 'U') + return (pcl::PCLPointField::UINT32); + if (type == 'F') + return (pcl::PCLPointField::FLOAT32); + break; + + case 8: + if (type == 'F') + return (pcl::PCLPointField::FLOAT64); + break; + } + return (-1); + } + + /** \brief Obtains the type of the PCLPointField from a specific PCLPointField as a char + * \param[in] type the PCLPointField field type + * \ingroup common + */ + inline char + getFieldType (const int type) + { + switch (type) + { + case pcl::PCLPointField::INT8: + case pcl::PCLPointField::INT16: + case pcl::PCLPointField::INT32: + return ('I'); + + case pcl::PCLPointField::UINT8: + case pcl::PCLPointField::UINT16: + case pcl::PCLPointField::UINT32: + return ('U'); + + case pcl::PCLPointField::FLOAT32: + case pcl::PCLPointField::FLOAT64: + return ('F'); + default: + return ('?'); + } + } + + enum InterpolationType + { + BORDER_CONSTANT = 0, BORDER_REPLICATE = 1, + BORDER_REFLECT = 2, BORDER_WRAP = 3, + BORDER_REFLECT_101 = 4, BORDER_TRANSPARENT = 5, + BORDER_DEFAULT = BORDER_REFLECT_101 + }; + + /** \brief \return the right index according to the interpolation type. + * \note this is adapted from OpenCV + * \param p the index of point to interpolate + * \param length the top/bottom row or left/right column index + * \param type the requested interpolation + * \throws pcl::BadArgumentException if type is unknown + */ + PCL_EXPORTS int + interpolatePointIndex (int p, int length, InterpolationType type); + + /** \brief Concatenate two pcl::PointCloud + * \param[in] cloud1 the first input point cloud dataset + * \param[in] cloud2 the second input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \return true if successful, false otherwise + * \ingroup common + */ + template + PCL_EXPORTS bool + concatenate (const pcl::PointCloud &cloud1, + const pcl::PointCloud &cloud2, + pcl::PointCloud &cloud_out) + { + return pcl::PointCloud::concatenate(cloud1, cloud2, cloud_out); + } + + /** \brief Concatenate two pcl::PCLPointCloud2 + * + * \warn This function subtly differs from the deprecated `concatenatePointloud` + * The difference is thatthis function will concatenate IFF the non-skip fields + * are in the correct order and same in number. The deprecated function skipped + * fields even if both clouds didn't agree on the number of output fields + * \param[in] cloud1 the first input point cloud dataset + * \param[in] cloud2 the second input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \return true if successful, false otherwise + * \ingroup common + */ + PCL_EXPORTS inline bool + concatenate (const pcl::PCLPointCloud2 &cloud1, + const pcl::PCLPointCloud2 &cloud2, + pcl::PCLPointCloud2 &cloud_out) + { + return pcl::PCLPointCloud2::concatenate(cloud1, cloud2, cloud_out); + } + + /** \brief Concatenate two pcl::PolygonMesh + * \param[in] mesh1 the first input mesh + * \param[in] mesh2 the second input mesh + * \param[out] mesh_out the resultant output mesh + * \return true if successful, false otherwise + * \ingroup common + */ + PCL_EXPORTS inline bool + concatenate (const pcl::PolygonMesh &mesh1, + const pcl::PolygonMesh &mesh2, + pcl::PolygonMesh &mesh_out) + { + return pcl::PolygonMesh::concatenate(mesh1, mesh2, mesh_out); + } + + /** \brief Concatenate two pcl::PCLPointCloud2 + * \param[in] cloud1 the first input point cloud dataset + * \param[in] cloud2 the second input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \return true if successful, false otherwise (e.g., name/number of fields differs) + * \ingroup common + */ + PCL_DEPRECATED("use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)") + PCL_EXPORTS bool + concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1, + const pcl::PCLPointCloud2 &cloud2, + pcl::PCLPointCloud2 &cloud_out); + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + PCL_EXPORTS void + copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, + const std::vector &indices, + pcl::PCLPointCloud2 &cloud_out); + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + PCL_EXPORTS void + copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, + const std::vector > &indices, + pcl::PCLPointCloud2 &cloud_out); + + /** \brief Copy fields and point cloud data from \a cloud_in to \a cloud_out + * \param[in] cloud_in the input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \ingroup common + */ + PCL_EXPORTS void + copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, + pcl::PCLPointCloud2 &cloud_out); + + /** \brief Check if two given point types are the same or not. */ + template inline bool + isSamePointType () + { + return (typeid (Point1T) == typeid (Point2T)); + } + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + template > void + copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out); + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + const PointIndices &indices, + pcl::PointCloud &cloud_out); + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out); + + /** \brief Copy all the fields from a given point cloud into a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out); + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the vector of indices representing the points to be copied from \a cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + template > void + copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out); + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the PointIndices structure representing the points to be copied from cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + const PointIndices &indices, + pcl::PointCloud &cloud_out); + + /** \brief Extract the indices of a given point cloud as a new point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] indices the vector of indices representing the points to be copied from cloud_in + * \param[out] cloud_out the resultant output point cloud dataset + * \note Assumes unique indices. + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out); + + /** \brief Copy a point cloud inside a larger one interpolating borders. + * \param[in] cloud_in the input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \param top + * \param bottom + * \param left + * \param right + * Position of cloud_in inside cloud_out is given by \a top, \a left, \a bottom \a right. + * \param[in] border_type the interpolating method (pcl::BORDER_XXX) + * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh + * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb + * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba + * BORDER_WRAP: cdefgh|abcdefgh|abcdefg + * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i' + * BORDER_TRANSPARENT: mnopqr|abcdefgh|tuvwxyz where m-r and t-z are original values of cloud_out + * \param value + * \throw pcl::BadArgumentException if any of top, bottom, left or right is negative. + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + int top, int bottom, int left, int right, + pcl::InterpolationType border_type, const PointT& value); + + /** \brief Concatenate two datasets representing different fields. + * + * \note If the input datasets have overlapping fields (i.e., both contain + * the same fields), then the data in the second cloud (cloud2_in) will + * overwrite the data in the first (cloud1_in). + * + * \param[in] cloud1_in the first input dataset + * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared) + * \param[out] cloud_out the resultant output dataset created by the concatenation of all the fields in the input datasets + * \ingroup common + */ + template void + concatenateFields (const pcl::PointCloud &cloud1_in, + const pcl::PointCloud &cloud2_in, + pcl::PointCloud &cloud_out); + + /** \brief Concatenate two datasets representing different fields. + * + * \note If the input datasets have overlapping fields (i.e., both contain + * the same fields), then the data in the second cloud (cloud2_in) will + * overwrite the data in the first (cloud1_in). + * + * \param[in] cloud1_in the first input dataset + * \param[in] cloud2_in the second input dataset (overwrites the fields of the first dataset for those that are shared) + * \param[out] cloud_out the output dataset created by concatenating all the fields in the input datasets + * \ingroup common + */ + PCL_EXPORTS bool + concatenateFields (const pcl::PCLPointCloud2 &cloud1_in, + const pcl::PCLPointCloud2 &cloud2_in, + pcl::PCLPointCloud2 &cloud_out); + + /** \brief Copy the XYZ dimensions of a pcl::PCLPointCloud2 into Eigen format + * \param[in] in the point cloud message + * \param[out] out the resultant Eigen MatrixXf format containing XYZ0 / point + * \ingroup common + */ + PCL_EXPORTS bool + getPointCloudAsEigen (const pcl::PCLPointCloud2 &in, Eigen::MatrixXf &out); + + /** \brief Copy the XYZ dimensions from an Eigen MatrixXf into a pcl::PCLPointCloud2 message + * \param[in] in the Eigen MatrixXf format containing XYZ0 / point + * \param[out] out the resultant point cloud message + * \note the method assumes that the PCLPointCloud2 message already has the fields set up properly ! + * \ingroup common + */ + PCL_EXPORTS bool + getEigenAsPointCloud (Eigen::MatrixXf &in, pcl::PCLPointCloud2 &out); + + namespace io + { + /** \brief swap bytes order of a char array of length N + * \param bytes char array to swap + * \ingroup common + */ + template void + swapByte (char* bytes); + + /** \brief specialization of swapByte for dimension 1 + * \param bytes char array to swap + */ + template <> inline void + swapByte<1> (char* bytes) { bytes[0] = bytes[0]; } + + + /** \brief specialization of swapByte for dimension 2 + * \param bytes char array to swap + */ + template <> inline void + swapByte<2> (char* bytes) { std::swap (bytes[0], bytes[1]); } + + /** \brief specialization of swapByte for dimension 4 + * \param bytes char array to swap + */ + template <> inline void + swapByte<4> (char* bytes) + { + std::swap (bytes[0], bytes[3]); + std::swap (bytes[1], bytes[2]); + } + + /** \brief specialization of swapByte for dimension 8 + * \param bytes char array to swap + */ + template <> inline void + swapByte<8> (char* bytes) + { + std::swap (bytes[0], bytes[7]); + std::swap (bytes[1], bytes[6]); + std::swap (bytes[2], bytes[5]); + std::swap (bytes[3], bytes[4]); + } + + /** \brief swaps byte of an arbitrary type T casting it to char* + * \param value the data you want its bytes swapped + */ + template void + swapByte (T& value) + { + pcl::io::swapByte (reinterpret_cast (&value)); + } + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/norms.h b/deps_install/include/pcl-1.10/pcl/common/norms.h new file mode 100755 index 0000000..c97a162 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/norms.h @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +/** + * \file norms.h + * Define standard C methods to calculate different norms + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + /** \brief Enum that defines all the types of norms available. + * \note Any new norm type should have its own enum value and its own case in the selectNorm () method + * \ingroup common + */ + enum NormType {L1, L2_SQR, L2, LINF, JM, B, SUBLINEAR, CS, DIV, PF, K, KL, HIK}; + + /** \brief Method that calculates any norm type available, based on the norm_type variable + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + * */ + template inline float + selectNorm (FloatVectorT A, FloatVectorT B, int dim, NormType norm_type); + + /** \brief Compute the L1 norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + L1_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the squared L2 norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + L2_Norm_SQR (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the L2 norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + L2_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the L-infinity norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + Linf_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the JM norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + JM_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the B norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + B_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the sublinear norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + Sublinear_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the CS norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + CS_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the div norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + Div_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the PF norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \param P1 the first parameter + * \param P2 the second parameter + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + PF_Norm (FloatVectorT A, FloatVectorT B, int dim, float P1, float P2); + + /** \brief Compute the K norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \param P1 the first parameter + * \param P2 the second parameter + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + K_Norm (FloatVectorT A, FloatVectorT B, int dim, float P1, float P2); + + /** \brief Compute the KL between two discrete probability density functions + * \param A the first discrete PDF + * \param B the second discrete PDF + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + KL_Norm (FloatVectorT A, FloatVectorT B, int dim); + + /** \brief Compute the HIK norm of the vector between two points + * \param A the first point + * \param B the second point + * \param dim the number of dimensions in \a A and \a B (dimensions must match) + * \note FloatVectorT is any type of vector with its values accessible via [ ] + * \ingroup common + */ + template inline float + HIK_Norm (FloatVectorT A, FloatVectorT B, int dim); +} +/*@}*/ +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/pca.h b/deps_install/include/pcl-1.10/pcl/common/pca.h new file mode 100755 index 0000000..f48f0e1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/pca.h @@ -0,0 +1,284 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** Principal Component analysis (PCA) class.\n + * Principal components are extracted by singular values decomposition on the + * covariance matrix of the centered input cloud. Available data after pca computation + * are:\n + * - The Mean of the input data\n + * - The Eigenvectors: Ordered set of vectors representing the resultant principal components and the eigenspace cartesian basis (right-handed coordinate system).\n + * - The Eigenvalues: Eigenvectors correspondent loadings ordered in descending order.\n\n + * Other methods allow projection in the eigenspace, reconstruction from eigenspace and + * update of the eigenspace with a new datum (according Matej Artec, Matjaz Jogan and + * Ales Leonardis: "Incremental PCA for On-line Visual Learning and Recognition"). + * + * \author Nizar Sallem + * \ingroup common + */ + template + class PCA : public pcl::PCLBase + { + public: + using Base = pcl::PCLBase; + using PointCloud = typename Base::PointCloud; + using PointCloudPtr = typename Base::PointCloudPtr; + using PointCloudConstPtr = typename Base::PointCloudConstPtr; + using PointIndicesPtr = typename Base::PointIndicesPtr; + using PointIndicesConstPtr = typename Base::PointIndicesConstPtr; + + using Base::input_; + using Base::indices_; + using Base::initCompute; + using Base::setInputCloud; + + /** Updating method flag */ + enum FLAG + { + /** keep the new basis vectors if possible */ + increase, + /** preserve subspace dimension */ + preserve + }; + + /** \brief Default Constructor + * \param basis_only flag to compute only the PCA basis + */ + PCA (bool basis_only = false) + : Base () + , compute_done_ (false) + , basis_only_ (basis_only) + {} + + /** Copy Constructor + * \param[in] pca PCA object + */ + PCA (PCA const & pca) + : Base (pca) + , compute_done_ (pca.compute_done_) + , basis_only_ (pca.basis_only_) + , eigenvectors_ (pca.eigenvectors_) + , coefficients_ (pca.coefficients_) + , mean_ (pca.mean_) + , eigenvalues_ (pca.eigenvalues_) + {} + + /** Assignment operator + * \param[in] pca PCA object + */ + inline PCA& + operator= (PCA const & pca) + { + eigenvectors_ = pca.eigenvectors_; + coefficients_ = pca.coefficients_; + eigenvalues_ = pca.eigenvalues_; + mean_ = pca.mean_; + return (*this); + } + + /** \brief Provide a pointer to the input dataset + * \param cloud the const boost shared pointer to a PointCloud message + */ + inline void + setInputCloud (const PointCloudConstPtr &cloud) override + { + Base::setInputCloud (cloud); + compute_done_ = false; + } + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + void + setIndices (const IndicesPtr &indices) override + { + Base::setIndices (indices); + compute_done_ = false; + } + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + void + setIndices (const IndicesConstPtr &indices) override + { + Base::setIndices (indices); + compute_done_ = false; + } + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + void + setIndices (const PointIndicesConstPtr &indices) override + { + Base::setIndices (indices); + compute_done_ = false; + } + + /** \brief Set the indices for the points laying within an interest region of + * the point cloud. + * \note you shouldn't call this method on unorganized point clouds! + * \param[in] row_start the offset on rows + * \param[in] col_start the offset on columns + * \param[in] nb_rows the number of rows to be considered row_start included + * \param[in] nb_cols the number of columns to be considered col_start included + */ + void + setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols) override + { + Base::setIndices (row_start, col_start, nb_rows, nb_cols); + compute_done_ = false; + } + + /** \brief Mean accessor + * \throw InitFailedException + */ + inline Eigen::Vector4f& + getMean () + { + if (!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::PCA::getMean] PCA initCompute failed"); + return (mean_); + } + + /** Eigen Vectors accessor + * \return Column ordered eigenvectors, representing the eigenspace cartesian basis (right-handed coordinate system). + * \throw InitFailedException + */ + inline Eigen::Matrix3f& + getEigenVectors () + { + if (!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::PCA::getEigenVectors] PCA initCompute failed"); + return (eigenvectors_); + } + + /** Eigen Values accessor + * \throw InitFailedException + */ + inline Eigen::Vector3f& + getEigenValues () + { + if (!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::PCA::getEigenVectors] PCA getEigenValues failed"); + return (eigenvalues_); + } + + /** Coefficients accessor + * \throw InitFailedException + */ + inline Eigen::MatrixXf& + getCoefficients () + { + if (!compute_done_) + initCompute (); + if (!compute_done_) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::PCA::getEigenVectors] PCA getCoefficients failed"); + return (coefficients_); + } + + /** update PCA with a new point + * \param[in] input input point + * \param[in] flag update flag + * \throw InitFailedException + */ + inline void + update (const PointT& input, FLAG flag = preserve); + + /** Project point on the eigenspace. + * \param[in] input point from original dataset + * \param[out] projection the point in eigen vectors space + * \throw InitFailedException + */ + inline void + project (const PointT& input, PointT& projection); + + /** Project cloud on the eigenspace. + * \param[in] input cloud from original dataset + * \param[out] projection the cloud in eigen vectors space + * \throw InitFailedException + */ + inline void + project (const PointCloud& input, PointCloud& projection); + + /** Reconstruct point from its projection + * \param[in] projection point from eigenvector space + * \param[out] input reconstructed point + * \throw InitFailedException + */ + inline void + reconstruct (const PointT& projection, PointT& input); + + /** Reconstruct cloud from its projection + * \param[in] projection cloud from eigenvector space + * \param[out] input reconstructed cloud + * \throw InitFailedException + */ + inline void + reconstruct (const PointCloud& projection, PointCloud& input); + private: + inline bool + initCompute (); + + bool compute_done_; + bool basis_only_; + Eigen::Matrix3f eigenvectors_; + Eigen::MatrixXf coefficients_; + Eigen::Vector4f mean_; + Eigen::Vector3f eigenvalues_; + }; // class PCA +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/piecewise_linear_function.h b/deps_install/include/pcl-1.10/pcl/common/piecewise_linear_function.h new file mode 100755 index 0000000..837f9ce --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/piecewise_linear_function.h @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/* \author Bastian Steder */ + +#pragma once + +#include + +namespace pcl +{ + /** + * \brief This provides functionalities to efficiently return values for piecewise linear function + * \ingroup common + */ + class PiecewiseLinearFunction + { + public: + // =====CONSTRUCTOR & DESTRUCTOR===== + //! Constructor + PiecewiseLinearFunction (float factor, float offset); + + // =====PUBLIC METHODS===== + //! Get the list of known data points + std::vector& + getDataPoints () + { + return data_points_; + } + + //! Get the value of the function at the given point + inline float + getValue (float point) const; + + // =====PUBLIC MEMBER VARIABLES===== + + protected: + // =====PROTECTED MEMBER VARIABLES===== + std::vector data_points_; + float factor_; + float offset_; + }; + +} // end namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/point_operators.h b/deps_install/include/pcl-1.10/pcl/common/point_operators.h new file mode 100755 index 0000000..b32c86e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/point_operators.h @@ -0,0 +1,41 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once diff --git a/deps_install/include/pcl-1.10/pcl/common/point_tests.h b/deps_install/include/pcl-1.10/pcl/common/point_tests.h new file mode 100755 index 0000000..abd58c3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/point_tests.h @@ -0,0 +1,144 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: point_types.hpp 6415 2012-07-16 20:11:47Z rusu $ + * + */ + +#pragma once + +#include + +#ifdef _MSC_VER +#include +#endif + +namespace pcl +{ + /** Tests if the 3D components of a point are all finite + * param[in] pt point to be tested + * return true if finite, false otherwise + */ + template inline bool + isFinite (const PointT &pt) + { + return (std::isfinite (pt.x) && std::isfinite (pt.y) && std::isfinite (pt.z)); + } + +#ifdef _MSC_VER + template inline bool + isFinite (const Eigen::internal::workaround_msvc_stl_support &pt) + { + return isFinite (static_cast (pt)); + } +#endif + + template<> inline bool isFinite(const pcl::Axis&) { return (true); } + template<> inline bool isFinite(const pcl::BRISKSignature512&) { return (true); } + template<> inline bool isFinite(const pcl::BorderDescription &) { return true; } + template<> inline bool isFinite(const pcl::Boundary&) { return (true); } + template<> inline bool isFinite(const pcl::ESFSignature640&) { return (true); } + template<> inline bool isFinite(const pcl::FPFHSignature33&) { return (true); } + template<> inline bool isFinite(const pcl::Intensity&) { return (true); } + template<> inline bool isFinite(const pcl::IntensityGradient&) { return (true); } + template<> inline bool isFinite(const pcl::Label&) { return (true); } + template<> inline bool isFinite(const pcl::MomentInvariants&) { return (true); } + template<> inline bool isFinite(const pcl::NormalBasedSignature12&) { return (true); } + template<> inline bool isFinite(const pcl::PFHRGBSignature250&) { return (true); } + template<> inline bool isFinite(const pcl::PFHSignature125&) { return (true); } + template<> inline bool isFinite(const pcl::PPFRGBSignature&) { return (true); } + template<> inline bool isFinite(const pcl::PPFSignature&) { return (true); } + template<> inline bool isFinite(const pcl::PrincipalCurvatures&) { return (true); } + template<> inline bool isFinite(const pcl::PrincipalRadiiRSD&) { return (true); } + template<> inline bool isFinite(const pcl::RGB&) { return (true); } + template<> inline bool isFinite(const pcl::ReferenceFrame&) { return (true); } + template<> inline bool isFinite(const pcl::SHOT1344&) { return (true); } + template<> inline bool isFinite(const pcl::SHOT352&) { return (true); } + template<> inline bool isFinite(const pcl::ShapeContext1980&) { return (true); } + template<> inline bool isFinite(const pcl::UniqueShapeContext1960&) { return (true); } + template<> inline bool isFinite(const pcl::VFHSignature308&) { return (true); } + + // specification for pcl::PointXY + template <> inline bool + isFinite (const pcl::PointXY &p) + { + return (std::isfinite (p.x) && std::isfinite (p.y)); + } + + // specification for pcl::Normal + template <> inline bool + isFinite (const pcl::Normal &n) + { + return (std::isfinite (n.normal_x) && std::isfinite (n.normal_y) && std::isfinite (n.normal_z)); + } + + // generic fallback cases + template = true> constexpr inline bool + isXYFinite (const PointT&) noexcept + { + return true; + } + + template = true> constexpr inline bool + isXYZFinite (const PointT&) noexcept + { + return true; + } + + template = true> constexpr inline bool + isNormalFinite (const PointT&) noexcept + { + return true; + } + + // special cases for checks + template = true> inline bool + isXYFinite (const PointT& pt) noexcept + { + return std::isfinite(pt.x) && std::isfinite(pt.y); + } + + template = true> inline bool + isXYZFinite (const PointT& pt) noexcept + { + return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z); + } + + template = true> inline bool + isNormalFinite (const PointT& pt) noexcept + { + return std::isfinite(pt.normal_x) && std::isfinite(pt.normal_y) && std::isfinite(pt.normal_z); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/common/polynomial_calculations.h b/deps_install/include/pcl-1.10/pcl/common/polynomial_calculations.h new file mode 100755 index 0000000..26d5cc7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/polynomial_calculations.h @@ -0,0 +1,128 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief This provides some functionality for polynomials, + * like finding roots or approximating bivariate polynomials + * \author Bastian Steder + * \ingroup common + */ + template + class PolynomialCalculationsT + { + public: + // =====PUBLIC STRUCTS===== + //! Parameters used in this class + struct Parameters + { + Parameters () { setZeroValue (1e-6);} + //! Set zero_value + void + setZeroValue (real new_zero_value); + + real zero_value = {}; //!< Every value below this is considered to be zero + real sqr_zero_value = {}; //!< sqr of the above + }; + + // =====PUBLIC METHODS===== + /** Solves an equation of the form ax^4 + bx^3 + cx^2 +dx + e = 0 + * See http://en.wikipedia.org/wiki/Quartic_equation#Summary_of_Ferrari.27s_method */ + inline void + solveQuarticEquation (real a, real b, real c, real d, real e, std::vector& roots) const; + + /** Solves an equation of the form ax^3 + bx^2 + cx + d = 0 + * See http://en.wikipedia.org/wiki/Cubic_equation */ + inline void + solveCubicEquation (real a, real b, real c, real d, std::vector& roots) const; + + /** Solves an equation of the form ax^2 + bx + c = 0 + * See http://en.wikipedia.org/wiki/Quadratic_equation */ + inline void + solveQuadraticEquation (real a, real b, real c, std::vector& roots) const; + + /** Solves an equation of the form ax + b = 0 */ + inline void + solveLinearEquation (real a, real b, std::vector& roots) const; + + /** Get the bivariate polynomial approximation for Z(X,Y) from the given sample points. + * The parameters a,b,c,... for the polynom are returned. + * The order is, e.g., for degree 1: ax+by+c and for degree 2: ax²+bxy+cx+dy²+ey+f. + * error is set to true if the approximation did not work for any reason + * (not enough points, matrix not invertible, etc.) */ + inline BivariatePolynomialT + bivariatePolynomialApproximation (std::vector, Eigen::aligned_allocator > >& samplePoints, + unsigned int polynomial_degree, bool& error) const; + + //! Same as above, using a reference for the return value + inline bool + bivariatePolynomialApproximation (std::vector, Eigen::aligned_allocator > >& samplePoints, + unsigned int polynomial_degree, BivariatePolynomialT& ret) const; + + //! Set the minimum value under which values are considered zero + inline void + setZeroValue (real new_zero_value) { parameters_.setZeroValue(new_zero_value); } + + protected: + // =====PROTECTED METHODS===== + //! check if std::abs(d); + using PolynomialCalculations = PolynomialCalculationsT; + +} // end namespace + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/poses_from_matches.h b/deps_install/include/pcl-1.10/pcl/common/poses_from_matches.h new file mode 100755 index 0000000..ada6d97 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/poses_from_matches.h @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** + * \brief calculate 3D transformation based on point correspondences + * \author Bastian Steder + * \ingroup common + */ + class PCL_EXPORTS PosesFromMatches + { + public: + // =====STRUCTS===== + //! Parameters used in this class + struct PCL_EXPORTS Parameters + { + float max_correspondence_distance_error = 0.2f; // As a fraction + }; + + //! A result of the pose estimation process + struct PoseEstimate + { + Eigen::Affine3f transformation = Eigen::Affine3f::Identity (); //!< The estimated transformation between the two coordinate systems + float score = 0; //!< An estimate in [0,1], how good the estimated pose is + std::vector correspondence_indices; //!< The indices of the used correspondences + + struct IsBetter + { + bool operator()(const PoseEstimate& pe1, const PoseEstimate& pe2) const { return pe1.score>pe2.score;} + }; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + // =====TYPEDEFS===== + using PoseEstimatesVector = std::vector >; + + + // =====STATIC METHODS===== + + // =====PUBLIC METHODS===== + /** Use single 6DOF correspondences to estimate transformations between the coordinate systems. + * Use max_no_of_results=-1 to use all. + * It is assumed, that the correspondences are sorted from good to bad. */ + void + estimatePosesUsing1Correspondence ( + const PointCorrespondences6DVector& correspondences, + int max_no_of_results, PoseEstimatesVector& pose_estimates) const; + + /** Use pairs of 6DOF correspondences to estimate transformations between the coordinate systems. + * It is assumed, that the correspondences are sorted from good to bad. */ + void + estimatePosesUsing2Correspondences ( + const PointCorrespondences6DVector& correspondences, + int max_no_of_tested_combinations, int max_no_of_results, + PoseEstimatesVector& pose_estimates) const; + + /** Use triples of 6DOF correspondences to estimate transformations between the coordinate systems. + * It is assumed, that the correspondences are sorted from good to bad. */ + void + estimatePosesUsing3Correspondences ( + const PointCorrespondences6DVector& correspondences, + int max_no_of_tested_combinations, int max_no_of_results, + PoseEstimatesVector& pose_estimates) const; + + /// Get a reference to the parameters struct + Parameters& + getParameters () { return parameters_; } + + protected: + // =====PROTECTED MEMBER VARIABLES===== + Parameters parameters_; + + }; + +} // end namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/common/projection_matrix.h b/deps_install/include/pcl-1.10/pcl/common/projection_matrix.h new file mode 100755 index 0000000..b02a361 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/projection_matrix.h @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +/** + * \file common/geometry.h + * Defines some geometrical functions and utility functions + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + template class PointCloud; + + /** \brief Estimates the projection matrix P = K * (R|-R*t) from organized point clouds, with + * K = [[fx, s, cx], [0, fy, cy], [0, 0, 1]] + * R = rotation matrix and + * t = translation vector + * + * \param[in] cloud input cloud. Must be organized and from a projective device. e.g. stereo or kinect, ... + * \param[out] projection_matrix output projection matrix + * \param[in] indices The indices to be used to determine the projection matrix + * \return the resudial error. A high residual indicates, that the point cloud was not from a projective device. + */ + template double + estimateProjectionMatrix (typename pcl::PointCloud::ConstPtr cloud, Eigen::Matrix& projection_matrix, const std::vector& indices = std::vector ()); + + /** \brief Determines the camera matrix from the given projection matrix. + * \note This method does NOT use a RQ decomposition, but uses the fact that the left 3x3 matrix P' of P squared eliminates the rotational part. + * P' = K * R -> P' * P'^T = K * R * R^T * K = K * K^T + * \param[in] projection_matrix + * \param[out] camera_matrix + */ + PCL_EXPORTS void + getCameraMatrixFromProjectionMatrix (const Eigen::Matrix& projection_matrix, Eigen::Matrix3f& camera_matrix); +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/random.h b/deps_install/include/pcl-1.10/pcl/common/random.h new file mode 100755 index 0000000..6262172 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/random.h @@ -0,0 +1,217 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + namespace common + { + /// uniform distribution dummy struct + template struct uniform_distribution; + /// uniform distribution int specialized + template + struct uniform_distribution::value>> + { + using type = std::uniform_int_distribution; + }; + /// uniform distribution float specialized + template + struct uniform_distribution::value>> + { + using type = std::uniform_real_distribution; + }; + /// normal distribution + template + struct normal_distribution + { + using type = std::normal_distribution; + }; + + /** \brief UniformGenerator class generates a random number from range [min, max] at each run picked + * according to a uniform distribution i.e eaach number within [min, max] has almost the same + * probability of being drawn. + * + * \author Nizar Sallem + */ + template + class UniformGenerator + { + public: + struct Parameters + { + Parameters (T _min = 0, T _max = 1, std::uint32_t _seed = 1) + : min (_min) + , max (_max) + , seed (_seed) + {} + + T min; + T max; + std::uint32_t seed; + }; + + /** Constructor + * \param min: included lower bound + * \param max: included higher bound + * \param seed: seeding value + */ + UniformGenerator(T min = 0, T max = 1, std::uint32_t seed = -1); + + /** Constructor + * \param parameters uniform distribution parameters and generator seed + */ + UniformGenerator(const Parameters& parameters); + + /** Change seed value + * \param[in] seed new generator seed value + */ + void + setSeed (std::uint32_t seed); + + /** Set the uniform number generator parameters + * \param[in] min minimum allowed value + * \param[in] max maximum allowed value + * \param[in] seed random number generator seed (applied if != -1) + */ + void + setParameters (T min, T max, std::uint32_t seed = -1); + + /** Set generator parameters + * \param parameters uniform distribution parameters and generator seed + */ + void + setParameters (const Parameters& parameters); + + /// \return uniform distribution parameters and generator seed + const Parameters& + getParameters () { return (parameters_); } + + /// \return a randomly generated number in the interval [min, max] + inline T + run () { return (distribution_ (rng_)); } + + private: + using DistributionType = typename uniform_distribution::type; + /// parameters + Parameters parameters_; + /// random number generator + std::mt19937 rng_; + /// uniform distribution + DistributionType distribution_; + }; + + /** \brief NormalGenerator class generates a random number from a normal distribution specified + * by (mean, sigma). + * + * \author Nizar Sallem + */ + template + class NormalGenerator + { + public: + struct Parameters + { + Parameters (T _mean = 0, T _sigma = 1, std::uint32_t _seed = 1) + : mean (_mean) + , sigma (_sigma) + , seed (_seed) + {} + + T mean; + T sigma; + std::uint32_t seed; + }; + + /** Constructor + * \param[in] mean normal mean + * \param[in] sigma normal variation + * \param[in] seed seeding value + */ + NormalGenerator(T mean = 0, T sigma = 1, std::uint32_t seed = -1); + + /** Constructor + * \param parameters normal distribution parameters and seed + */ + NormalGenerator(const Parameters& parameters); + + /** Change seed value + * \param[in] seed new seed value + */ + void + setSeed (std::uint32_t seed); + + /** Set the normal number generator parameters + * \param[in] mean mean of the normal distribution + * \param[in] sigma standard variation of the normal distribution + * \param[in] seed random number generator seed (applied if != -1) + */ + void + setParameters (T mean, T sigma, std::uint32_t seed = -1); + + /** Set generator parameters + * \param parameters normal distribution parameters and seed + */ + void + setParameters (const Parameters& parameters); + + /// \return normal distribution parameters and generator seed + const Parameters& + getParameters () { return (parameters_); } + + /// \return a randomly generated number in the normal distribution (mean, sigma) + inline T + run () { return (distribution_ (rng_)); } + + using DistributionType = typename normal_distribution::type; + /// parameters + Parameters parameters_; + /// random number generator + std::mt19937 rng_; + /// normal distribution + DistributionType distribution_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/spring.h b/deps_install/include/pcl-1.10/pcl/common/spring.h new file mode 100755 index 0000000..db0b846 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/spring.h @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace common + { + /** expand point cloud inserting \a amount rows at the + * top and the bottom of a point cloud and filling them with + * custom values. + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] val the point value to be inserted + * \param[in] amount the amount of rows to be added + */ + template void + expandRows (const PointCloud& input, PointCloud& output, + const PointT& val, const std::size_t& amount); + + /** expand point cloud inserting \a amount columns at + * the right and the left of a point cloud and filling them with + * custom values. + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] val the point value to be inserted + * \param[in] amount the amount of columns to be added + */ + template void + expandColumns (const PointCloud& input, PointCloud& output, + const PointT& val, const std::size_t& amount); + + /** expand point cloud duplicating the \a amount top and bottom rows times. + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] amount the amount of rows to be added + */ + template void + duplicateRows (const PointCloud& input, PointCloud& output, + const std::size_t& amount); + + /** expand point cloud duplicating the \a amount right and left columns + * times. + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] amount the amount of cilumns to be added + */ + template void + duplicateColumns (const PointCloud& input, PointCloud& output, + const std::size_t& amount); + + /** expand point cloud mirroring \a amount top and bottom rows. + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] amount the amount of rows to be added + */ + template void + mirrorRows (const PointCloud& input, PointCloud& output, + const std::size_t& amount); + + /** expand point cloud mirroring \a amount right and left columns. + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] amount the amount of rows to be added + */ + template void + mirrorColumns (const PointCloud& input, PointCloud& output, + const std::size_t& amount); + + /** delete \a amount rows in top and bottom of point cloud + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] amount the amount of rows to be added + */ + template void + deleteRows (const PointCloud& input, PointCloud& output, + const std::size_t& amount); + + /** delete \a amount columns in top and bottom of point cloud + * \param[in] input the input point cloud + * \param[out] output the output point cloud + * \param[in] amount the amount of rows to be added + */ + template void + deleteCols (const PointCloud& input, PointCloud& output, + const std::size_t& amount); + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/synchronizer.h b/deps_install/include/pcl-1.10/pcl/common/synchronizer.h new file mode 100755 index 0000000..8fbb516 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/synchronizer.h @@ -0,0 +1,179 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * Author: Nico Blodow (blodow@in.tum.de), Suat Gedikli (gedikli@willowgarage.com) + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** /brief This template class synchronizes two data streams of different types. + * The data can be added using add0 and add1 methods which expects also a timestamp of type unsigned long. + * If two matching data objects are found, registered callback functions are invoked with the objects and the time stamps. + * The only assumption of the timestamp is, that they are in the same unit, linear and strictly monotonic increasing. + * If filtering is desired, e.g. thresholding of time differences, the user can do that in the callback method. + * This class is thread safe. + * /ingroup common + */ + template + class Synchronizer + { + using T1Stamped = std::pair; + using T2Stamped = std::pair; + std::mutex mutex1_; + std::mutex mutex2_; + std::mutex publish_mutex_; + std::deque queueT1; + std::deque queueT2; + + using CallbackFunction = std::function; + + std::map cb_; + int callback_counter = 0; + public: + + int + addCallback (const CallbackFunction& callback) + { + std::unique_lock publish_lock (publish_mutex_); + cb_[callback_counter] = callback; + return callback_counter++; + } + + void + removeCallback (int i) + { + std::unique_lock publish_lock (publish_mutex_); + cb_.erase (i); + } + + void + add0 (const T1& t, unsigned long time) + { + mutex1_.lock (); + queueT1.push_back (T1Stamped (time, t)); + mutex1_.unlock (); + publish (); + } + + void + add1 (const T2& t, unsigned long time) + { + mutex2_.lock (); + queueT2.push_back (T2Stamped (time, t)); + mutex2_.unlock (); + publish (); + } + + private: + + void + publishData () + { + std::unique_lock lock1 (mutex1_); + std::unique_lock lock2 (mutex2_); + + for (const auto& cb: cb_) + { + if (cb.second) + { + cb.second.operator()(queueT1.front ().second, queueT2.front ().second, queueT1.front ().first, queueT2.front ().first); + } + } + + queueT1.pop_front (); + queueT2.pop_front (); + } + + void + publish () + { + // only one publish call at once allowed + std::unique_lock publish_lock (publish_mutex_); + + std::unique_lock lock1 (mutex1_); + if (queueT1.empty ()) + return; + T1Stamped t1 = queueT1.front (); + lock1.unlock (); + + std::unique_lock lock2 (mutex2_); + if (queueT2.empty ()) + return; + T2Stamped t2 = queueT2.front (); + lock2.unlock (); + + bool do_publish = false; + + if (t1.first <= t2.first) + { // iterate over queue1 + lock1.lock (); + while (queueT1.size () > 1 && queueT1[1].first <= t2.first) + queueT1.pop_front (); + + if (queueT1.size () > 1) + { // we have at least 2 measurements; first in past and second in future -> find out closer one! + if ( (t2.first << 1) > (queueT1[0].first + queueT1[1].first) ) + queueT1.pop_front (); + + do_publish = true; + } + lock1.unlock (); + } + else + { // iterate over queue2 + lock2.lock (); + while (queueT2.size () > 1 && (queueT2[1].first <= t1.first) ) + queueT2.pop_front (); + + if (queueT2.size () > 1) + { // we have at least 2 measurements; first in past and second in future -> find out closer one! + if ( (t1.first << 1) > queueT2[0].first + queueT2[1].first ) + queueT2.pop_front (); + + do_publish = true; + } + lock2.unlock (); + } + + if (do_publish) + publishData (); + } + } ; +} // namespace diff --git a/deps_install/include/pcl-1.10/pcl/common/time.h b/deps_install/include/pcl-1.10/pcl/common/time.h new file mode 100755 index 0000000..7fbd4ea --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/time.h @@ -0,0 +1,216 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +/** + * \file pcl/common/time.h + * Define methods for measuring time spent in code blocks + * \ingroup common + */ + +/*@{*/ +namespace pcl +{ + /** \brief Simple stopwatch. + * \ingroup common + */ + class StopWatch + { + public: + /** \brief Constructor. */ + StopWatch () : start_time_ (std::chrono::steady_clock::now()) + { + } + + /** \brief Retrieve the time in milliseconds spent since the last call to \a reset(). */ + inline double + getTime () const + { + auto end_time = std::chrono::steady_clock::now(); + return std::chrono::duration>(end_time - start_time_).count(); + } + + /** \brief Retrieve the time in seconds spent since the last call to \a reset(). */ + inline double + getTimeSeconds () const + { + return (getTime () * 0.001); + } + + /** \brief Reset the stopwatch to 0. */ + inline void + reset () + { + start_time_ = std::chrono::steady_clock::now(); + } + + protected: + std::chrono::time_point start_time_; + }; + + /** \brief Class to measure the time spent in a scope + * + * To use this class, e.g. to measure the time spent in a function, + * just create an instance at the beginning of the function. Example: + * + * \code + * { + * pcl::ScopeTime t1 ("calculation"); + * + * // ... perform calculation here + * } + * \endcode + * + * \ingroup common + */ + class ScopeTime : public StopWatch + { + public: + inline ScopeTime (const std::string &title = "") : + title_ (title) + { + } + + inline ~ScopeTime () + { + double val = this->getTime (); + std::cerr << title_ << " took " << val << "ms.\n"; + } + + private: + std::string title_; + }; + + /** \brief A helper class to measure frequency of a certain event. + * + * To use this class create an instance and call event() function every time + * the event in question occurs. The estimated frequency can be retrieved + * with getFrequency() function. + * + * \author Sergey Alexandrov + * \ingroup common + */ + class EventFrequency + { + + public: + + /** \brief Constructor. + * + * \param[in] window_size number of most recent events that are + * considered in frequency estimation (default: 30) */ + EventFrequency (std::size_t window_size = 30) + : window_size_ (window_size) + { + stop_watch_.reset (); + } + + /** \brief Notifies the class that the event occurred. */ + void event () + { + event_time_queue_.push (stop_watch_.getTimeSeconds ()); + if (event_time_queue_.size () > window_size_) + event_time_queue_.pop (); + } + + /** \brief Retrieve the estimated frequency. */ + double + getFrequency () const + { + if (event_time_queue_.size () < 2) + return (0.0); + return ((event_time_queue_.size () - 1) / + (event_time_queue_.back () - event_time_queue_.front ())); + } + + /** \brief Reset frequency computation. */ + void reset () + { + stop_watch_.reset (); + event_time_queue_ = std::queue (); + } + + private: + + pcl::StopWatch stop_watch_; + std::queue event_time_queue_; + const std::size_t window_size_; + + }; + +#ifndef MEASURE_FUNCTION_TIME +#define MEASURE_FUNCTION_TIME \ + ScopeTime scopeTime(__func__) +#endif + +inline double +getTime () +{ + return std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); +} + +/// Executes code, only if secs are gone since last exec. +#ifndef DO_EVERY_TS +#define DO_EVERY_TS(secs, currentTime, code) \ +if (1) {\ + static double s_lastDone_ = 0.0; \ + double s_now_ = (currentTime); \ + if (s_lastDone_ > s_now_) \ + s_lastDone_ = s_now_; \ + if ((s_now_ - s_lastDone_) > (secs)) { \ + code; \ + s_lastDone_ = s_now_; \ + }\ +} else \ + (void)0 +#endif + +/// Executes code, only if secs are gone since last exec. +#ifndef DO_EVERY +#define DO_EVERY(secs, code) \ + DO_EVERY_TS(secs, pcl::getTime(), code) +#endif + +} // end namespace +/*@}*/ diff --git a/deps_install/include/pcl-1.10/pcl/common/time_trigger.h b/deps_install/include/pcl-1.10/pcl/common/time_trigger.h new file mode 100755 index 0000000..b9cfa72 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/time_trigger.h @@ -0,0 +1,108 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#ifndef Q_MOC_RUN +#include +#endif + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Timer class that invokes registered callback methods periodically. + * \ingroup common + */ + class PCL_EXPORTS TimeTrigger + { + public: + using callback_type = std::function; + + /** \brief Timer class that calls a callback method periodically. Due to possible blocking calls, only one callback method can be registered per instance. + * \param[in] interval_seconds interval in seconds + * \param[in] callback callback to be invoked periodically + */ + TimeTrigger (double interval_seconds, const callback_type& callback); + + /** \brief Timer class that calls a callback method periodically. Due to possible blocking calls, only one callback method can be registered per instance. + * \param[in] interval_seconds interval in seconds + */ + TimeTrigger (double interval_seconds = 1.0); + + /** \brief Destructor. */ + ~TimeTrigger (); + + /** \brief registers a callback + * \param[in] callback callback function to the list of callbacks. signature has to be std::function + * \return connection the connection, which can be used to disable/enable and remove callback from list + */ + boost::signals2::connection registerCallback (const callback_type& callback); + + /** \brief Resets the timer interval + * \param[in] interval_seconds interval in seconds + */ + void + setInterval (double interval_seconds); + + /** \brief Start the Trigger. */ + void + start (); + + /** \brief Stop the Trigger. */ + void + stop (); + private: + void + thread_function (); + boost::signals2::signal callbacks_; + + double interval_; + + bool quit_; + bool running_; + + std::thread timer_thread_; + std::condition_variable condition_; + std::mutex condition_mutex_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/common/transformation_from_correspondences.h b/deps_install/include/pcl-1.10/pcl/common/transformation_from_correspondences.h new file mode 100755 index 0000000..2e0bca6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/transformation_from_correspondences.h @@ -0,0 +1,91 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + +namespace pcl +{ + /** + * \brief Calculates a transformation based on corresponding 3D points + * \author Bastian Steder + * \ingroup common + */ + class TransformationFromCorrespondences + { + public: + //-----CONSTRUCTOR&DESTRUCTOR----- + /** Constructor - dimension gives the size of the vectors to work with. */ + TransformationFromCorrespondences () + { reset (); } + + //-----METHODS----- + /** Reset the object to work with a new data set */ + inline void + reset (); + + /** Get the summed up weight of all added vectors */ + inline float + getAccumulatedWeight () const { return accumulated_weight_;} + + /** Get the number of added vectors */ + inline unsigned int + getNoOfSamples () { return no_of_samples_;} + + /** Add a new sample */ + inline void + add (const Eigen::Vector3f& point, const Eigen::Vector3f& corresponding_point, float weight=1.0); + + /** Calculate the transformation that will best transform the points into their correspondences */ + inline Eigen::Affine3f + getTransformation (); + + //-----VARIABLES----- + + protected: + //-----METHODS----- + //-----VARIABLES----- + unsigned int no_of_samples_ = 0; + float accumulated_weight_ = 0; + Eigen::Vector3f mean1_ = Eigen::Vector3f::Identity (); + Eigen::Vector3f mean2_ = Eigen::Vector3f::Identity (); + Eigen::Matrix covariance_ = Eigen::Matrix::Identity (); + }; + +} // END namespace + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/transforms.h b/deps_install/include/pcl-1.10/pcl/common/transforms.h new file mode 100755 index 0000000..7d6a9df --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/transforms.h @@ -0,0 +1,498 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Apply an affine transform defined by an Eigen Transform + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \note Can be used with cloud_in equal to cloud_out + * \ingroup common + */ + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields = true); + + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Affine3f &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, cloud_out, transform, copy_all_fields)); + } + + /** \brief Apply an affine transform defined by an Eigen Transform + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \ingroup common + */ + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields = true); + + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Affine3f &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Apply an affine transform defined by an Eigen Transform + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \ingroup common + */ + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, indices.indices, cloud_out, transform, copy_all_fields)); + } + + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Affine3f &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud + * \note Can be used with cloud_in equal to cloud_out + */ + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields = true); + + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Affine3f &transform, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, cloud_out, transform, copy_all_fields)); + } + + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud + */ + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields = true); + + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Affine3f &transform, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud + */ + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Transform &transform, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, indices.indices, cloud_out, transform, copy_all_fields)); + } + + + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Affine3f &transform, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Apply a rigid transform defined by a 4x4 matrix + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform a rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \note Can be used with cloud_in equal to cloud_out + * \ingroup common + */ + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &transform, + bool copy_all_fields = true) + { + Eigen::Transform t (transform); + return (transformPointCloud (cloud_in, cloud_out, t, copy_all_fields)); + } + + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, cloud_out, transform, copy_all_fields)); + } + + /** \brief Apply a rigid transform defined by a 4x4 matrix + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform a rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \ingroup common + */ + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &transform, + bool copy_all_fields = true) + { + Eigen::Transform t (transform); + return (transformPointCloud (cloud_in, indices, cloud_out, t, copy_all_fields)); + } + + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Apply a rigid transform defined by a 4x4 matrix + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform a rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \ingroup common + */ + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, indices.indices, cloud_out, transform, copy_all_fields)); + } + + template void + transformPointCloud (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud + * \note Can be used with cloud_in equal to cloud_out + * \ingroup common + */ + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &transform, + bool copy_all_fields = true) + { + Eigen::Transform t (transform); + return (transformPointCloudWithNormals (cloud_in, cloud_out, t, copy_all_fields)); + } + + + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, cloud_out, transform, copy_all_fields)); + } + + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud + * \note Can be used with cloud_in equal to cloud_out + * \ingroup common + */ + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &transform, + bool copy_all_fields = true) + { + Eigen::Transform t (transform); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, t, copy_all_fields)); + } + + + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const std::vector &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param[in] cloud_in the input point cloud + * \param[in] indices the set of point indices to use from the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation (typically a rigid transformation) + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud + * \note Can be used with cloud_in equal to cloud_out + * \ingroup common + */ + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &transform, + bool copy_all_fields = true) + { + Eigen::Transform t (transform); + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, t, copy_all_fields)); + } + + + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const pcl::PointIndices &indices, + pcl::PointCloud &cloud_out, + const Eigen::Matrix4f &transform, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, indices, cloud_out, transform, copy_all_fields)); + } + + /** \brief Apply a rigid transform defined by a 3D offset and a quaternion + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] offset the translation component of the rigid transformation + * \param[in] rotation the rotation component of the rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \ingroup common + */ + template inline void + transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &offset, + const Eigen::Quaternion &rotation, + bool copy_all_fields = true); + + template inline void + transformPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Vector3f &offset, + const Eigen::Quaternionf &rotation, + bool copy_all_fields = true) + { + return (transformPointCloud (cloud_in, cloud_out, offset, rotation, copy_all_fields)); + } + + /** \brief Transform a point cloud and rotate its normals using an Eigen transform. + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] offset the translation component of the rigid transformation + * \param[in] rotation the rotation component of the rigid transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z, normal_x, normal_y, normal_z) should be copied into the new + * transformed cloud + * \ingroup common + */ + template inline void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Matrix &offset, + const Eigen::Quaternion &rotation, + bool copy_all_fields = true); + + template void + transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Vector3f &offset, + const Eigen::Quaternionf &rotation, + bool copy_all_fields = true) + { + return (transformPointCloudWithNormals (cloud_in, cloud_out, offset, rotation, copy_all_fields)); + } + + /** \brief Transform a point with members x,y,z + * \param[in] point the point to transform + * \param[out] transform the transformation to apply + * \return the transformed point + * \ingroup common + */ + template inline PointT + transformPoint (const PointT &point, + const Eigen::Transform &transform); + + template inline PointT + transformPoint (const PointT &point, + const Eigen::Affine3f &transform) + { + return (transformPoint (point, transform)); + } + + /** \brief Transform a point with members x,y,z,normal_x,normal_y,normal_z + * \param[in] point the point to transform + * \param[out] transform the transformation to apply + * \return the transformed point + * \ingroup common + */ + template inline PointT + transformPointWithNormal (const PointT &point, + const Eigen::Transform &transform); + + template inline PointT + transformPointWithNormal (const PointT &point, + const Eigen::Affine3f &transform) + { + return (transformPointWithNormal (point, transform)); + } + + /** \brief Calculates the principal (PCA-based) alignment of the point cloud + * \param[in] cloud the input point cloud + * \param[out] transform the resultant transform + * \return the ratio lambda1/lambda2 or lambda2/lambda3, whatever is closer to 1. + * \note If the return value is close to one then the transformation might be not unique -> two principal directions have + * almost same variance (extend) + */ + template inline double + getPrincipalTransformation (const pcl::PointCloud &cloud, + Eigen::Transform &transform); + + template inline double + getPrincipalTransformation (const pcl::PointCloud &cloud, + Eigen::Affine3f &transform) + { + return (getPrincipalTransformation (cloud, transform)); + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/common/utils.h b/deps_install/include/pcl-1.10/pcl/common/utils.h new file mode 100755 index 0000000..6c91b00 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/utils.h @@ -0,0 +1,60 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace utils + { + /** \brief Check if val1 and val2 are equal to an epsilon extent + * \param[in] val1 first number to check + * \param[in] val2 second number to check + * \param[in] eps epsilon + * \return true if val1 is equal to val2, false otherwise. + */ + template bool + equal (T val1, T val2, T eps = std::numeric_limits::min ()) + { + return (std::fabs (val1 - val2) < eps); + } + } +} diff --git a/deps_install/include/pcl-1.10/pcl/common/vector_average.h b/deps_install/include/pcl-1.10/pcl/common/vector_average.h new file mode 100755 index 0000000..26a5ba6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/common/vector_average.h @@ -0,0 +1,119 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Calculates the weighted average and the covariance matrix + * + * A class to calculate the weighted average and the covariance matrix of a set of vectors with given weights. + * The original data is not saved. Mean and covariance are calculated iteratively. + * \author Bastian Steder + * \ingroup common + */ + template + class VectorAverage + { + public: + using VectorType = Eigen::Matrix; + using MatrixType = Eigen::Matrix; + //-----CONSTRUCTOR&DESTRUCTOR----- + /** Constructor - dimension gives the size of the vectors to work with. */ + VectorAverage (); + + //-----METHODS----- + /** Reset the object to work with a new data set */ + inline void + reset (); + + /** Get the mean of the added vectors */ + inline const + VectorType& getMean () const { return mean_;} + + /** Get the covariance matrix of the added vectors */ + inline const + MatrixType& getCovariance () const { return covariance_;} + + /** Get the summed up weight of all added vectors */ + inline real + getAccumulatedWeight () const { return accumulatedWeight_;} + + /** Get the number of added vectors */ + inline unsigned int + getNoOfSamples () { return noOfSamples_;} + + /** Add a new sample */ + inline void + add (const VectorType& sample, real weight=1.0); + + /** Do Principal component analysis */ + inline void + doPCA (VectorType& eigen_values, VectorType& eigen_vector1, + VectorType& eigen_vector2, VectorType& eigen_vector3) const; + + /** Do Principal component analysis */ + inline void + doPCA (VectorType& eigen_values) const; + + /** Get the eigenvector corresponding to the smallest eigenvalue */ + inline void + getEigenVector1 (VectorType& eigen_vector1) const; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + //-----VARIABLES----- + + + protected: + //-----METHODS----- + //-----VARIABLES----- + unsigned int noOfSamples_ = 0; + real accumulatedWeight_ = 0; + VectorType mean_ = VectorType::Identity (); + MatrixType covariance_ = MatrixType::Identity (); + }; + + using VectorAverage2f = VectorAverage; + using VectorAverage3f = VectorAverage; + using VectorAverage4f = VectorAverage; +} // END namespace + +#include diff --git a/deps_install/include/pcl-1.10/pcl/compression/color_coding.h b/deps_install/include/pcl-1.10/pcl/compression/color_coding.h new file mode 100755 index 0000000..5ab9333 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/color_coding.h @@ -0,0 +1,401 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace octree + { + using namespace std; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ColorCoding class + * \note This class encodes 8-bit color information for octree-based point cloud compression. + * \note + * \note typename: PointT: type of point used in pointcloud + * \author Julius Kammerl (julius@kammerl.de) + */ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + class ColorCoding + { + + // public typedefs + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + + /** \brief Constructor. + * + * */ + ColorCoding () : + output_ (), colorBitReduction_ (0) + { + } + + /** \brief Empty class constructor. */ + virtual + ~ColorCoding () + { + } + + /** \brief Define color bit depth of encoded color information. + * \param bitDepth_arg: amounts of bits for representing one color component + */ + inline + void + setBitDepth (unsigned char bitDepth_arg) + { + colorBitReduction_ = static_cast (8 - bitDepth_arg); + } + + /** \brief Retrieve color bit depth of encoded color information. + * \return amounts of bits for representing one color component + */ + inline unsigned char + getBitDepth () + { + return (static_cast (8 - colorBitReduction_)); + } + + /** \brief Set amount of voxels containing point color information and reserve memory + * \param voxelCount_arg: amounts of voxels + */ + inline void + setVoxelCount (unsigned int voxelCount_arg) + { + pointAvgColorDataVector_.reserve (voxelCount_arg * 3); + } + + /** \brief Set amount of points within point cloud to be encoded and reserve memory + * \param pointCount_arg: amounts of points within point cloud + * */ + inline + void + setPointCount (unsigned int pointCount_arg) + { + pointDiffColorDataVector_.reserve (pointCount_arg * 3); + } + + /** \brief Initialize encoding of color information + * */ + void + initializeEncoding () + { + pointAvgColorDataVector_.clear (); + + pointDiffColorDataVector_.clear (); + } + + /** \brief Initialize decoding of color information + * */ + void + initializeDecoding () + { + pointAvgColorDataVector_Iterator_ = pointAvgColorDataVector_.begin (); + + pointDiffColorDataVector_Iterator_ = pointDiffColorDataVector_.begin (); + } + + /** \brief Get reference to vector containing averaged color data + * */ + std::vector& + getAverageDataVector () + { + return pointAvgColorDataVector_; + } + + /** \brief Get reference to vector containing differential color data + * */ + std::vector& + getDifferentialDataVector () + { + return pointDiffColorDataVector_; + } + + /** \brief Encode averaged color information for a subset of points from point cloud + * \param indexVector_arg indices defining a subset of points from points cloud + * \param rgba_offset_arg offset to color information + * \param inputCloud_arg input point cloud + * */ + void + encodeAverageOfPoints (const typename std::vector& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) + { + unsigned int avgRed = 0; + unsigned int avgGreen = 0; + unsigned int avgBlue = 0; + + // iterate over points + std::size_t len = indexVector_arg.size (); + for (std::size_t i = 0; i < len; i++) + { + // get color information from points + const int& idx = indexVector_arg[i]; + const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); + const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + + // add color information + avgRed += (colorInt >> 0) & 0xFF; + avgGreen += (colorInt >> 8) & 0xFF; + avgBlue += (colorInt >> 16) & 0xFF; + + } + + // calculated average color information + if (len > 1) + { + avgRed /= static_cast (len); + avgGreen /= static_cast (len); + avgBlue /= static_cast (len); + } + + // remove least significant bits + avgRed >>= colorBitReduction_; + avgGreen >>= colorBitReduction_; + avgBlue >>= colorBitReduction_; + + // add to average color vector + pointAvgColorDataVector_.push_back (static_cast (avgRed)); + pointAvgColorDataVector_.push_back (static_cast (avgGreen)); + pointAvgColorDataVector_.push_back (static_cast (avgBlue)); + } + + /** \brief Encode color information of a subset of points from point cloud + * \param indexVector_arg indices defining a subset of points from points cloud + * \param rgba_offset_arg offset to color information + * \param inputCloud_arg input point cloud + * */ + void + encodePoints (const typename std::vector& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) + { + unsigned int avgRed; + unsigned int avgGreen; + unsigned int avgBlue; + + // initialize + avgRed = avgGreen = avgBlue = 0; + + // iterate over points + std::size_t len = indexVector_arg.size (); + for (std::size_t i = 0; i < len; i++) + { + // get color information from point + const int& idx = indexVector_arg[i]; + const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); + const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + + // add color information + avgRed += (colorInt >> 0) & 0xFF; + avgGreen += (colorInt >> 8) & 0xFF; + avgBlue += (colorInt >> 16) & 0xFF; + + } + + if (len > 1) + { + unsigned char diffRed; + unsigned char diffGreen; + unsigned char diffBlue; + + // calculated average color information + avgRed /= static_cast (len); + avgGreen /= static_cast (len); + avgBlue /= static_cast (len); + + // iterate over points for differential encoding + for (std::size_t i = 0; i < len; i++) + { + const int& idx = indexVector_arg[i]; + const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); + const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + + // extract color components and do XOR encoding with predicted average color + diffRed = (static_cast (avgRed)) ^ static_cast (((colorInt >> 0) & 0xFF)); + diffGreen = (static_cast (avgGreen)) ^ static_cast (((colorInt >> 8) & 0xFF)); + diffBlue = (static_cast (avgBlue)) ^ static_cast (((colorInt >> 16) & 0xFF)); + + // remove least significant bits + diffRed = static_cast (diffRed >> colorBitReduction_); + diffGreen = static_cast (diffGreen >> colorBitReduction_); + diffBlue = static_cast (diffBlue >> colorBitReduction_); + + // add to differential color vector + pointDiffColorDataVector_.push_back (static_cast (diffRed)); + pointDiffColorDataVector_.push_back (static_cast (diffGreen)); + pointDiffColorDataVector_.push_back (static_cast (diffBlue)); + } + } + + // remove least significant bits from average color information + avgRed >>= colorBitReduction_; + avgGreen >>= colorBitReduction_; + avgBlue >>= colorBitReduction_; + + // add to differential color vector + pointAvgColorDataVector_.push_back (static_cast (avgRed)); + pointAvgColorDataVector_.push_back (static_cast (avgGreen)); + pointAvgColorDataVector_.push_back (static_cast (avgBlue)); + + } + + /** \brief Decode color information + * \param outputCloud_arg output point cloud + * \param beginIdx_arg index indicating first point to be assigned with color information + * \param endIdx_arg index indicating last point to be assigned with color information + * \param rgba_offset_arg offset to color information + */ + void + decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) + { + assert (beginIdx_arg <= endIdx_arg); + + // amount of points to be decoded + unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); + + // get averaged color information for current voxel + unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++); + unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++); + unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++); + + // invert bit shifts during encoding + avgRed = static_cast (avgRed << colorBitReduction_); + avgGreen = static_cast (avgGreen << colorBitReduction_); + avgBlue = static_cast (avgBlue << colorBitReduction_); + + // iterate over points + for (std::size_t i = 0; i < pointCount; i++) + { + unsigned int colorInt; + if (pointCount > 1) + { + // get differential color information from input vector + unsigned char diffRed = static_cast (*(pointDiffColorDataVector_Iterator_++)); + unsigned char diffGreen = static_cast (*(pointDiffColorDataVector_Iterator_++)); + unsigned char diffBlue = static_cast (*(pointDiffColorDataVector_Iterator_++)); + + // invert bit shifts during encoding + diffRed = static_cast (diffRed << colorBitReduction_); + diffGreen = static_cast (diffGreen << colorBitReduction_); + diffBlue = static_cast (diffBlue << colorBitReduction_); + + // decode color information + colorInt = ((avgRed ^ diffRed) << 0) | + ((avgGreen ^ diffGreen) << 8) | + ((avgBlue ^ diffBlue) << 16); + } + else + { + // decode color information + colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16); + } + + char* idxPointPtr = reinterpret_cast (&outputCloud_arg->points[beginIdx_arg + i]); + int& pointColor = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + // assign color to point from point cloud + pointColor=colorInt; + } + } + + /** \brief Set default color to points + * \param outputCloud_arg output point cloud + * \param beginIdx_arg index indicating first point to be assigned with color information + * \param endIdx_arg index indicating last point to be assigned with color information + * \param rgba_offset_arg offset to color information + * */ + void + setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) + { + assert (beginIdx_arg <= endIdx_arg); + + // amount of points to be decoded + unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); + + // iterate over points + for (std::size_t i = 0; i < pointCount; i++) + { + char* idxPointPtr = reinterpret_cast (&outputCloud_arg->points[beginIdx_arg + i]); + int& pointColor = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + // assign color to point from point cloud + pointColor = defaultColor_; + } + } + + + protected: + + /** \brief Pointer to output point cloud dataset. */ + PointCloudPtr output_; + + /** \brief Vector for storing average color information */ + std::vector pointAvgColorDataVector_; + + /** \brief Iterator on average color information vector */ + std::vector::const_iterator pointAvgColorDataVector_Iterator_; + + /** \brief Vector for storing differential color information */ + std::vector pointDiffColorDataVector_; + + /** \brief Iterator on differential color information vector */ + std::vector::const_iterator pointDiffColorDataVector_Iterator_; + + /** \brief Amount of bits to be removed from color components before encoding */ + unsigned char colorBitReduction_; + + // frame header identifier + static const int defaultColor_; + + }; + + // define default color + template + const int ColorCoding::defaultColor_ = ((255) << 0) | + ((255) << 8) | + ((255) << 16); + + } +} + +#define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding; diff --git a/deps_install/include/pcl-1.10/pcl/compression/compression_profiles.h b/deps_install/include/pcl-1.10/pcl/compression/compression_profiles.h new file mode 100755 index 0000000..f822ce5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/compression_profiles.h @@ -0,0 +1,180 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (julius@kammerl.de) + */ + +#pragma once + +namespace pcl +{ + namespace io + { + + enum compression_Profiles_e + { + LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR, + LOW_RES_ONLINE_COMPRESSION_WITH_COLOR, + + MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR, + MED_RES_ONLINE_COMPRESSION_WITH_COLOR, + + HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR, + HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR, + + LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR, + LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR, + + MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR, + MED_RES_OFFLINE_COMPRESSION_WITH_COLOR, + + HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR, + HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR, + + COMPRESSION_PROFILE_COUNT, + MANUAL_CONFIGURATION + }; + + // compression configuration profile + struct configurationProfile_t + { + double pointResolution; + const double octreeResolution; + bool doVoxelGridDownSampling; + unsigned int iFrameRate; + const unsigned char colorBitResolution; + bool doColorEncoding; + }; + + // predefined configuration parameters + const struct configurationProfile_t compressionProfiles_[COMPRESSION_PROFILE_COUNT] = { + { + // PROFILE: LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR + 0.01, /* pointResolution = */ + 0.01, /* octreeResolution = */ + true, /* doVoxelGridDownDownSampling = */ + 50, /* iFrameRate = */ + 4, /* colorBitResolution = */ + false /* doColorEncoding = */ + }, { + // PROFILE: LOW_RES_ONLINE_COMPRESSION_WITH_COLOR + 0.01, /* pointResolution = */ + 0.01, /* octreeResolution = */ + true, /* doVoxelGridDownDownSampling = */ + 50, /* iFrameRate = */ + 4, /* colorBitResolution = */ + true /* doColorEncoding = */ + }, { + // PROFILE: MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR + 0.005, /* pointResolution = */ + 0.01, /* octreeResolution = */ + false, /* doVoxelGridDownDownSampling = */ + 40, /* iFrameRate = */ + 5, /* colorBitResolution = */ + false /* doColorEncoding = */ + }, { + // PROFILE: MED_RES_ONLINE_COMPRESSION_WITH_COLOR + 0.005, /* pointResolution = */ + 0.01, /* octreeResolution = */ + false, /* doVoxelGridDownDownSampling = */ + 40, /* iFrameRate = */ + 5, /* colorBitResolution = */ + true /* doColorEncoding = */ + }, { + // PROFILE: HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR + 0.0001, /* pointResolution = */ + 0.01, /* octreeResolution = */ + false, /* doVoxelGridDownDownSampling = */ + 30, /* iFrameRate = */ + 7, /* colorBitResolution = */ + false /* doColorEncoding = */ + }, { + // PROFILE: HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR + 0.0001, /* pointResolution = */ + 0.01, /* octreeResolution = */ + false, /* doVoxelGridDownDownSampling = */ + 30, /* iFrameRate = */ + 7, /* colorBitResolution = */ + true /* doColorEncoding = */ + }, { + // PROFILE: LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR + 0.01, /* pointResolution = */ + 0.01, /* octreeResolution = */ + true, /* doVoxelGridDownDownSampling = */ + 100, /* iFrameRate = */ + 4, /* colorBitResolution = */ + false /* doColorEncoding = */ + }, { + // PROFILE: LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR + 0.01, /* pointResolution = */ + 0.01, /* octreeResolution = */ + true, /* doVoxelGridDownDownSampling = */ + 100, /* iFrameRate = */ + 4, /* colorBitResolution = */ + true /* doColorEncoding = */ + }, { + // PROFILE: MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR + 0.005, /* pointResolution = */ + 0.005, /* octreeResolution = */ + true, /* doVoxelGridDownDownSampling = */ + 100, /* iFrameRate = */ + 5, /* colorBitResolution = */ + false /* doColorEncoding = */ + }, { + // PROFILE: MED_RES_OFFLINE_COMPRESSION_WITH_COLOR + 0.005, /* pointResolution = */ + 0.01, /* octreeResolution = */ + false, /* doVoxelGridDownDownSampling = */ + 100, /* iFrameRate = */ + 5, /* colorBitResolution = */ + true /* doColorEncoding = */ + }, { + // PROFILE: HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR + 0.0001, /* pointResolution = */ + 0.0001, /* octreeResolution = */ + true, /* doVoxelGridDownDownSampling = */ + 100, /* iFrameRate = */ + 8, /* colorBitResolution = */ + false /* doColorEncoding = */ + }, { + // PROFILE: HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR + 0.0001, /* pointResolution = */ + 0.01, /* octreeResolution = */ + false, /* doVoxelGridDownDownSampling = */ + 100, /* iFrameRate = */ + 8, /* colorBitResolution = */ + true /* doColorEncoding = */ + }}; + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/compression/entropy_range_coder.h b/deps_install/include/pcl-1.10/pcl/compression/entropy_range_coder.h new file mode 100755 index 0000000..34adb11 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/entropy_range_coder.h @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * Range Coder based on Dmitry Subbotin's carry-less implementation (http://www.compression.ru/ds/) + * Added optimized symbol lookup and added implementation for static range coding (uses fixed precomputed frequency table) + * + * Author: Julius Kammerl (julius@kammerl.de) + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace pcl +{ + + using std::uint8_t; + using std::uint32_t; + using std::uint64_t; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b AdaptiveRangeCoder compression class + * \note This class provides adaptive range coding functionality. + * \note Its symbol probability/frequency table is adaptively updated during encoding + * \note + * \author Julius Kammerl (julius@kammerl.de) + */ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + class AdaptiveRangeCoder + { + + public: + + /** \brief Empty constructor. */ + AdaptiveRangeCoder () + { + } + + /** \brief Empty deconstructor. */ + virtual + ~AdaptiveRangeCoder () + { + } + + /** \brief Encode char vector to output stream + * \param inputByteVector_arg input vector + * \param outputByteStream_arg output stream containing compressed data + * \return amount of bytes written to output stream + */ + unsigned long + encodeCharVectorToStream (const std::vector& inputByteVector_arg, std::ostream& outputByteStream_arg); + + /** \brief Decode char stream to output vector + * \param inputByteStream_arg input stream of compressed data + * \param outputByteVector_arg decompressed output vector + * \return amount of bytes read from input stream + */ + unsigned long + decodeStreamToCharVector (std::istream& inputByteStream_arg, std::vector& outputByteVector_arg); + + protected: + using DWord = std::uint32_t; // 4 bytes + + private: + /** vector containing compressed data + */ + std::vector outputCharVector_; + + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b StaticRangeCoder compression class + * \note This class provides static range coding functionality. + * \note Its symbol probability/frequency table is precomputed and encoded to the output stream + * \note + * \author Julius Kammerl (julius@kammerl.de) + */ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + class StaticRangeCoder + { + public: + /** \brief Constructor. */ + StaticRangeCoder () : + cFreqTable_ (65537) + { + } + + /** \brief Empty deconstructor. */ + virtual + ~StaticRangeCoder () + { + } + + /** \brief Encode integer vector to output stream + * \param[in] inputIntVector_arg input vector + * \param[out] outputByterStream_arg output stream containing compressed data + * \return amount of bytes written to output stream + */ + unsigned long + encodeIntVectorToStream (std::vector& inputIntVector_arg, std::ostream& outputByterStream_arg); + + /** \brief Decode stream to output integer vector + * \param inputByteStream_arg input stream of compressed data + * \param outputIntVector_arg decompressed output vector + * \return amount of bytes read from input stream + */ + unsigned long + decodeStreamToIntVector (std::istream& inputByteStream_arg, std::vector& outputIntVector_arg); + + /** \brief Encode char vector to output stream + * \param inputByteVector_arg input vector + * \param outputByteStream_arg output stream containing compressed data + * \return amount of bytes written to output stream + */ + unsigned long + encodeCharVectorToStream (const std::vector& inputByteVector_arg, std::ostream& outputByteStream_arg); + + /** \brief Decode char stream to output vector + * \param inputByteStream_arg input stream of compressed data + * \param outputByteVector_arg decompressed output vector + * \return amount of bytes read from input stream + */ + unsigned long + decodeStreamToCharVector (std::istream& inputByteStream_arg, std::vector& outputByteVector_arg); + + protected: + using DWord = std::uint32_t; // 4 bytes + + /** \brief Helper function to calculate the binary logarithm + * \param n_arg: some value + * \return binary logarithm (log2) of argument n_arg + */ + PCL_DEPRECATED("use std::log2 instead") + inline double + Log2 (double n_arg) + { + return std::log2 (n_arg); + } + + private: + /** \brief Vector containing cumulative symbol frequency table. */ + std::vector cFreqTable_; + + /** \brief Vector containing compressed data. */ + std::vector outputCharVector_; + + }; +} + + +//#include "impl/entropy_range_coder.hpp" diff --git a/deps_install/include/pcl-1.10/pcl/compression/libpng_wrapper.h b/deps_install/include/pcl-1.10/pcl/compression/libpng_wrapper.h new file mode 100755 index 0000000..e10d7ea --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/libpng_wrapper.h @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (julius@kammerl.de) + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace io + { + /** \brief Encodes 8-bit mono image to PNG format. + * \param[in] image_arg input image data + * \param[in] width_arg image width + * \param[in] height_arg image height + * \param[out] pngData_arg PNG compressed image data + * \param[in] png_level_arg zLib compression level (default level: -1) + * \ingroup io + */ + PCL_EXPORTS void + encodeMonoImageToPNG (std::vector& image_arg, + std::size_t width_arg, + std::size_t height_arg, + std::vector& pngData_arg, + int png_level_arg = -1); + + /** \brief Encodes 16-bit mono image to PNG format. + * \param[in] image_arg input image data + * \param[in] width_arg image width + * \param[in] height_arg image height + * \param[out] pngData_arg PNG compressed image data + * \param[in] png_level_arg zLib compression level (default level: -1) + * \ingroup io + */ + PCL_EXPORTS void + encodeMonoImageToPNG (std::vector& image_arg, + std::size_t width_arg, + std::size_t height_arg, + std::vector& pngData_arg, + int png_level_arg = -1); + + /** \brief Encodes 8-bit RGB image to PNG format. + * \param[in] image_arg input image data + * \param[in] width_arg image width + * \param[in] height_arg image height + * \param[out] pngData_arg PNG compressed image data + * \param[in] png_level_arg zLib compression level (default level: -1) + * \ingroup io + */ + PCL_EXPORTS void + encodeRGBImageToPNG (std::vector& image_arg, + std::size_t width_arg, + std::size_t height_arg, + std::vector& pngData_arg, + int png_level_arg = -1); + + /** \brief Encodes 16-bit RGB image to PNG format. + * \param[in] image_arg input image data + * \param[in] width_arg image width + * \param[in] height_arg image height + * \param[out] pngData_arg PNG compressed image data + * \param[in] png_level_arg zLib compression level (default level: -1) + * \ingroup io + */ + PCL_EXPORTS void + encodeRGBImageToPNG (std::vector& image_arg, + std::size_t width_arg, + std::size_t height_arg, + std::vector& pngData_arg, + int png_level_arg = -1); + + /** \brief Decode compressed PNG to 8-bit image + * \param[in] pngData_arg PNG compressed input data + * \param[in] imageData_arg image output data + * \param[out] width_arg image width + * \param[out] heigh_argt image height + * \param[out] channels_arg number of channels + * \ingroup io + */ + PCL_EXPORTS void + decodePNGToImage (std::vector& pngData_arg, + std::vector& imageData_arg, + std::size_t& width_arg, + std::size_t& heigh_argt, + unsigned int& channels_arg); + + /** \brief Decode compressed PNG to 16-bit image + * \param[in] pngData_arg PNG compressed input data + * \param[in] imageData_arg image output data + * \param[out] width_arg image width + * \param[out] height_arg image height + * \param[out] channels_arg number of channels + * \ingroup io + */ + PCL_EXPORTS void + decodePNGToImage (std::vector& pngData_arg, + std::vector& imageData_arg, + std::size_t& width_arg, + std::size_t& height_arg, + unsigned int& channels_arg); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/compression/octree_pointcloud_compression.h b/deps_install/include/pcl-1.10/pcl/compression/octree_pointcloud_compression.h new file mode 100755 index 0000000..fda475a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/octree_pointcloud_compression.h @@ -0,0 +1,312 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include "entropy_range_coder.h" +#include "color_coding.h" +#include "point_coding.h" + +#include "compression_profiles.h" + +#include +#include +#include +#include +#include + +using namespace pcl::octree; + +namespace pcl +{ + namespace io + { + /** \brief @b Octree pointcloud compression class + * \note This class enables compression and decompression of point cloud data based on octree data structures. + * \note + * \note typename: PointT: type of point used in pointcloud + * \author Julius Kammerl (julius@kammerl.de) + */ + template > + class OctreePointCloudCompression : public OctreePointCloud + { + public: + // public typedefs + using PointCloud = typename OctreePointCloud::PointCloud; + using PointCloudPtr = typename OctreePointCloud::PointCloudPtr; + using PointCloudConstPtr = typename OctreePointCloud::PointCloudConstPtr; + + // Boost shared pointers + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + using RealTimeStreamCompression = OctreePointCloudCompression >; + using SinglePointCloudCompressionLowMemory = OctreePointCloudCompression >; + + + /** \brief Constructor + * \param compressionProfile_arg: define compression profile + * \param octreeResolution_arg: octree resolution at lowest octree level + * \param pointResolution_arg: precision of point coordinates + * \param doVoxelGridDownDownSampling_arg: voxel grid filtering + * \param iFrameRate_arg: i-frame encoding rate + * \param doColorEncoding_arg: enable/disable color coding + * \param colorBitResolution_arg: color bit depth + * \param showStatistics_arg: output compression statistics + */ + OctreePointCloudCompression (compression_Profiles_e compressionProfile_arg = MED_RES_ONLINE_COMPRESSION_WITH_COLOR, + bool showStatistics_arg = false, + const double pointResolution_arg = 0.001, + const double octreeResolution_arg = 0.01, + bool doVoxelGridDownDownSampling_arg = false, + const unsigned int iFrameRate_arg = 30, + bool doColorEncoding_arg = true, + const unsigned char colorBitResolution_arg = 6) : + OctreePointCloud (octreeResolution_arg), + output_ (PointCloudPtr ()), + color_coder_ (), + point_coder_ (), + do_voxel_grid_enDecoding_ (doVoxelGridDownDownSampling_arg), i_frame_rate_ (iFrameRate_arg), + i_frame_counter_ (0), frame_ID_ (0), point_count_ (0), i_frame_ (true), + do_color_encoding_ (doColorEncoding_arg), cloud_with_color_ (false), data_with_color_ (false), + point_color_offset_ (0), b_show_statistics_ (showStatistics_arg), + compressed_point_data_len_ (), compressed_color_data_len_ (), selected_profile_(compressionProfile_arg), + point_resolution_(pointResolution_arg), octree_resolution_(octreeResolution_arg), + color_bit_resolution_(colorBitResolution_arg), + object_count_(0) + { + initialization(); + } + + /** \brief Empty deconstructor. */ + + ~OctreePointCloudCompression () + { + } + + /** \brief Initialize globals */ + void initialization () { + if (selected_profile_ != MANUAL_CONFIGURATION) + { + // apply selected compression profile + + // retrieve profile settings + const configurationProfile_t selectedProfile = compressionProfiles_[selected_profile_]; + + // apply profile settings + i_frame_rate_ = selectedProfile.iFrameRate; + do_voxel_grid_enDecoding_ = selectedProfile.doVoxelGridDownSampling; + this->setResolution (selectedProfile.octreeResolution); + point_coder_.setPrecision (static_cast (selectedProfile.pointResolution)); + do_color_encoding_ = selectedProfile.doColorEncoding; + color_coder_.setBitDepth (selectedProfile.colorBitResolution); + + } + else + { + // configure point & color coder + point_coder_.setPrecision (static_cast (point_resolution_)); + color_coder_.setBitDepth (color_bit_resolution_); + } + + if (point_coder_.getPrecision () == this->getResolution ()) + //disable differential point colding + do_voxel_grid_enDecoding_ = true; + + } + + /** \brief Add point at index from input pointcloud dataset to octree + * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added + */ + void + addPointIdx (const int pointIdx_arg) override + { + ++object_count_; + OctreePointCloud::addPointIdx(pointIdx_arg); + } + + /** \brief Provide a pointer to the output data set. + * \param cloud_arg: the boost shared pointer to a PointCloud message + */ + inline void + setOutputCloud (const PointCloudPtr &cloud_arg) + { + if (output_ != cloud_arg) + { + output_ = cloud_arg; + } + } + + /** \brief Get a pointer to the output point cloud dataset. + * \return pointer to pointcloud output class. + */ + inline PointCloudPtr + getOutputCloud () const + { + return (output_); + } + + /** \brief Encode point cloud to output stream + * \param cloud_arg: point cloud to be compressed + * \param compressed_tree_data_out_arg: binary output stream containing compressed data + */ + void + encodePointCloud (const PointCloudConstPtr &cloud_arg, std::ostream& compressed_tree_data_out_arg); + + /** \brief Decode point cloud from input stream + * \param compressed_tree_data_in_arg: binary input stream containing compressed data + * \param cloud_arg: reference to decoded point cloud + */ + void + decodePointCloud (std::istream& compressed_tree_data_in_arg, PointCloudPtr &cloud_arg); + + protected: + + /** \brief Write frame information to output stream + * \param compressed_tree_data_out_arg: binary output stream + */ + void + writeFrameHeader (std::ostream& compressed_tree_data_out_arg); + + /** \brief Read frame information to output stream + * \param compressed_tree_data_in_arg: binary input stream + */ + void + readFrameHeader (std::istream& compressed_tree_data_in_arg); + + /** \brief Synchronize to frame header + * \param compressed_tree_data_in_arg: binary input stream + */ + void + syncToHeader (std::istream& compressed_tree_data_in_arg); + + /** \brief Apply entropy encoding to encoded information and output to binary stream + * \param compressed_tree_data_out_arg: binary output stream + */ + void + entropyEncoding (std::ostream& compressed_tree_data_out_arg); + + /** \brief Entropy decoding of input binary stream and output to information vectors + * \param compressed_tree_data_in_arg: binary input stream + */ + void + entropyDecoding (std::istream& compressed_tree_data_in_arg); + + /** \brief Encode leaf node information during serialization + * \param leaf_arg: reference to new leaf node + * \param key_arg: octree key of new leaf node + */ + void + serializeTreeCallback (LeafT &leaf_arg, const OctreeKey& key_arg) override; + + /** \brief Decode leaf nodes information during deserialization + * \param key_arg octree key of new leaf node + */ + // param leaf_arg reference to new leaf node + void + deserializeTreeCallback (LeafT&, const OctreeKey& key_arg) override; + + + /** \brief Pointer to output point cloud dataset. */ + PointCloudPtr output_; + + /** \brief Vector for storing binary tree structure */ + std::vector binary_tree_data_vector_; + + /** \brief Iterator on binary tree structure vector */ + std::vector binary_color_tree_vector_; + + /** \brief Vector for storing points per voxel information */ + std::vector point_count_data_vector_; + + /** \brief Iterator on points per voxel vector */ + std::vector::const_iterator point_count_data_vector_iterator_; + + /** \brief Color coding instance */ + ColorCoding color_coder_; + + /** \brief Point coding instance */ + PointCoding point_coder_; + + /** \brief Static range coder instance */ + StaticRangeCoder entropy_coder_; + + bool do_voxel_grid_enDecoding_; + std::uint32_t i_frame_rate_; + std::uint32_t i_frame_counter_; + std::uint32_t frame_ID_; + std::uint64_t point_count_; + bool i_frame_; + + bool do_color_encoding_; + bool cloud_with_color_; + bool data_with_color_; + unsigned char point_color_offset_; + + //bool activating statistics + bool b_show_statistics_; + std::uint64_t compressed_point_data_len_; + std::uint64_t compressed_color_data_len_; + + // frame header identifier + static const char* frame_header_identifier_; + + const compression_Profiles_e selected_profile_; + const double point_resolution_; + const double octree_resolution_; + const unsigned char color_bit_resolution_; + + std::size_t object_count_; + + }; + + // define frame identifier + template + const char* OctreePointCloudCompression::frame_header_identifier_ = ""; + } + +} diff --git a/deps_install/include/pcl-1.10/pcl/compression/organized_pointcloud_compression.h b/deps_install/include/pcl-1.10/pcl/compression/organized_pointcloud_compression.h new file mode 100755 index 0000000..dac8be2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/organized_pointcloud_compression.h @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace pcl +{ + namespace io + { + /** \author Julius Kammerl (julius@kammerl.de) + */ + template + class OrganizedPointCloudCompression + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + /** \brief Empty Constructor. */ + OrganizedPointCloudCompression () + { + } + + /** \brief Empty deconstructor. */ + virtual ~OrganizedPointCloudCompression () + { + } + + /** \brief Encode point cloud to output stream + * \param[in] cloud_arg: point cloud to be compressed + * \param[out] compressedDataOut_arg: binary output stream containing compressed data + * \param[in] doColorEncoding: encode color information (if available) + * \param[in] convertToMono: convert rgb to mono + * \param[in] pngLevel_arg: png compression level (default compression: -1) + * \param[in] bShowStatistics_arg: show statistics + */ + void encodePointCloud (const PointCloudConstPtr &cloud_arg, + std::ostream& compressedDataOut_arg, + bool doColorEncoding = false, + bool convertToMono = false, + bool bShowStatistics_arg = true, + int pngLevel_arg = -1); + + /** \brief Encode raw disparity map and color image. + * \note Default values are configured according to the kinect/asus device specifications + * \param[in] disparityMap_arg: pointer to raw 16-bit disparity map + * \param[in] colorImage_arg: pointer to raw 8-bit rgb color image + * \param[in] width_arg: width of disparity map/color image + * \param[in] height_arg: height of disparity map/color image + * \param[out] compressedDataOut_arg: binary output stream containing compressed data + * \param[in] doColorEncoding: encode color information (if available) + * \param[in] convertToMono: convert rgb to mono + * \param[in] pngLevel_arg: png compression level (default compression: -1) + * \param[in] bShowStatistics_arg: show statistics + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + */ + void encodeRawDisparityMapWithColorImage ( std::vector& disparityMap_arg, + std::vector& colorImage_arg, + std::uint32_t width_arg, + std::uint32_t height_arg, + std::ostream& compressedDataOut_arg, + bool doColorEncoding = false, + bool convertToMono = false, + bool bShowStatistics_arg = true, + int pngLevel_arg = -1, + float focalLength_arg = 525.0f, + float disparityShift_arg = 174.825f, + float disparityScale_arg = -0.161175f); + + /** \brief Decode point cloud from input stream + * \param[in] compressedDataIn_arg: binary input stream containing compressed data + * \param[out] cloud_arg: reference to decoded point cloud + * \param[in] bShowStatistics_arg: show compression statistics during decoding + * \return false if an I/O error occurred. + */ + bool decodePointCloud (std::istream& compressedDataIn_arg, + PointCloudPtr &cloud_arg, + bool bShowStatistics_arg = true); + + protected: + /** \brief Analyze input point cloud and calculate the maximum depth and focal length + * \param[in] cloud_arg: input point cloud + * \param[out] maxDepth_arg: calculated maximum depth + * \param[out] focalLength_arg: estimated focal length + */ + void analyzeOrganizedCloud (PointCloudConstPtr cloud_arg, + float& maxDepth_arg, + float& focalLength_arg) const; + + private: + // frame header identifier + static const char* frameHeaderIdentifier_; + + // + openni_wrapper::ShiftToDepthConverter sd_converter_; + }; + + // define frame identifier + template + const char* OrganizedPointCloudCompression::frameHeaderIdentifier_ = ""; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/compression/organized_pointcloud_conversion.h b/deps_install/include/pcl-1.10/pcl/compression/organized_pointcloud_conversion.h new file mode 100755 index 0000000..541d725 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/organized_pointcloud_conversion.h @@ -0,0 +1,569 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * Authors: Julius Kammerl (julius@kammerl.de) + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace pcl +{ + namespace io + { + + template struct CompressionPointTraits + { + static const bool hasColor = false; + static const unsigned int channels = 1; + static const std::size_t bytesPerPoint = 3 * sizeof(float); + }; + + template<> + struct CompressionPointTraits + { + static const bool hasColor = true; + static const unsigned int channels = 4; + static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t); + }; + + template<> + struct CompressionPointTraits + { + static const bool hasColor = true; + static const unsigned int channels = 4; + static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t); + }; + + ////////////////////////////////////////////////////////////////////////////////////////////// + template ::hasColor > + struct OrganizedConversion; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // Uncolored point cloud specialization + ////////////////////////////////////////////////////////////////////////////////////////////// + template + struct OrganizedConversion + { + /** \brief Convert point cloud to disparity image + * \param[in] cloud_arg input point cloud + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[out] disparityData_arg output disparity image + * \ingroup io + */ + static void convert(const pcl::PointCloud& cloud_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + bool , + typename std::vector& disparityData_arg, + typename std::vector&) + { + std::size_t cloud_size = cloud_arg.points.size (); + + // Clear image data + disparityData_arg.clear (); + + disparityData_arg.reserve (cloud_size); + + for (std::size_t i = 0; i < cloud_size; ++i) + { + // Get point from cloud + const PointT& point = cloud_arg.points[i]; + + if (pcl::isFinite (point)) + { + // Inverse depth quantization + std::uint16_t disparity = static_cast ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); + disparityData_arg.push_back (disparity); + } + else + { + // Non-valid points are encoded with zeros + disparityData_arg.push_back (0); + } + } + } + + /** \brief Convert disparity image to point cloud + * \param[in] disparityData_arg input depth image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& disparityData_arg, + typename std::vector&, + bool, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg * height_arg; + + assert(disparityData_arg.size()==cloud_size); + + // Reset point cloud + cloud_arg.points.clear (); + cloud_arg.points.reserve (cloud_size); + + // Define point cloud parameters + cloud_arg.width = static_cast (width_arg); + cloud_arg.height = static_cast (height_arg); + cloud_arg.is_dense = false; + + // Calculate center of disparity image + int centerX = static_cast (width_arg / 2); + int centerY = static_cast (height_arg / 2); + + const float fl_const = 1.0f / focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); + + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + PointT newPoint; + const std::uint16_t& pixel_disparity = disparityData_arg[i]; + ++i; + + if (pixel_disparity) + { + // Inverse depth decoding + float depth = focalLength_arg / (static_cast (pixel_disparity) * disparityScale_arg + disparityShift_arg); + + // Generate new points + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; + newPoint.z = depth; + + } + else + { + // Generate bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + } + + cloud_arg.points.push_back (newPoint); + } + + } + + + /** \brief Convert disparity image to point cloud + * \param[in] depthData_arg input depth image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& depthData_arg, + typename std::vector&, + bool, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg * height_arg; + + assert(depthData_arg.size()==cloud_size); + + // Reset point cloud + cloud_arg.points.clear (); + cloud_arg.points.reserve (cloud_size); + + // Define point cloud parameters + cloud_arg.width = static_cast (width_arg); + cloud_arg.height = static_cast (height_arg); + cloud_arg.is_dense = false; + + // Calculate center of disparity image + int centerX = static_cast (width_arg / 2); + int centerY = static_cast (height_arg / 2); + + const float fl_const = 1.0f / focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); + + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + PointT newPoint; + const float& pixel_depth = depthData_arg[i]; + ++i; + + if (pixel_depth) + { + // Inverse depth decoding + float depth = focalLength_arg / pixel_depth; + + // Generate new points + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; + newPoint.z = depth; + + } + else + { + // Generate bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + } + + cloud_arg.points.push_back (newPoint); + } + + } + + }; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // Colored point cloud specialization + ////////////////////////////////////////////////////////////////////////////////////////////// + template + struct OrganizedConversion + { + /** \brief Convert point cloud to disparity image and rgb image + * \param[in] cloud_arg input point cloud + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[in] convertToMono convert color to mono/grayscale + * \param[out] disparityData_arg output disparity image + * \param[out] rgbData_arg output rgb image + * \ingroup io + */ + static void convert(const pcl::PointCloud& cloud_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + bool convertToMono, + typename std::vector& disparityData_arg, + typename std::vector& rgbData_arg) + { + std::size_t cloud_size = cloud_arg.points.size (); + + // Reset output vectors + disparityData_arg.clear (); + rgbData_arg.clear (); + + // Allocate memory + disparityData_arg.reserve (cloud_size); + if (convertToMono) + { + rgbData_arg.reserve (cloud_size); + } else + { + rgbData_arg.reserve (cloud_size * 3); + } + + + for (std::size_t i = 0; i < cloud_size; ++i) + { + const PointT& point = cloud_arg.points[i]; + + if (pcl::isFinite (point)) + { + if (convertToMono) + { + // Encode point color + std::uint8_t grayvalue = static_cast(0.2989 * point.r + + 0.5870 * point.g + + 0.1140 * point.b); + + rgbData_arg.push_back (grayvalue); + } else + { + // Encode point color + rgbData_arg.push_back (point.r); + rgbData_arg.push_back (point.g); + rgbData_arg.push_back (point.b); + } + + + // Inverse depth quantization + std::uint16_t disparity = static_cast (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); + + // Encode disparity + disparityData_arg.push_back (disparity); + } + else + { + + // Encode black point + if (convertToMono) + { + rgbData_arg.push_back (0); + } else + { + rgbData_arg.push_back (0); + rgbData_arg.push_back (0); + rgbData_arg.push_back (0); + } + + // Encode bad point + disparityData_arg.push_back (0); + } + } + + } + + /** \brief Convert disparity image to point cloud + * \param[in] disparityData_arg output disparity image + * \param[in] rgbData_arg output rgb image + * \param[in] monoImage_arg input image is a single-channel mono image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& disparityData_arg, + typename std::vector& rgbData_arg, + bool monoImage_arg, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg*height_arg; + bool hasColor = (!rgbData_arg.empty ()); + + // Check size of input data + assert (disparityData_arg.size()==cloud_size); + if (hasColor) + { + if (monoImage_arg) + { + assert (rgbData_arg.size()==cloud_size); + } else + { + assert (rgbData_arg.size()==cloud_size*3); + } + } + + // Reset point cloud + cloud_arg.points.clear(); + cloud_arg.points.reserve(cloud_size); + + // Define point cloud parameters + cloud_arg.width = static_cast(width_arg); + cloud_arg.height = static_cast(height_arg); + cloud_arg.is_dense = false; + + // Calculate center of disparity image + int centerX = static_cast(width_arg/2); + int centerY = static_cast(height_arg/2); + + const float fl_const = 1.0f/focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); + + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + PointT newPoint; + + const std::uint16_t& pixel_disparity = disparityData_arg[i]; + + if (pixel_disparity && (pixel_disparity!=0x7FF)) + { + float depth = focalLength_arg / (static_cast (pixel_disparity) * disparityScale_arg + disparityShift_arg); + + // Define point location + newPoint.z = depth; + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; + + if (hasColor) + { + if (monoImage_arg) + { + // Define point color + newPoint.r = rgbData_arg[i]; + newPoint.g = rgbData_arg[i]; + newPoint.b = rgbData_arg[i]; + } else + { + // Define point color + newPoint.r = rgbData_arg[i*3+0]; + newPoint.g = rgbData_arg[i*3+1]; + newPoint.b = rgbData_arg[i*3+2]; + } + + } else + { + // Set white point color + newPoint.rgba = 0xffffffffu; + } + } else + { + // Define bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + newPoint.rgb = 0.0f; + } + + // Add point to cloud + cloud_arg.points.push_back(newPoint); + // Increment point iterator + ++i; + } + } + + /** \brief Convert disparity image to point cloud + * \param[in] depthData_arg output disparity image + * \param[in] rgbData_arg output rgb image + * \param[in] monoImage_arg input image is a single-channel mono image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& depthData_arg, + typename std::vector& rgbData_arg, + bool monoImage_arg, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg*height_arg; + bool hasColor = (!rgbData_arg.empty ()); + + // Check size of input data + assert (depthData_arg.size()==cloud_size); + if (hasColor) + { + if (monoImage_arg) + { + assert (rgbData_arg.size()==cloud_size); + } else + { + assert (rgbData_arg.size()==cloud_size*3); + } + } + + // Reset point cloud + cloud_arg.points.clear(); + cloud_arg.points.reserve(cloud_size); + + // Define point cloud parameters + cloud_arg.width = static_cast(width_arg); + cloud_arg.height = static_cast(height_arg); + cloud_arg.is_dense = false; + + // Calculate center of disparity image + int centerX = static_cast(width_arg/2); + int centerY = static_cast(height_arg/2); + + const float fl_const = 1.0f/focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); + + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + PointT newPoint; + + const float& pixel_depth = depthData_arg[i]; + + if (pixel_depth==pixel_depth) + { + float depth = focalLength_arg / pixel_depth; + + // Define point location + newPoint.z = depth; + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; + + if (hasColor) + { + if (monoImage_arg) + { + // Define point color + newPoint.r = rgbData_arg[i]; + newPoint.g = rgbData_arg[i]; + newPoint.b = rgbData_arg[i]; + } else + { + // Define point color + newPoint.r = rgbData_arg[i*3+0]; + newPoint.g = rgbData_arg[i*3+1]; + newPoint.b = rgbData_arg[i*3+2]; + } + + } else + { + // Set white point color + newPoint.rgba = 0xffffffffu; + } + } else + { + // Define bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + newPoint.rgb = 0.0f; + } + + // Add point to cloud + cloud_arg.points.push_back(newPoint); + // Increment point iterator + ++i; + } + } + }; + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/compression/point_coding.h b/deps_install/include/pcl-1.10/pcl/compression/point_coding.h new file mode 100755 index 0000000..fe5041a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/compression/point_coding.h @@ -0,0 +1,205 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace octree + { + /** \brief @b PointCoding class + * \note This class encodes 8-bit differential point information for octree-based point cloud compression. + * \note typename: PointT: type of point used in pointcloud + * \author Julius Kammerl (julius@kammerl.de) + */ + template + class PointCoding + { + // public typedefs + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + /** \brief Constructor. */ + PointCoding () : + output_ (), + pointCompressionResolution_ (0.001f) // 1mm + { + } + + /** \brief Empty class constructor. */ + virtual + ~PointCoding () + { + } + + /** \brief Define precision of point information + * \param precision_arg: precision + */ + inline void + setPrecision (float precision_arg) + { + pointCompressionResolution_ = precision_arg; + } + + /** \brief Retrieve precision of point information + * \return precision + */ + inline float + getPrecision () + { + return (pointCompressionResolution_); + } + + /** \brief Set amount of points within point cloud to be encoded and reserve memory + * \param pointCount_arg: amounts of points within point cloud + */ + inline void + setPointCount (unsigned int pointCount_arg) + { + pointDiffDataVector_.reserve (pointCount_arg * 3); + } + + /** \brief Initialize encoding of differential point */ + void + initializeEncoding () + { + pointDiffDataVector_.clear (); + } + + /** \brief Initialize decoding of differential point */ + void + initializeDecoding () + { + pointDiffDataVectorIterator_ = pointDiffDataVector_.begin (); + } + + /** \brief Get reference to vector containing differential color data */ + std::vector& + getDifferentialDataVector () + { + return (pointDiffDataVector_); + } + + /** \brief Encode differential point information for a subset of points from point cloud + * \param indexVector_arg indices defining a subset of points from points cloud + * \param referencePoint_arg coordinates of reference point + * \param inputCloud_arg input point cloud + */ + void + encodePoints (const typename std::vector& indexVector_arg, const double* referencePoint_arg, + PointCloudConstPtr inputCloud_arg) + { + std::size_t len = indexVector_arg.size (); + + // iterate over points within current voxel + for (std::size_t i = 0; i < len; i++) + { + unsigned char diffX, diffY, diffZ; + + // retrieve point from cloud + const int& idx = indexVector_arg[i]; + const PointT& idxPoint = inputCloud_arg->points[idx]; + + // differentially encode point coordinates and truncate overflow + diffX = static_cast (max (-127, min(127, static_cast ((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_)))); + diffY = static_cast (max (-127, min(127, static_cast ((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_)))); + diffZ = static_cast (max (-127, min(127, static_cast ((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_)))); + + // store information in differential point vector + pointDiffDataVector_.push_back (diffX); + pointDiffDataVector_.push_back (diffY); + pointDiffDataVector_.push_back (diffZ); + } + } + + /** \brief Decode differential point information + * \param outputCloud_arg output point cloud + * \param referencePoint_arg coordinates of reference point + * \param beginIdx_arg index indicating first point to be assigned with color information + * \param endIdx_arg index indicating last point to be assigned with color information + */ + void + decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg, + std::size_t endIdx_arg) + { + assert (beginIdx_arg <= endIdx_arg); + + unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); + + // iterate over points within current voxel + for (std::size_t i = 0; i < pointCount; i++) + { + // retrieve differential point information + const unsigned char& diffX = static_cast (*(pointDiffDataVectorIterator_++)); + const unsigned char& diffY = static_cast (*(pointDiffDataVectorIterator_++)); + const unsigned char& diffZ = static_cast (*(pointDiffDataVectorIterator_++)); + + // retrieve point from point cloud + PointT& point = outputCloud_arg->points[beginIdx_arg + i]; + + // decode point position + point.x = static_cast (referencePoint_arg[0] + diffX * pointCompressionResolution_); + point.y = static_cast (referencePoint_arg[1] + diffY * pointCompressionResolution_); + point.z = static_cast (referencePoint_arg[2] + diffZ * pointCompressionResolution_); + } + } + + protected: + /** \brief Pointer to output point cloud dataset. */ + PointCloudPtr output_; + + /** \brief Vector for storing differential point information */ + std::vector pointDiffDataVector_; + + /** \brief Iterator on differential point information vector */ + std::vector::const_iterator pointDiffDataVectorIterator_; + + /** \brief Precision of point coding*/ + float pointCompressionResolution_; + }; + } +} + +#define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding; diff --git a/deps_install/include/pcl-1.10/pcl/console/parse.h b/deps_install/include/pcl-1.10/pcl/console/parse.h new file mode 100755 index 0000000..04d0e28 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/console/parse.h @@ -0,0 +1,380 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace console + { + /** \brief Finds whether the argument with name "argument_name" is in the argument list "argv". + * An example for a widely used switch argument is the "-r" flag for unix commands that indicates whether + * the command should run recursively or not. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] argument_name the string value to search for + * \return true if argument was found, false otherwise + * \note find_switch is simply returning find_argument != -1. + */ + PCL_EXPORTS bool + find_switch (int argc, const char * const * argv, const char * argument_name); + + /** \brief Finds the position of the argument with name "argument_name" in the argument list "argv" + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] argument_name the string value to search for + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + find_argument (int argc, const char * const * argv, const char * argument_name); + + /** \brief Template version for parsing arguments. Template parameter needs to have input stream operator overloaded! + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] argument_name the name of the argument to search for + * \param[out] value The value of the argument + * \return index of found argument or -1 if arguments do not appear in list + */ + template int + parse (int argc, const char * const * argv, const char * argument_name, Type& value) + { + int index = find_argument (argc, argv, argument_name) + 1; + + if (index > 0 && index < argc) + { + std::istringstream stream; + stream.clear (); + stream.str (argv[index]); + stream >> value; + } + + return (index - 1); + } + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, std::string &val); + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, bool &val); + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, float &val); + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, double &val); + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, int &val); + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, unsigned int &val); + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, long int &val) noexcept; + + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, char &val); + + /** \brief Parse for specific given command line arguments (2x values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] f the first output value + * \param[out] s the second output value + * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_2x_arguments (int argc, const char * const * argv, const char * str, float &f, float &s, bool debug = true); + + /** \brief Parse for specific given command line arguments (2x values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] f the first output value + * \param[out] s the second output value + * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_2x_arguments (int argc, const char * const * argv, const char * str, double &f, double &s, bool debug = true); + + /** \brief Parse for specific given command line arguments (2x values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] f the first output value + * \param[out] s the second output value + * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_2x_arguments (int argc, const char * const * argv, const char * str, int &f, int &s, bool debug = true); + + /** \brief Parse for specific given command line arguments (3x values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] f the first output value + * \param[out] s the second output value + * \param[out] t the third output value + * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_3x_arguments (int argc, const char * const * argv, const char * str, float &f, float &s, float &t, bool debug = true); + + /** \brief Parse for specific given command line arguments (3x values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] f the first output value + * \param[out] s the second output value + * \param[out] t the third output value + * \param[in] debug whether to print debug info or not + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_3x_arguments (int argc, const char * const * argv, const char * str, double &f, double &s, double &t, bool debug = true); + + /** \brief Parse for specific given command line arguments (3x values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] f the first output value + * \param[out] s the second output value + * \param[out] t the third output value + * \param[in] debug whether to print debug info or not + * return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_3x_arguments (int argc, const char * const * argv, const char * str, int &f, int &s, int &t, bool debug = true); + + /** \brief Parse for specific given command line arguments (3x values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] v the vector into which the parsed values will be copied + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_x_arguments (int argc, const char * const * argv, const char * str, std::vector& v); + + /** \brief Parse for specific given command line arguments (N values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] v the vector into which the parsed values will be copied + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_x_arguments (int argc, const char * const * argv, const char * str, std::vector& v); + + /** \brief Parse for specific given command line arguments (N values comma + * separated). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] v the vector into which the parsed values will be copied + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_x_arguments (int argc, const char * const * argv, const char * str, std::vector& v); + + /** \brief Parse for specific given command line arguments (multiple occurrences + * of the same command line parameter). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] values the resultant output values + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS bool + parse_multiple_arguments (int argc, const char * const * argv, const char * str, std::vector &values); + + /** \brief Parse for specific given command line arguments (multiple occurrences + * of the same command line parameter). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] values the resultant output values + * \return true if found, false otherwise + */ + PCL_EXPORTS bool + parse_multiple_arguments (int argc, const char * const * argv, const char * str, std::vector &values); + + /** \brief Parse for specific given command line arguments (multiple occurrences + * of the same command line parameter). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] values the resultant output values + * \return true if found, false otherwise + */ + PCL_EXPORTS bool + parse_multiple_arguments (int argc, const char * const * argv, const char * str, std::vector &values); + + /** \brief Parse for a specific given command line argument (multiple occurrences + * of the same command line parameter). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] values the resultant output values + * \return true if found, false otherwise + */ + PCL_EXPORTS bool + parse_multiple_arguments (int argc, const char * const * argv, const char * str, std::vector &values); + + /** \brief Parse command line arguments for file names with given extension (multiple occurrences + * of 2x argument groups, separated by commas). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] values_f the first vector of output values + * \param[out] values_s the second vector of output values + * \return true if found, false otherwise + */ + PCL_EXPORTS bool + parse_multiple_2x_arguments (int argc, const char * const * argv, const char * str, + std::vector &values_f, + std::vector &values_s); + + /** \brief Parse command line arguments for file names with given extension (multiple occurrences + * of 3x argument groups, separated by commas). + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the command line argument to search for + * \param[out] values_f the first vector of output values + * \param[out] values_s the second vector of output values + * \param[out] values_t the third vector of output values + * \return true if found, false otherwise + */ + PCL_EXPORTS bool + parse_multiple_3x_arguments (int argc, const char * const * argv, const char * str, + std::vector &values_f, + std::vector &values_s, + std::vector &values_t); + + /** \brief Parse command line arguments for file names with given extension vector + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] extensions the extensions to search for + * \return a vector with file names indices + */ + PCL_EXPORTS std::vector + parse_file_extension_argument (int argc, const char * const * argv, + const std::vector &extensions); + + /** \brief Parse command line arguments for file names with given extension + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] ext the extension to search for + * \return a vector with file names indices + */ + PCL_EXPORTS std::vector + parse_file_extension_argument (int argc, const char * const * argv, const std::string &ext); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/console/print.h b/deps_install/include/pcl-1.10/pcl/console/print.h new file mode 100755 index 0000000..5961771 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/console/print.h @@ -0,0 +1,260 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +#include +#include + +#define PCL_ALWAYS(...) pcl::console::print (pcl::console::L_ALWAYS, __VA_ARGS__) +#define PCL_ERROR(...) pcl::console::print (pcl::console::L_ERROR, __VA_ARGS__) +#define PCL_WARN(...) pcl::console::print (pcl::console::L_WARN, __VA_ARGS__) +#define PCL_INFO(...) pcl::console::print (pcl::console::L_INFO, __VA_ARGS__) +#define PCL_DEBUG(...) pcl::console::print (pcl::console::L_DEBUG, __VA_ARGS__) +#define PCL_VERBOSE(...) pcl::console::print (pcl::console::L_VERBOSE, __VA_ARGS__) + +#define PCL_ASSERT_ERROR_PRINT_CHECK(pred, msg) \ + do \ + { \ + if (!(pred)) \ + { \ + PCL_ERROR(msg); \ + PCL_ERROR("In File %s, in line %d\n" __FILE__, __LINE__); \ + } \ + } while (0) + +#define PCL_ASSERT_ERROR_PRINT_RETURN(pred, msg, err) \ + do \ + { \ + PCL_ASSERT_ERROR_PRINT_CHECK(pred, msg); \ + if (!(pred)) return err; \ + } while (0) + +namespace pcl +{ + namespace console + { + enum TT_ATTIBUTES + { + TT_RESET = 0, + TT_BRIGHT = 1, + TT_DIM = 2, + TT_UNDERLINE = 3, + TT_BLINK = 4, + TT_REVERSE = 7, + TT_HIDDEN = 8 + }; + + enum TT_COLORS + { + TT_BLACK, + TT_RED, + TT_GREEN, + TT_YELLOW, + TT_BLUE, + TT_MAGENTA, + TT_CYAN, + TT_WHITE + }; + + enum VERBOSITY_LEVEL + { + L_ALWAYS, + L_ERROR, + L_WARN, + L_INFO, + L_DEBUG, + L_VERBOSE + }; + + /** set the verbosity level */ + PCL_EXPORTS void + setVerbosityLevel (VERBOSITY_LEVEL level); + + /** get the verbosity level. */ + PCL_EXPORTS VERBOSITY_LEVEL + getVerbosityLevel (); + + /** initialize verbosity level. */ + PCL_EXPORTS bool + initVerbosityLevel (); + + /** is verbosity level enabled? */ + PCL_EXPORTS bool + isVerbosityLevelEnabled (VERBOSITY_LEVEL severity); + + /** \brief Enable or disable colored text output, overriding the default behavior. + * + * By default, colored output is enabled for interactive terminals or when the environment + * variable PCL_CLICOLOR_FORCE is set. + * + * \param stream the output stream (stdout, stderr, etc) + * \param enable whether to emit color codes when calling any of the color related methods + */ + PCL_EXPORTS void + enableColoredOutput (FILE *stream, bool enable); + + /** \brief Change the text color (on either stdout or stderr) with an attr:fg:bg + * \param stream the output stream (stdout, stderr, etc) + * \param attribute the text attribute + * \param fg the foreground color + * \param bg the background color + */ + PCL_EXPORTS void + change_text_color (FILE *stream, int attribute, int fg, int bg); + + /** \brief Change the text color (on either stdout or stderr) with an attr:fg + * \param stream the output stream (stdout, stderr, etc) + * \param attribute the text attribute + * \param fg the foreground color + */ + PCL_EXPORTS void + change_text_color (FILE *stream, int attribute, int fg); + + /** \brief Reset the text color (on either stdout or stderr) to its original state + * \param stream the output stream (stdout, stderr, etc) + */ + PCL_EXPORTS void + reset_text_color (FILE *stream); + + /** \brief Print a message on stream with colors + * \param stream the output stream (stdout, stderr, etc) + * \param attr the text attribute + * \param fg the foreground color + * \param format the message + */ + PCL_EXPORTS void + print_color (FILE *stream, int attr, int fg, const char *format, ...); + + /** \brief Print an info message on stream with colors + * \param format the message + */ + PCL_EXPORTS void + print_info (const char *format, ...); + + /** \brief Print an info message on stream with colors + * \param stream the output stream (stdout, stderr, etc) + * \param format the message + */ + PCL_EXPORTS void + print_info (FILE *stream, const char *format, ...); + + /** \brief Print a highlighted info message on stream with colors + * \param format the message + */ + PCL_EXPORTS void + print_highlight (const char *format, ...); + + /** \brief Print a highlighted info message on stream with colors + * \param stream the output stream (stdout, stderr, etc) + * \param format the message + */ + PCL_EXPORTS void + print_highlight (FILE *stream, const char *format, ...); + + /** \brief Print an error message on stream with colors + * \param format the message + */ + PCL_EXPORTS void + print_error (const char *format, ...); + + /** \brief Print an error message on stream with colors + * \param stream the output stream (stdout, stderr, etc) + * \param format the message + */ + PCL_EXPORTS void + print_error (FILE *stream, const char *format, ...); + + /** \brief Print a warning message on stream with colors + * \param format the message + */ + PCL_EXPORTS void + print_warn (const char *format, ...); + + /** \brief Print a warning message on stream with colors + * \param stream the output stream (stdout, stderr, etc) + * \param format the message + */ + PCL_EXPORTS void + print_warn (FILE *stream, const char *format, ...); + + /** \brief Print a debug message on stream with colors + * \param format the message + */ + PCL_EXPORTS void + print_debug (const char *format, ...); + + /** \brief Print a debug message on stream with colors + * \param stream the output stream (stdout, stderr, etc) + * \param format the message + */ + PCL_EXPORTS void + print_debug (FILE *stream, const char *format, ...); + + + /** \brief Print a value message on stream with colors + * \param format the message + */ + PCL_EXPORTS void + print_value (const char *format, ...); + + /** \brief Print a value message on stream with colors + * \param stream the output stream (stdout, stderr, etc) + * \param format the message + */ + PCL_EXPORTS void + print_value (FILE *stream, const char *format, ...); + + /** \brief Print a message on stream + * \param level the verbosity level + * \param stream the output stream (stdout, stderr, etc) + * \param format the message + */ + PCL_EXPORTS void + print (VERBOSITY_LEVEL level, FILE *stream, const char *format, ...); + + /** \brief Print a message + * \param level the verbosity level + * \param format the message + */ + PCL_EXPORTS void + print (VERBOSITY_LEVEL level, const char *format, ...); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/console/time.h b/deps_install/include/pcl-1.10/pcl/console/time.h new file mode 100755 index 0000000..0794532 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/console/time.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + namespace console + { + class TicToc + { + public: + + TicToc () {} + + void + tic () + { + tictic_ = std::chrono::steady_clock::now(); + }; + + inline double + toc () const + { + auto end_time = std::chrono::steady_clock::now(); + return std::chrono::duration>(end_time - tictic_).count(); + }; + + inline void + toc_print () const + { + double milliseconds = toc (); + //int minutes = (int) std::floor ( seconds / 60.0 ); + //seconds -= minutes * 60.0; + //if (minutes != 0) + //{ + // print_value ("%i", minutes); + // print_info (" minutes, "); + //} + print_value ("%g", milliseconds); + print_info (" ms\n"); + }; + + private: + std::chrono::time_point tictic_; + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/conversions.h b/deps_install/include/pcl-1.10/pcl/conversions.h new file mode 100755 index 0000000..041b720 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/conversions.h @@ -0,0 +1,347 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#ifndef Q_MOC_RUN +#include +#endif + +namespace pcl +{ + namespace detail + { + // For converting template point cloud to message. + template + struct FieldAdder + { + FieldAdder (std::vector& fields) : fields_ (fields) {}; + + template void operator() () + { + pcl::PCLPointField f; + f.name = traits::name::value; + f.offset = traits::offset::value; + f.datatype = traits::datatype::value; + f.count = traits::datatype::size; + fields_.push_back (f); + } + + std::vector& fields_; + }; + + // For converting message to template point cloud. + template + struct FieldMapper + { + FieldMapper (const std::vector& fields, + std::vector& map) + : fields_ (fields), map_ (map) + { + } + + template void + operator () () + { + for (const auto& field : fields_) + { + if (FieldMatches()(field)) + { + FieldMapping mapping; + mapping.serialized_offset = field.offset; + mapping.struct_offset = traits::offset::value; + mapping.size = sizeof (typename traits::datatype::type); + map_.push_back (mapping); + return; + } + } + // Disable thrown exception per #595: http://dev.pointclouds.org/issues/595 + PCL_WARN ("Failed to find match for field '%s'.\n", traits::name::value); + //throw pcl::InvalidConversionException (ss.str ()); + } + + const std::vector& fields_; + std::vector& map_; + }; + + inline bool + fieldOrdering (const FieldMapping& a, const FieldMapping& b) + { + return (a.serialized_offset < b.serialized_offset); + } + + } //namespace detail + + template void + createMapping (const std::vector& msg_fields, MsgFieldMap& field_map) + { + // Create initial 1-1 mapping between serialized data segments and struct fields + detail::FieldMapper mapper (msg_fields, field_map); + for_each_type< typename traits::fieldList::type > (mapper); + + // Coalesce adjacent fields into single memcpy's where possible + if (field_map.size() > 1) + { + std::sort(field_map.begin(), field_map.end(), detail::fieldOrdering); + MsgFieldMap::iterator i = field_map.begin(), j = i + 1; + while (j != field_map.end()) + { + // This check is designed to permit padding between adjacent fields. + /// @todo One could construct a pathological case where the struct has a + /// field where the serialized data has padding + if (j->serialized_offset - i->serialized_offset == j->struct_offset - i->struct_offset) + { + i->size += (j->struct_offset + j->size) - (i->struct_offset + i->size); + j = field_map.erase(j); + } + else + { + ++i; + ++j; + } + } + } + } + + /** \brief Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map. + * \param[in] msg the PCLPointCloud2 binary blob + * \param[out] cloud the resultant pcl::PointCloud + * \param[in] field_map a MsgFieldMap object + * + * \note Use fromPCLPointCloud2 (PCLPointCloud2, PointCloud) directly or create you + * own MsgFieldMap using: + * + * \code + * MsgFieldMap field_map; + * createMapping (msg.fields, field_map); + * \endcode + */ + template void + fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud& cloud, + const MsgFieldMap& field_map) + { + // Copy info fields + cloud.header = msg.header; + cloud.width = msg.width; + cloud.height = msg.height; + cloud.is_dense = msg.is_dense == 1; + + // Copy point data + std::uint32_t num_points = msg.width * msg.height; + cloud.points.resize (num_points); + std::uint8_t* cloud_data = reinterpret_cast(&cloud.points[0]); + + // Check if we can copy adjacent points in a single memcpy. We can do so if there + // is exactly one field to copy and it is the same size as the source and destination + // point types. + if (field_map.size() == 1 && + field_map[0].serialized_offset == 0 && + field_map[0].struct_offset == 0 && + field_map[0].size == msg.point_step && + field_map[0].size == sizeof(PointT)) + { + std::uint32_t cloud_row_step = static_cast (sizeof (PointT) * cloud.width); + const std::uint8_t* msg_data = &msg.data[0]; + // Should usually be able to copy all rows at once + if (msg.row_step == cloud_row_step) + { + memcpy (cloud_data, msg_data, msg.data.size ()); + } + else + { + for (std::uint32_t i = 0; i < msg.height; ++i, cloud_data += cloud_row_step, msg_data += msg.row_step) + memcpy (cloud_data, msg_data, cloud_row_step); + } + + } + else + { + // If not, memcpy each group of contiguous fields separately + for (std::uint32_t row = 0; row < msg.height; ++row) + { + const std::uint8_t* row_data = &msg.data[row * msg.row_step]; + for (std::uint32_t col = 0; col < msg.width; ++col) + { + const std::uint8_t* msg_data = row_data + col * msg.point_step; + for (const detail::FieldMapping& mapping : field_map) + { + memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size); + } + cloud_data += sizeof (PointT); + } + } + } + } + + /** \brief Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object. + * \param[in] msg the PCLPointCloud2 binary blob + * \param[out] cloud the resultant pcl::PointCloud + */ + template void + fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud& cloud) + { + MsgFieldMap field_map; + createMapping (msg.fields, field_map); + fromPCLPointCloud2 (msg, cloud, field_map); + } + + /** \brief Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob. + * \param[in] cloud the input pcl::PointCloud + * \param[out] msg the resultant PCLPointCloud2 binary blob + */ + template void + toPCLPointCloud2 (const pcl::PointCloud& cloud, pcl::PCLPointCloud2& msg) + { + // Ease the user's burden on specifying width/height for unorganized datasets + if (cloud.width == 0 && cloud.height == 0) + { + msg.width = static_cast(cloud.points.size ()); + msg.height = 1; + } + else + { + assert (cloud.points.size () == cloud.width * cloud.height); + msg.height = cloud.height; + msg.width = cloud.width; + } + + // Fill point cloud binary data (padding and all) + std::size_t data_size = sizeof (PointT) * cloud.points.size (); + msg.data.resize (data_size); + if (data_size) + { + memcpy(&msg.data[0], &cloud.points[0], data_size); + } + + // Fill fields metadata + msg.fields.clear (); + for_each_type::type> (detail::FieldAdder(msg.fields)); + + msg.header = cloud.header; + msg.point_step = sizeof (PointT); + msg.row_step = static_cast (sizeof (PointT) * msg.width); + msg.is_dense = cloud.is_dense; + /// @todo msg.is_bigendian = ?; + } + + /** \brief Copy the RGB fields of a PointCloud into pcl::PCLImage format + * \param[in] cloud the point cloud message + * \param[out] msg the resultant pcl::PCLImage + * CloudT cloud type, CloudT should be akin to pcl::PointCloud + * \note will throw std::runtime_error if there is a problem + */ + template void + toPCLPointCloud2 (const CloudT& cloud, pcl::PCLImage& msg) + { + // Ease the user's burden on specifying width/height for unorganized datasets + if (cloud.width == 0 && cloud.height == 0) + throw std::runtime_error("Needs to be a dense like cloud!!"); + else + { + if (cloud.points.size () != cloud.width * cloud.height) + throw std::runtime_error("The width and height do not match the cloud size!"); + msg.height = cloud.height; + msg.width = cloud.width; + } + + // ensor_msgs::image_encodings::BGR8; + msg.encoding = "bgr8"; + msg.step = msg.width * sizeof (std::uint8_t) * 3; + msg.data.resize (msg.step * msg.height); + for (std::size_t y = 0; y < cloud.height; y++) + { + for (std::size_t x = 0; x < cloud.width; x++) + { + std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); + memcpy (pixel, &cloud (x, y).rgb, 3 * sizeof(std::uint8_t)); + } + } + } + + /** \brief Copy the RGB fields of a PCLPointCloud2 msg into pcl::PCLImage format + * \param cloud the point cloud message + * \param msg the resultant pcl::PCLImage + * will throw std::runtime_error if there is a problem + */ + inline void + toPCLPointCloud2 (const pcl::PCLPointCloud2& cloud, pcl::PCLImage& msg) + { + const auto predicate = [](const auto& field) { return field.name == "rgb"; }; + const auto result = std::find_if(cloud.fields.cbegin (), cloud.fields.cend (), predicate); + if (result == cloud.fields.end ()) + throw std::runtime_error ("No rgb field!!"); + + const auto rgb_index = std::distance(cloud.fields.begin (), result); + if (cloud.width == 0 && cloud.height == 0) + throw std::runtime_error ("Needs to be a dense like cloud!!"); + else + { + msg.height = cloud.height; + msg.width = cloud.width; + } + int rgb_offset = cloud.fields[rgb_index].offset; + int point_step = cloud.point_step; + + // pcl::image_encodings::BGR8; + msg.encoding = "bgr8"; + msg.step = static_cast(msg.width * sizeof (std::uint8_t) * 3); + msg.data.resize (msg.step * msg.height); + + for (std::size_t y = 0; y < cloud.height; y++) + { + for (std::size_t x = 0; x < cloud.width; x++, rgb_offset += point_step) + { + std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); + memcpy (pixel, &(cloud.data[rgb_offset]), 3 * sizeof (std::uint8_t)); + } + } + } +} diff --git a/deps_install/include/pcl-1.10/pcl/correspondence.h b/deps_install/include/pcl-1.10/pcl/correspondence.h new file mode 100755 index 0000000..ed79763 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/correspondence.h @@ -0,0 +1,149 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Correspondence represents a match between two entities (e.g., points, descriptors, etc). This is + * represented via the indices of a \a source point and a \a target point, and the distance between them. + * + * \author Dirk Holz, Radu B. Rusu, Bastian Steder + * \ingroup common + */ + struct Correspondence + { + /** \brief Index of the query (source) point. */ + int index_query = 0; + /** \brief Index of the matching (target) point. Set to -1 if no correspondence found. */ + int index_match = -1; + /** \brief Distance between the corresponding points, or the weight denoting the confidence in correspondence estimation */ + union + { + float distance = std::numeric_limits::max(); + float weight; + }; + + /** \brief Standard constructor. + * Sets \ref index_query to 0, \ref index_match to -1, and \ref distance to FLT_MAX. + */ + inline Correspondence () = default; + + /** \brief Constructor. */ + inline Correspondence (int _index_query, int _index_match, float _distance) : + index_query (_index_query), index_match (_index_match), distance (_distance) + {} + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief overloaded << operator */ + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Correspondence& c); + + using Correspondences = std::vector< pcl::Correspondence, Eigen::aligned_allocator >; + using CorrespondencesPtr = shared_ptr; + using CorrespondencesConstPtr = shared_ptr; + + /** + * \brief Get the query points of correspondences that are present in + * one correspondence vector but not in the other, e.g., to compare + * correspondences before and after rejection. + * \param[in] correspondences_before Vector of correspondences before rejection + * \param[in] correspondences_after Vector of correspondences after rejection + * \param[out] indices Query point indices of correspondences that have been rejected + * \param[in] presorting_required Enable/disable internal sorting of vectors. + * By default (true), vectors are internally sorted before determining their difference. + * If the order of correspondences in \a correspondences_after is not different (has not been changed) + * from the order in \b correspondences_before this pre-processing step can be disabled + * in order to gain efficiency. In order to disable pre-sorting set \a presorting_required to false. + */ + void + getRejectedQueryIndices (const pcl::Correspondences &correspondences_before, + const pcl::Correspondences &correspondences_after, + std::vector& indices, + bool presorting_required = true); + + /** + * \brief Representation of a (possible) correspondence between two 3D points in two different coordinate frames + * (e.g. from feature matching) + * \ingroup common + */ + struct PointCorrespondence3D : public Correspondence + { + Eigen::Vector3f point1; //!< The 3D position of the point in the first coordinate frame + Eigen::Vector3f point2; //!< The 3D position of the point in the second coordinate frame + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + using PointCorrespondences3DVector = std::vector >; + + /** + * \brief Representation of a (possible) correspondence between two points (e.g. from feature matching), + * that encode complete 6DOF transformations. + * \ingroup common + */ + struct PointCorrespondence6D : public PointCorrespondence3D + { + Eigen::Affine3f transformation; //!< The transformation to go from the coordinate system + //!< of point2 to the coordinate system of point1 + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + using PointCorrespondences6DVector = std::vector >; + + /** + * \brief Comparator to enable us to sort a vector of PointCorrespondences according to their scores using + * std::sort (begin(), end(), isBetterCorrespondence); + * \ingroup common + */ + inline bool + isBetterCorrespondence (const Correspondence &pc1, const Correspondence &pc2) + { + return (pc1.distance > pc2.distance); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/exceptions.h b/deps_install/include/pcl-1.10/pcl/exceptions.h new file mode 100755 index 0000000..10a0079 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/exceptions.h @@ -0,0 +1,265 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +/** PCL_THROW_EXCEPTION a helper macro to be used for throwing exceptions. + * This is an example on how to use: + * PCL_THROW_EXCEPTION(IOException, + * "encountered an error while opening " << filename << " PCD file"); + */ +#define PCL_THROW_EXCEPTION(ExceptionName, message) \ +{ \ + std::ostringstream s; \ + s << message; \ + throw ExceptionName(s.str(), __FILE__, BOOST_CURRENT_FUNCTION, __LINE__); \ +} + +namespace pcl +{ + + /** \class PCLException + * \brief A base class for all pcl exceptions which inherits from std::runtime_error + * \author Eitan Marder-Eppstein, Suat Gedikli, Nizar Sallem + */ + class PCLException : public std::runtime_error + { + public: + + PCLException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : std::runtime_error (createDetailedMessage (error_description, + file_name, + function_name, + line_number)) + , file_name_ (file_name) + , function_name_ (function_name) + , line_number_ (line_number) + {} + + const char* + getFileName () const throw () + { + return (file_name_); + } + + const char* + getFunctionName () const throw () + { + return (function_name_); + } + + unsigned + getLineNumber () const throw () + { + return (line_number_); + } + + const char* + detailedMessage () const throw () + { + return (what ()); + } + + + protected: + static std::string + createDetailedMessage (const std::string& error_description, + const char* file_name, + const char* function_name, + unsigned line_number) + { + std::ostringstream sstream; + if (function_name != nullptr) + sstream << function_name << " "; + + if (file_name != nullptr) + { + sstream << "in " << file_name << " "; + if (line_number != 0) + sstream << "@ " << line_number << " "; + } + sstream << ": " << error_description; + + return (sstream.str ()); + } + + const char* file_name_; + const char* function_name_; + unsigned line_number_; + } ; + + /** \class InvalidConversionException + * \brief An exception that is thrown when a PCLPointCloud2 message cannot be converted into a PCL type + */ + class InvalidConversionException : public PCLException + { + public: + + InvalidConversionException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + /** \class IsNotDenseException + * \brief An exception that is thrown when a PointCloud is not dense but is attempted to be used as dense + */ + class IsNotDenseException : public PCLException + { + public: + + IsNotDenseException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + /** \class InvalidSACModelTypeException + * \brief An exception that is thrown when a sample consensus model doesn't + * have the correct number of samples defined in model_types.h + */ + class InvalidSACModelTypeException : public PCLException + { + public: + + InvalidSACModelTypeException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + /** \class IOException + * \brief An exception that is thrown during an IO error (typical read/write errors) + */ + class IOException : public PCLException + { + public: + + IOException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + /** \class InitFailedException + * \brief An exception thrown when init can not be performed should be used in all the + * PCLBase class inheritants. + */ + class InitFailedException : public PCLException + { + public: + InitFailedException (const std::string& error_description = "", + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + /** \class UnorganizedPointCloudException + * \brief An exception that is thrown when an organized point cloud is needed + * but not provided. + */ + class UnorganizedPointCloudException : public PCLException + { + public: + + UnorganizedPointCloudException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + /** \class KernelWidthTooSmallException + * \brief An exception that is thrown when the kernel size is too small + */ + class KernelWidthTooSmallException : public PCLException + { + public: + + KernelWidthTooSmallException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + class UnhandledPointTypeException : public PCLException + { + public: + UnhandledPointTypeException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + }; + + class ComputeFailedException : public PCLException + { + public: + ComputeFailedException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + }; + + /** \class BadArgumentException + * \brief An exception that is thrown when the arguments number or type is wrong/unhandled. + */ + class BadArgumentException : public PCLException + { + public: + BadArgumentException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/features/3dsc.h b/deps_install/include/pcl-1.10/pcl/features/3dsc.h new file mode 100755 index 0000000..2d8d41c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/3dsc.h @@ -0,0 +1,243 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include +#include +#include + +namespace pcl +{ + /** \brief ShapeContext3DEstimation implements the 3D shape context descriptor as + * described in: + * - Andrea Frome, Daniel Huber, Ravi Kolluri and Thomas Bülow, Jitendra Malik + * Recognizing Objects in Range Data Using Regional Point Descriptors, + * In proceedings of the 8th European Conference on Computer Vision (ECCV), + * Prague, May 11-14, 2004 + * + * The suggested PointOutT is pcl::ShapeContext1980 + * + * \attention + * The convention for a 3D shape context descriptor is: + * - if a query point's nearest neighbors cannot be estimated, the feature descriptor will be set to NaN (not a number), and the RF to 0 + * - it is impossible to estimate a 3D shape context descriptor for a + * point that doesn't have finite 3D coordinates. Therefore, any point + * that contains NaN data on x, y, or z, will have its boundary feature + * property set to NaN. + * + * \author Alessandro Franchi, Samuele Salti, Federico Tombari (original code) + * \author Nizar Sallem (port to PCL) + * \ingroup features + */ + template + class ShapeContext3DEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::search_parameter_; + using Feature::search_radius_; + using Feature::surface_; + using Feature::input_; + using Feature::searchForNeighbors; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + + /** \brief Constructor. + * \param[in] random If true the random seed is set to current time, else it is + * set to 12345 prior to computing the descriptor (used to select X axis) + */ + ShapeContext3DEstimation (bool random = false) : + radii_interval_(0), + theta_divisions_(0), + phi_divisions_(0), + volume_lut_(0), + azimuth_bins_(12), + elevation_bins_(11), + radius_bins_(15), + min_radius_(0.1), + point_density_radius_(0.2), + descriptor_length_ (), + rng_dist_ (0.0f, 1.0f) + { + feature_name_ = "ShapeContext3DEstimation"; + search_radius_ = 2.5; + + // Create a random number generator object + if (random) + { + std::random_device rd; + rng_.seed (rd()); + } + else + rng_.seed (12345u); + } + + ~ShapeContext3DEstimation() {} + + //inline void + //setAzimuthBins (std::size_t bins) { azimuth_bins_ = bins; } + + /** \return the number of bins along the azimuth */ + inline std::size_t + getAzimuthBins () { return (azimuth_bins_); } + + //inline void + //setElevationBins (std::size_t bins) { elevation_bins_ = bins; } + + /** \return The number of bins along the elevation */ + inline std::size_t + getElevationBins () { return (elevation_bins_); } + + //inline void + //setRadiusBins (std::size_t bins) { radius_bins_ = bins; } + + /** \return The number of bins along the radii direction */ + inline std::size_t + getRadiusBins () { return (radius_bins_); } + + /** \brief The minimal radius value for the search sphere (rmin) in the original paper + * \param[in] radius the desired minimal radius + */ + inline void + setMinimalRadius (double radius) { min_radius_ = radius; } + + /** \return The minimal sphere radius */ + inline double + getMinimalRadius () { return (min_radius_); } + + /** \brief This radius is used to compute local point density + * density = number of points within this radius + * \param[in] radius value of the point density search radius + */ + inline void + setPointDensityRadius (double radius) { point_density_radius_ = radius; } + + /** \return The point density search radius */ + inline double + getPointDensityRadius () { return (point_density_radius_); } + + protected: + /** \brief Initialize computation by allocating all the intervals and the volume lookup table. */ + bool + initCompute () override; + + /** \brief Estimate a descriptor for a given point. + * \param[in] index the index of the point to estimate a descriptor for + * \param[in] normals a pointer to the set of normals + * \param[in] rf the reference frame + * \param[out] desc the resultant estimated descriptor + * \return true if the descriptor was computed successfully, false if there was an error + * (e.g. the nearest neighbor didn't return any neighbors) + */ + bool + computePoint (std::size_t index, const pcl::PointCloud &normals, float rf[9], std::vector &desc); + + /** \brief Estimate the actual feature. + * \param[out] output the resultant feature + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Values of the radii interval */ + std::vector radii_interval_; + + /** \brief Theta divisions interval */ + std::vector theta_divisions_; + + /** \brief Phi divisions interval */ + std::vector phi_divisions_; + + /** \brief Volumes look up table */ + std::vector volume_lut_; + + /** \brief Bins along the azimuth dimension */ + std::size_t azimuth_bins_; + + /** \brief Bins along the elevation dimension */ + std::size_t elevation_bins_; + + /** \brief Bins along the radius dimension */ + std::size_t radius_bins_; + + /** \brief Minimal radius value */ + double min_radius_; + + /** \brief Point density radius */ + double point_density_radius_; + + /** \brief Descriptor length */ + std::size_t descriptor_length_; + + /** \brief Random number generator algorithm. */ + std::mt19937 rng_; + + /** \brief Random number generator distribution. */ + std::uniform_real_distribution rng_dist_; + + /* \brief Shift computed descriptor "L" times along the azimuthal direction + * \param[in] block_size the size of each azimuthal block + * \param[in] desc at input desc == original descriptor and on output it contains + * shifted descriptor resized descriptor_length_ * azimuth_bins_ + */ + //void + //shiftAlongAzimuth (std::size_t block_size, std::vector& desc); + + /** \brief Boost-based random number generator. */ + inline float + rnd () + { + return (rng_dist_ (rng_)); + } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/board.h b/deps_install/include/pcl-1.10/pcl/features/board.h new file mode 100755 index 0000000..693db2c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/board.h @@ -0,0 +1,361 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm + * for local reference frame estimation as described here: + * + * - A. Petrelli, L. Di Stefano, + * "On the repeatability of the local reference frame for partial shape matching", + * 13th International Conference on Computer Vision (ICCV), 2011 + * + * \author Alioscia Petrelli (original), Tommaso Cavallari (PCL port) + * \ingroup features + */ + template + class BOARDLocalReferenceFrameEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + BOARDLocalReferenceFrameEstimation () : + tangent_radius_ (0.0f), + find_holes_ (false), + margin_thresh_ (0.85f), + check_margin_array_size_ (24), + hole_size_prob_thresh_ (0.2f), + steep_thresh_ (0.1f) + { + feature_name_ = "BOARDLocalReferenceFrameEstimation"; + setCheckMarginArraySize (check_margin_array_size_); + } + + /** \brief Empty destructor */ + ~BOARDLocalReferenceFrameEstimation () {} + + //Getters/Setters + + /** \brief Set the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference Frame for a given point. + * + * \param[in] radius The search radius for x and y axes. If not set or set to 0 the parameter given with setRadiusSearch is used. + */ + inline void + setTangentRadius (float radius) + { + tangent_radius_ = radius; + } + + /** \brief Get the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference Frame for a given point. + * + * \return The search radius for x and y axes. If set to 0 the parameter given with setRadiusSearch is used. + */ + inline float + getTangentRadius () const + { + return (tangent_radius_); + } + + /** \brief Sets whether holes in the margin of the support, for each point, are searched and accounted for in the estimation of the + * Reference Frame or not. + * + * \param[in] find_holes Enable/Disable the search for holes in the support. + */ + inline void + setFindHoles (bool find_holes) + { + find_holes_ = find_holes; + } + + /** \brief Gets whether holes in the margin of the support, for each point, are searched and accounted for in the estimation of the + * Reference Frame or not. + * + * \return The search for holes in the support is enabled/disabled. + */ + inline bool + getFindHoles () const + { + return (find_holes_); + } + + /** \brief Sets the percentage of the search radius (or tangent radius if set) after which a point is considered part of the support margin. + * + * \param[in] margin_thresh the percentage of the search radius after which a point is considered a margin point. + */ + inline void + setMarginThresh (float margin_thresh) + { + margin_thresh_ = margin_thresh; + } + + /** \brief Gets the percentage of the search radius (or tangent radius if set) after which a point is considered part of the support margin. + * + * \return The percentage of the search radius after which a point is considered a margin point. + */ + inline float + getMarginThresh () const + { + return (margin_thresh_); + } + + /** \brief Sets the number of slices in which is divided the margin for the search of missing regions. + * + * \param[in] size the number of margin slices. + */ + void + setCheckMarginArraySize (int size) + { + check_margin_array_size_ = size; + + check_margin_array_.clear (); + check_margin_array_.resize (check_margin_array_size_); + + margin_array_min_angle_.clear (); + margin_array_min_angle_.resize (check_margin_array_size_); + + margin_array_max_angle_.clear (); + margin_array_max_angle_.resize (check_margin_array_size_); + + margin_array_min_angle_normal_.clear (); + margin_array_min_angle_normal_.resize (check_margin_array_size_); + + margin_array_max_angle_normal_.clear (); + margin_array_max_angle_normal_.resize (check_margin_array_size_); + } + + /** \brief Gets the number of slices in which is divided the margin for the search of missing regions. + * + * \return the number of margin slices. + */ + inline int + getCheckMarginArraySize () const + { + return (check_margin_array_size_); + } + + /** \brief Given the angle width of a hole in the support margin, sets the minimum percentage of a circumference this angle + * must cover to be considered a missing region in the support and hence used for the estimation of the Reference Frame. + * + * \param[in] prob_thresh the minimum percentage of a circumference after which a hole is considered in the calculation + */ + inline void + setHoleSizeProbThresh (float prob_thresh) + { + hole_size_prob_thresh_ = prob_thresh; + } + + /** \brief Given the angle width of a hole in the support margin, gets the minimum percentage of a circumference this angle + * must cover to be considered a missing region in the support and hence used for the estimation of the Reference Frame. + * + * \return the minimum percentage of a circumference after which a hole is considered in the calculation + */ + inline float + getHoleSizeProbThresh () const + { + return (hole_size_prob_thresh_); + } + + /** \brief Sets the minimum steepness that the normals of the points situated on the borders of the hole, with reference + * to the normal of the best point found by the algorithm, must have in order to be considered in the calculation of the Reference Frame. + * + * \param[in] steep_thresh threshold that defines if a missing region contains a point with the most different normal. + */ + inline void + setSteepThresh (float steep_thresh) + { + steep_thresh_ = steep_thresh; + } + + /** \brief Gets the minimum steepness that the normals of the points situated on the borders of the hole, with reference + * to the normal of the best point found by the algorithm, must have in order to be considered in the calculation of the Reference Frame. + * + * \return threshold that defines if a missing region contains a point with the most different normal. + */ + inline float + getSteepThresh () const + { + return (steep_thresh_); + } + + protected: + using Feature::feature_name_; + using Feature::getClassName; + using Feature::input_; + using Feature::indices_; + using Feature::surface_; + using Feature::tree_; + using Feature::search_parameter_; + using FeatureFromNormals::normals_; + + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudOut = typename Feature::PointCloudOut; + + void + resetData () + { + setCheckMarginArraySize (check_margin_array_size_); + } + + /** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] index the index of the point in input_ + * \param[out] lrf the resultant local reference frame + */ + float + computePointLRF (const int &index, Eigen::Matrix3f &lrf); + + /** \brief Abstract feature estimation method. + * \param[out] output the resultant features + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Given an axis (with origin axis_origin), return the orthogonal axis directed to point. + * + * \note axis must be normalized. + * + * \param[in] axis the axis + * \param[in] axis_origin the axis_origin + * \param[in] point the point towards which the resulting axis is directed + * \param[out] directed_ortho_axis the directed orthogonal axis calculated + */ + void + directedOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, Eigen::Vector3f &directed_ortho_axis); + + /** \brief return the angle (in radians) that rotate v1 to v2 with respect to axis . + * + * \param[in] v1 the first vector + * \param[in] v2 the second vector + * \param[in] axis the rotation axis. Axis must be normalized and orthogonal to plane defined by v1 and v2. + * \return angle + */ + float + getAngleBetweenUnitVectors (Eigen::Vector3f const &v1, Eigen::Vector3f const &v2, Eigen::Vector3f const &axis); + + /** \brief Disambiguates a normal direction using adjacent normals + * + * \param[in] normals_cloud a cloud of normals used for the calculation + * \param[in] normal_indices the indices of the normals in the cloud that should to be used for the calculation + * \param[in,out] normal the normal to disambiguate, the calculation is performed in place + */ + void + normalDisambiguation (pcl::PointCloud const &normals_cloud, std::vector const &normal_indices, + Eigen::Vector3f &normal); + + /** \brief Compute Least Square Plane Fitting in a set of 3D points + * + * \param[in] points Matrix(nPoints,3) of 3D points coordinates + * \param[out] center centroid of the distribution of points that belongs to the fitted plane + * \param[out] norm normal to the fitted plane + */ + void + planeFitting (Eigen::Matrix const &points, Eigen::Vector3f ¢er, + Eigen::Vector3f &norm); + + /** \brief Given a plane (origin and normal) and a point, return the projection of x on plane + * + * Equivalent to vtkPlane::ProjectPoint() + * + * \param[in] point the point to project + * \param[in] origin_point a point belonging to the plane + * \param[in] plane_normal normal of the plane + * \param[out] projected_point the projection of the point on the plane + */ + void + projectPointOnPlane (Eigen::Vector3f const &point, Eigen::Vector3f const &origin_point, + Eigen::Vector3f const &plane_normal, Eigen::Vector3f &projected_point); + + /** \brief Given an axis, return a random orthogonal axis + * + * \param[in] axis input axis + * \param[out] rand_ortho_axis an axis orthogonal to the input axis and whose direction is random + */ + void + randomOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis); + + /** \brief Check if val1 and val2 are equals. + * + * \param[in] val1 first number to check. + * \param[in] val2 second number to check. + * \param[in] zero_float_eps epsilon + * \return true if val1 is equal to val2, false otherwise. + */ + inline bool + areEquals (float val1, float val2, float zero_float_eps = 1E-8f) const + { + return (std::abs (val1 - val2) < zero_float_eps); + } + + private: + /** \brief Radius used to find tangent axis. */ + float tangent_radius_; + + /** \brief If true, check if support is complete or has missing regions because it is too near to mesh borders. */ + bool find_holes_; + + /** \brief Threshold that define if a support point is near the margins. */ + float margin_thresh_; + + /** \brief Number of slices that divide the support in order to determine if a missing region is present. */ + int check_margin_array_size_; + + /** \brief Threshold used to determine a missing region */ + float hole_size_prob_thresh_; + + /** \brief Threshold that defines if a missing region contains a point with the most different normal. */ + float steep_thresh_; + + std::vector check_margin_array_; + std::vector margin_array_min_angle_; + std::vector margin_array_max_angle_; + std::vector margin_array_min_angle_normal_; + std::vector margin_array_max_angle_normal_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/boost.h b/deps_install/include/pcl-1.10/pcl/features/boost.h new file mode 100755 index 0000000..53f51c5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/boost.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include diff --git a/deps_install/include/pcl-1.10/pcl/features/boundary.h b/deps_install/include/pcl-1.10/pcl/features/boundary.h new file mode 100755 index 0000000..98d2810 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/boundary.h @@ -0,0 +1,184 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief BoundaryEstimation estimates whether a set of points is lying on surface boundaries using an angle + * criterion. The code makes use of the estimated surface normals at each point in the input dataset. + * + * Here's an example for estimating boundary points for a PointXYZ point cloud: + * \code + * pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + * // fill in the cloud data here + * + * pcl::PointCloud::Ptr normals (new pcl::PointCloud); + * // estimate normals and fill in \a normals + * + * pcl::PointCloud boundaries; + * pcl::BoundaryEstimation est; + * est.setInputCloud (cloud); + * est.setInputNormals (normals); + * est.setRadiusSearch (0.02); // 2cm radius + * est.setSearchMethod (typename pcl::search::KdTree::Ptr (new pcl::search::KdTree) + * est.compute (boundaries); + * \endcode + * + * \attention + * The convention for Boundary features is: + * - if a query point's nearest neighbors cannot be estimated, the boundary feature will be set to NaN + * (not a number) + * - it is impossible to estimate a boundary property for a point that + * doesn't have finite 3D coordinates. Therefore, any point that contains + * NaN data on x, y, or z, will have its boundary feature property set to NaN. + * + * \author Radu B. Rusu + * \ingroup features + */ + template + class BoundaryEstimation: public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Feature::feature_name_; + using Feature::getClassName; + using Feature::input_; + using Feature::indices_; + using Feature::k_; + using Feature::tree_; + using Feature::search_radius_; + using Feature::search_parameter_; + using Feature::surface_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + + public: + /** \brief Empty constructor. + * The angular threshold \a angle_threshold_ is set to M_PI / 2.0 + */ + BoundaryEstimation () : angle_threshold_ (static_cast (M_PI) / 2.0f) + { + feature_name_ = "BoundaryEstimation"; + }; + + /** \brief Check whether a point is a boundary point in a planar patch of projected points given by indices. + * \note A coordinate system u-v-n must be computed a-priori using \a getCoordinateSystemOnPlane + * \param[in] cloud a pointer to the input point cloud + * \param[in] q_idx the index of the query point in \a cloud + * \param[in] indices the estimated point neighbors of the query point + * \param[in] u the u direction + * \param[in] v the v direction + * \param[in] angle_threshold the threshold angle (default \f$\pi / 2.0\f$) + */ + bool + isBoundaryPoint (const pcl::PointCloud &cloud, + int q_idx, const std::vector &indices, + const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold); + + /** \brief Check whether a point is a boundary point in a planar patch of projected points given by indices. + * \note A coordinate system u-v-n must be computed a-priori using \a getCoordinateSystemOnPlane + * \param[in] cloud a pointer to the input point cloud + * \param[in] q_point a pointer to the querry point + * \param[in] indices the estimated point neighbors of the query point + * \param[in] u the u direction + * \param[in] v the v direction + * \param[in] angle_threshold the threshold angle (default \f$\pi / 2.0\f$) + */ + bool + isBoundaryPoint (const pcl::PointCloud &cloud, + const PointInT &q_point, + const std::vector &indices, + const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold); + + /** \brief Set the decision boundary (angle threshold) that marks points as boundary or regular. + * (default \f$\pi / 2.0\f$) + * \param[in] angle the angle threshold + */ + inline void + setAngleThreshold (float angle) + { + angle_threshold_ = angle; + } + + /** \brief Get the decision boundary (angle threshold) as set by the user. */ + inline float + getAngleThreshold () + { + return (angle_threshold_); + } + + /** \brief Get a u-v-n coordinate system that lies on a plane defined by its normal + * \param[in] p_coeff the plane coefficients (containing the plane normal) + * \param[out] u the resultant u direction + * \param[out] v the resultant v direction + */ + inline void + getCoordinateSystemOnPlane (const PointNT &p_coeff, + Eigen::Vector4f &u, Eigen::Vector4f &v) + { + pcl::Vector4fMapConst p_coeff_v = p_coeff.getNormalVector4fMap (); + v = p_coeff_v.unitOrthogonal (); + u = p_coeff_v.cross3 (v); + } + + protected: + /** \brief Estimate whether a set of points is lying on surface boundaries using an angle criterion for all points + * given in using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains boundary point estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief The decision boundary (angle threshold) that marks points as boundary or regular. (default \f$\pi / 2.0\f$) */ + float angle_threshold_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/brisk_2d.h b/deps_install/include/pcl-1.10/pcl/features/brisk_2d.h new file mode 100755 index 0000000..a40d799 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/brisk_2d.h @@ -0,0 +1,263 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception , Inc. + * Copyright (C) 2011 The Autonomous Systems Lab (ASL), ETH Zurich, + * Stefan Leutenegger, Simon Lynen and Margarita Chli. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +// PCL includes +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Implementation of the BRISK-descriptor, based on the original code and paper reference by + * + * \par + * Stefan Leutenegger,Margarita Chli and Roland Siegwart, + * BRISK: Binary Robust Invariant Scalable Keypoints, + * in Proceedings of the IEEE International Conference on Computer Vision (ICCV2011). + * + * \warning The input keypoints cloud is not const, and it will be modified: keypoints for which descriptors can not + * be computed will be deleted from the cloud. + * + * \author Radu B. Rusu, Stefan Holzer + * \ingroup features + */ + template > + class BRISK2DEstimation// : public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudInT = pcl::PointCloud; + using PointCloudInTConstPtr = typename PointCloudInT::ConstPtr; + + using KeypointPointCloudT = pcl::PointCloud; + using KeypointPointCloudTPtr = typename KeypointPointCloudT::Ptr; + using KeypointPointCloudTConstPtr = typename KeypointPointCloudT::ConstPtr; + + using PointCloudOutT = pcl::PointCloud; + + /** \brief Constructor. */ + BRISK2DEstimation (); + + /** \brief Destructor. */ + virtual ~BRISK2DEstimation (); + + /** \brief Determines whether rotation invariance is enabled. + * \param[in] enable determines whether rotation invariance is enabled. + */ + inline void + setRotationInvariance (const bool enable) + { + rotation_invariance_enabled_ = enable; + } + + /** \brief Determines whether scale invariance is enabled. + * \param[in] enable determines whether scale invariance is enabled. + */ + inline void + setScaleInvariance (const bool enable) + { + scale_invariance_enabled_ = enable; + } + + /** \brief Sets the input cloud. + * \param[in] cloud the input cloud. + */ + inline void + setInputCloud (const PointCloudInTConstPtr & cloud) + { + input_cloud_ = cloud; + } + + /** \brief Sets the input keypoints. + * \param[in] keypoints the input cloud containing the keypoints. + */ + inline void + setKeypoints (const KeypointPointCloudTPtr &keypoints) + { + // Make a copy as we will have to erase keypoints that we don't use + // TO DO: change this later + //keypoints_.reset (new KeypointPointCloudT (*keypoints)); + keypoints_ = keypoints; + } + + /** \brief Computes the descriptors for the previously specified + * points and input data. + * \param[out] output descriptors the destination for the computed descriptors. + */ + void + compute (PointCloudOutT &output); + //td::vector & descriptors) const; + + protected: + /** \brief Call this to generate the kernel: + * circle of radius r (pixels), with n points; + * short pairings with dMax, long pairings with dMin + * + * \note This should never be called by a regular user. We use a fixed type in PCL + * (BRISKSignature512) and tampering with the parameters might lead to a different + * size descriptor which the user needs to accommodate in a new point type. + */ + void + generateKernel (std::vector &radius_list, + std::vector &number_list, + float d_max = 5.85f, float d_min = 8.2f, + std::vector index_change = std::vector ()); + + /** \brief Compute the smoothed intensity for a given x/y position in the image. */ + inline int + smoothedIntensity (const std::vector& image, + int image_width, int image_height, + const std::vector& integral_image, + const float key_x, const float key_y, const unsigned int scale, + const unsigned int rot, const unsigned int point) const; + + private: + /** \brief ROI predicate comparator. */ + bool + RoiPredicate (const float min_x, const float min_y, + const float max_x, const float max_y, const KeypointT& key_pt); + + /** \brief Specifies whether rotation invariance is enabled. */ + bool rotation_invariance_enabled_; + + /** \brief Specifies whether scale invariance is enabled. */ + bool scale_invariance_enabled_; + + /** \brief Specifies the scale of the pattern. */ + const float pattern_scale_; + + /** \brief the input cloud. */ + PointCloudInTConstPtr input_cloud_; + + /** \brief the input keypoints. */ + KeypointPointCloudTPtr keypoints_; + + // TODO: set + float scale_range_; + + // Some helper structures for the Brisk pattern representation + struct BriskPatternPoint + { + /** x coordinate relative to center. */ + float x; + /** x coordinate relative to center. */ + float y; + /** Gaussian smoothing sigma. */ + float sigma; + }; + + struct BriskShortPair + { + /** index of the first pattern point. */ + unsigned int i; + /** index of other pattern point. */ + unsigned int j; + }; + + struct BriskLongPair + { + /** index of the first pattern point. */ + unsigned int i; + /** index of other pattern point. */ + unsigned int j; + /** 1024.0/dx. */ + int weighted_dx; + /** 1024.0/dy. */ + int weighted_dy; + }; + + // pattern properties + /** [i][rotation][scale]. */ + BriskPatternPoint* pattern_points_; + + /** Total number of collocation points. */ + unsigned int points_; + + /** Discretization of the rotation look-up. */ + const unsigned int n_rot_; + + /** Lists the scaling per scale index [scale]. */ + float* scale_list_; + + /** Lists the total pattern size per scale index [scale]. */ + unsigned int* size_list_; + + /** Scales discretization. */ + const unsigned int scales_; + + /** Span of sizes 40->4 Octaves - else, this needs to be adjusted... */ + const float scalerange_; + + // general + const float basic_size_; + + // pairs + /** Number of uchars the descriptor consists of. */ + int strings_; + /** Short pair maximum distance. */ + float d_max_; + /** Long pair maximum distance. */ + float d_min_; + /** d<_d_max. */ + BriskShortPair* short_pairs_; + /** d>_d_min. */ + BriskLongPair* long_pairs_; + /** Number of short pairs. */ + unsigned int no_short_pairs_; + /** Number of long pairs. */ + unsigned int no_long_pairs_; + + /** \brief Intensity field accessor. */ + IntensityT intensity_; + + /** \brief The name of the class. */ + std::string name_; + }; + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/features/cppf.h b/deps_install/include/pcl-1.10/pcl/features/cppf.h new file mode 100755 index 0000000..84d283e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/cppf.h @@ -0,0 +1,117 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2013, Martin Szarski + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief + * \param[in] p1 + * \param[in] n1 + * \param[in] p2 + * \param[in] n2 + * \param[in] c1 + * \param[in] c2 + * \param[out] f1 + * \param[out] f2 + * \param[out] f3 + * \param[out] f4 + * \param[out] f5 + * \param[out] f6 + * \param[out] f7 + * \param[out] f8 + * \param[out] f9 + * \param[out] f10 + */ + PCL_EXPORTS bool + computeCPPFPairFeature (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &c1, + const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &c2, + float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7, float &f8, float &f9, float &f10); + + + + /** \brief Class that calculates the "surflet" features for each pair in the given + * pointcloud. Please refer to the following publication for more details: + * C. Choi, Henrik Christensen + * 3D Pose Estimation of Daily Objects Using an RGB-D Camera + * Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) + * 2012 + * + * PointOutT is meant to be pcl::CPPFSignature - contains the 10 values of the Surflet + * feature and in addition, alpha_m for the respective pair - optimization proposed by + * the authors (see above) + * + * \author Martin Szarski, Alexandru-Eugen Ichim + */ + + template + class CPPFEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PCLBase::indices_; + using Feature::input_; + using Feature::feature_name_; + using Feature::getClassName; + using FeatureFromNormals::normals_; + + using PointCloudOut = pcl::PointCloud; + + /** \brief Empty Constructor. */ + CPPFEstimation (); + + + private: + /** \brief The method called for actually doing the computations + * \param[out] output the resulting point cloud (which should be of type pcl::CPPFSignature); + * its size is the size of the input cloud, squared (i.e., one point for each pair in + * the input cloud); + */ + void + computeFeature (PointCloudOut &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/crh.h b/deps_install/include/pcl-1.10/pcl/features/crh.h new file mode 100755 index 0000000..6f64c5c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/crh.h @@ -0,0 +1,142 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: cvfh.h 4936 2012-03-07 11:12:45Z aaldoma $ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief CRHEstimation estimates the Camera Roll Histogram (CRH) descriptor for a given + * point cloud dataset containing XYZ data and normals, as presented in: + * - CAD-Model Recognition and 6 DOF Pose Estimation + * A. Aldoma, N. Blodow, D. Gossow, S. Gedikli, R.B. Rusu, M. Vincze and G. Bradski + * ICCV 2011, 3D Representation and Recognition (3dRR11) workshop + * Barcelona, Spain, (2011) + * + * The suggested PointOutT is pcl::Histogram<90>. //dc (real) + 44 complex numbers (real, imaginary) + nyquist (real) + * + * \author Aitor Aldoma + * \ingroup features + */ + template > + class CRHEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_radius_; + using Feature::surface_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Constructor. */ + CRHEstimation () : + vpx_ (0), vpy_ (0), vpz_ (0), nbins_ (90) + { + k_ = 1; + feature_name_ = "CRHEstimation"; + } + ; + + /** \brief Set the viewpoint. + * \param[in] vpx the X coordinate of the viewpoint + * \param[in] vpy the Y coordinate of the viewpoint + * \param[in] vpz the Z coordinate of the viewpoint + */ + inline void + setViewPoint (float vpx, float vpy, float vpz) + { + vpx_ = vpx; + vpy_ = vpy; + vpz_ = vpz; + } + + /** \brief Get the viewpoint. + * \param[out] vpx the X coordinate of the viewpoint + * \param[out] vpy the Y coordinate of the viewpoint + * \param[out] vpz the Z coordinate of the viewpoint + */ + inline void + getViewPoint (float &vpx, float &vpy, float &vpz) + { + vpx = vpx_; + vpy = vpy_; + vpz = vpz_; + } + + inline void + setCentroid (Eigen::Vector4f & centroid) + { + centroid_ = centroid; + } + + private: + /** \brief Values describing the viewpoint ("pinhole" camera model assumed). + * By default, the viewpoint is set to 0,0,0. + */ + float vpx_, vpy_, vpz_; + + /** \brief Number of bins, this should match the Output type */ + int nbins_; + + /** \brief Centroid to be used */ + Eigen::Vector4f centroid_; + + /** \brief Estimate the CRH histogram at + * a set of points given by using the surface in + * setSearchSurface () + * + * \param[out] output the resultant point cloud with a CRH histogram + */ + void + computeFeature (PointCloudOut &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/cvfh.h b/deps_install/include/pcl-1.10/pcl/features/cvfh.h new file mode 100755 index 0000000..a7b2067 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/cvfh.h @@ -0,0 +1,288 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given + * point cloud dataset containing XYZ data and normals, as presented in: + * - CAD-Model Recognition and 6 DOF Pose Estimation + * A. Aldoma, N. Blodow, D. Gossow, S. Gedikli, R.B. Rusu, M. Vincze and G. Bradski + * ICCV 2011, 3D Representation and Recognition (3dRR11) workshop + * Barcelona, Spain, (2011) + * + * The suggested PointOutT is pcl::VFHSignature308. + * + * \author Aitor Aldoma + * \ingroup features + */ + template + class CVFHEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_radius_; + using Feature::surface_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + using KdTreePtr = typename pcl::search::Search::Ptr; + using VFHEstimator = pcl::VFHEstimation; + + /** \brief Empty constructor. */ + CVFHEstimation () : + vpx_ (0), vpy_ (0), vpz_ (0), + leaf_size_ (0.005f), + normalize_bins_ (false), + curv_threshold_ (0.03f), + cluster_tolerance_ (leaf_size_ * 3), + eps_angle_threshold_ (0.125f), + min_points_ (50), + radius_normals_ (leaf_size_ * 3) + { + search_radius_ = 0; + k_ = 1; + feature_name_ = "CVFHEstimation"; + } + ; + + /** \brief Removes normals with high curvature caused by real edges or noisy data + * \param[in] cloud pointcloud to be filtered + * \param[in] indices_to_use the indices to use + * \param[out] indices_out the indices of the points with higher curvature than threshold + * \param[out] indices_in the indices of the remaining points after filtering + * \param[in] threshold threshold value for curvature + */ + void + filterNormalsWithHighCurvature (const pcl::PointCloud & cloud, std::vector & indices_to_use, std::vector &indices_out, + std::vector &indices_in, float threshold); + + /** \brief Set the viewpoint. + * \param[in] vpx the X coordinate of the viewpoint + * \param[in] vpy the Y coordinate of the viewpoint + * \param[in] vpz the Z coordinate of the viewpoint + */ + inline void + setViewPoint (float vpx, float vpy, float vpz) + { + vpx_ = vpx; + vpy_ = vpy; + vpz_ = vpz; + } + + /** \brief Set the radius used to compute normals + * \param[in] radius_normals the radius + */ + inline void + setRadiusNormals (float radius_normals) + { + radius_normals_ = radius_normals; + } + + /** \brief Get the viewpoint. + * \param[out] vpx the X coordinate of the viewpoint + * \param[out] vpy the Y coordinate of the viewpoint + * \param[out] vpz the Z coordinate of the viewpoint + */ + inline void + getViewPoint (float &vpx, float &vpy, float &vpz) + { + vpx = vpx_; + vpy = vpy_; + vpz = vpz_; + } + + /** \brief Get the centroids used to compute different CVFH descriptors + * \param[out] centroids vector to hold the centroids + */ + inline void + getCentroidClusters (std::vector > & centroids) + { + centroids.insert (centroids.cend (), centroids_dominant_orientations_.cbegin (), + centroids_dominant_orientations_.cend ()); + } + + /** \brief Get the normal centroids used to compute different CVFH descriptors + * \param[out] centroids vector to hold the normal centroids + */ + inline void + getCentroidNormalClusters (std::vector > & centroids) + { + for (const auto& normal: dominant_normals_) + centroids.push_back (normal); + } + + /** \brief Sets max. Euclidean distance between points to be added to the cluster + * \param[in] d the maximum Euclidean distance + */ + + inline void + setClusterTolerance (float d) + { + cluster_tolerance_ = d; + } + + /** \brief Sets max. deviation of the normals between two points so they can be clustered together + * \param[in] d the maximum deviation + */ + inline void + setEPSAngleThreshold (float d) + { + eps_angle_threshold_ = d; + } + + /** \brief Sets curvature threshold for removing normals + * \param[in] d the curvature threshold + */ + inline void + setCurvatureThreshold (float d) + { + curv_threshold_ = d; + } + + /** \brief Set minimum amount of points for a cluster to be considered + * \param[in] min the minimum amount of points to be set + */ + inline void + setMinPoints (std::size_t min) + { + min_points_ = min; + } + + /** \brief Sets whether if the CVFH signatures should be normalized or not + * \param[in] normalize true if normalization is required, false otherwise + */ + inline void + setNormalizeBins (bool normalize) + { + normalize_bins_ = normalize; + } + + /** \brief Overloaded computed method from pcl::Feature. + * \param[out] output the resultant point cloud model dataset containing the estimated features + */ + void + compute (PointCloudOut &output); + + private: + /** \brief Values describing the viewpoint ("pinhole" camera model assumed). + * By default, the viewpoint is set to 0,0,0. + */ + float vpx_, vpy_, vpz_; + + /** \brief Size of the voxels after voxel gridding. IMPORTANT: Must match the voxel + * size of the training data or the normalize_bins_ flag must be set to true. + */ + float leaf_size_; + + /** \brief Whether to normalize the signatures or not. Default: false. */ + bool normalize_bins_; + + /** \brief Curvature threshold for removing normals. */ + float curv_threshold_; + + /** \brief allowed Euclidean distance between points to be added to the cluster. */ + float cluster_tolerance_; + + /** \brief deviation of the normals between two points so they can be clustered together. */ + float eps_angle_threshold_; + + /** \brief Minimum amount of points in a clustered region to be considered stable for CVFH + * computation. + */ + std::size_t min_points_; + + /** \brief Radius for the normals computation. */ + float radius_normals_; + + /** \brief Estimate the Clustered Viewpoint Feature Histograms (CVFH) descriptors at + * a set of points given by using the surface in + * setSearchSurface () + * + * \param[out] output the resultant point cloud model dataset that contains the CVFH + * feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Region growing method using Euclidean distances and neighbors normals to + * add points to a region. + * \param[in] cloud point cloud to split into regions + * \param[in] normals are the normals of cloud + * \param[in] tolerance is the allowed Euclidean distance between points to be added to + * the cluster + * \param[in] tree is the spatial search structure for nearest neighbour search + * \param[out] clusters vector of indices representing the clustered regions + * \param[in] eps_angle deviation of the normals between two points so they can be + * clustered together + * \param[in] min_pts_per_cluster minimum cluster size. (default: 1 point) + * \param[in] max_pts_per_cluster maximum cluster size. (default: all the points) + */ + void + extractEuclideanClustersSmooth (const pcl::PointCloud &cloud, + const pcl::PointCloud &normals, float tolerance, + const pcl::search::Search::Ptr &tree, + std::vector &clusters, double eps_angle, + unsigned int min_pts_per_cluster = 1, + unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()); + + protected: + /** \brief Centroids that were used to compute different CVFH descriptors */ + std::vector > centroids_dominant_orientations_; + /** \brief Normal centroids that were used to compute different CVFH descriptors */ + std::vector > dominant_normals_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/don.h b/deps_install/include/pcl-1.10/pcl/features/don.h new file mode 100755 index 0000000..4112580 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/don.h @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Yani Ioannou + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief A Difference of Normals (DoN) scale filter implementation for point cloud data. + * + * For each point in the point cloud two normals estimated with a differing search radius (sigma_s, sigma_l) + * are subtracted, the difference of these normals provides a scale-based feature which + * can be further used to filter the point cloud, somewhat like the Difference of Guassians + * in image processing, but instead on surfaces. Best results are had when the two search + * radii are related as sigma_l=10*sigma_s, the octaves between the two search radii + * can be though of as a filter bandwidth. For appropriate values and thresholds it + * can be used for surface edge extraction. + * + * \attention The input normals given by setInputNormalsSmall and setInputNormalsLarge have + * to match the input point cloud given by setInputCloud. This behavior is different than + * feature estimation methods that extend FeatureFromNormals, which match the normals + * with the search surface. + * + * \note For more information please see + * Yani Ioannou. Automatic Urban Modelling using Mobile Urban LIDAR Data. + * Thesis (Master, Computing), Queen's University, March, 2010. + * + * \author Yani Ioannou. + * \ingroup features + */ + template + class DifferenceOfNormalsEstimation : public Feature + { + using Feature::getClassName; + using Feature::feature_name_; + using PCLBase::input_; + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + using PointCloudOut = typename Feature::PointCloudOut; + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** + * Creates a new Difference of Normals filter. + */ + DifferenceOfNormalsEstimation () + { + feature_name_ = "DifferenceOfNormalsEstimation"; + } + + ~DifferenceOfNormalsEstimation () + { + // + } + + /** + * Set the normals calculated using a smaller search radius (scale) for the DoN operator. + * @param normals the smaller radius (scale) of the DoN filter. + */ + inline void + setNormalScaleSmall (const PointCloudNConstPtr &normals) + { + input_normals_small_ = normals; + } + + /** + * Set the normals calculated using a larger search radius (scale) for the DoN operator. + * @param normals the larger radius (scale) of the DoN filter. + */ + inline void + setNormalScaleLarge (const PointCloudNConstPtr &normals) + { + input_normals_large_ = normals; + } + + /** + * Computes the DoN vector for each point in the input point cloud and outputs the vector cloud to the given output. + * @param output the cloud to output the DoN vector cloud to. + */ + void + computeFeature (PointCloudOut &output) override; + + /** + * Initialize for computation of features. + * @return true if parameters (input normals, input) are sufficient to perform computation. + */ + bool + initCompute () override; + private: + /** \brief Make the compute (&PointCloudOut); inaccessible from outside the class + * \param[out] output the output point cloud + */ + void + compute (PointCloudOut &) {} + + ///The smallest radius (scale) used in the DoN filter. + PointCloudNConstPtr input_normals_small_; + ///The largest radius (scale) used in the DoN filter. + PointCloudNConstPtr input_normals_large_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/eigen.h b/deps_install/include/pcl-1.10/pcl/features/eigen.h new file mode 100755 index 0000000..3fcbb27 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/eigen.h @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/features/esf.h b/deps_install/include/pcl-1.10/pcl/features/esf.h new file mode 100755 index 0000000..503d110 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/esf.h @@ -0,0 +1,141 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: pfh.hpp 5027 2012-03-12 03:10:45Z rusu $ + * + */ + +#pragma once + +#include +#define GRIDSIZE 64 +#define GRIDSIZE_H GRIDSIZE/2 +#include + +namespace pcl +{ + /** \brief @b ESFEstimation estimates the ensemble of shape functions descriptors for a given point cloud + * dataset containing points. Shape functions are D2, D3, A3. For more information about the ESF descriptor, see: + * Walter Wohlkinger and Markus Vincze, "Ensemble of Shape Functions for 3D Object Classification", + * IEEE International Conference on Robotics and Biomimetics (IEEE-ROBIO), 2011 + * \author Walter Wohlkinger + * \ingroup features + */ + + template + class ESFEstimation: public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_radius_; + using Feature::input_; + using Feature::surface_; + + using PointCloudIn = pcl::PointCloud; + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Empty constructor. */ + ESFEstimation () : local_cloud_ () + { + feature_name_ = "ESFEstimation"; + lut_.resize (GRIDSIZE); + for (int i = 0; i < GRIDSIZE; ++i) + { + lut_[i].resize (GRIDSIZE); + for (int j = 0; j < GRIDSIZE; ++j) + lut_[i][j].resize (GRIDSIZE); + } + //lut_.resize (boost::extents[GRIDSIZE][GRIDSIZE][GRIDSIZE]); + search_radius_ = 0; + k_ = 5; + } + + /** \brief Overloaded computed method from pcl::Feature. + * \param[out] output the resultant point cloud model dataset containing the estimated features + */ + void + compute (PointCloudOut &output); + + protected: + + /** \brief Estimate the Ensebmel of Shape Function (ESF) descriptors at a set of points given by + * &hist); + + /** \brief ... */ + void + voxelize9 (PointCloudIn &cluster); + + /** \brief ... */ + void + cleanup9 (PointCloudIn &cluster); + + /** \brief ... */ + void + scale_points_unit_sphere (const pcl::PointCloud &pc, float scalefactor, Eigen::Vector4f& centroid); + + private: + + /** \brief ... */ + std::vector > > lut_; + + /** \brief ... */ + PointCloudIn local_cloud_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/feature.h b/deps_install/include/pcl-1.10/pcl/features/feature.h new file mode 100755 index 0000000..1d54d60 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/feature.h @@ -0,0 +1,498 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +// PCL includes +#include +#include +#include + +#include + +namespace pcl +{ + /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares + * plane normal and surface curvature. + * \param covariance_matrix the 3x3 covariance matrix + * \param point a point lying on the least-squares plane (SSE aligned) + * \param plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0) + * \param curvature the estimated surface curvature as a measure of + * \f[ + * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) + * \f] + * \ingroup features + */ + inline void + solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + const Eigen::Vector4f &point, + Eigen::Vector4f &plane_parameters, float &curvature); + + /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares + * plane normal and surface curvature. + * \param covariance_matrix the 3x3 covariance matrix + * \param nx the resultant X component of the plane normal + * \param ny the resultant Y component of the plane normal + * \param nz the resultant Z component of the plane normal + * \param curvature the estimated surface curvature as a measure of + * \f[ + * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) + * \f] + * \ingroup features + */ + inline void + solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + float &nx, float &ny, float &nz, float &curvature); + + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Feature represents the base feature class. Some generic 3D operations that + * are applicable to all features are defined here as static methods. + * + * \attention + * The convention for a feature descriptor is: + * - if the nearest neighbors for the query point at which the descriptor is to be computed cannot be + * determined, the descriptor values will be set to NaN (not a number) + * - it is impossible to estimate a feature descriptor for a point that doesn't have finite 3D coordinates. + * Therefore, any point that has NaN data on x, y, or z, will most likely have its descriptor set to NaN. + * + * \author Radu B. Rusu + * \ingroup features + */ + template + class Feature : public PCLBase + { + public: + using PCLBase::indices_; + using PCLBase::input_; + + using BaseClass = PCLBase; + + using Ptr = shared_ptr< Feature >; + using ConstPtr = shared_ptr< const Feature >; + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudOut = pcl::PointCloud; + + using SearchMethod = std::function &, std::vector &)>; + using SearchMethodSurface = std::function &, std::vector &)>; + + public: + /** \brief Empty constructor. */ + Feature () : + feature_name_ (), search_method_surface_ (), + surface_(), tree_(), + search_parameter_(0), search_radius_(0), k_(0), + fake_surface_(false) + {} + + /** \brief Empty destructor */ + virtual ~Feature () {} + + /** \brief Provide a pointer to a dataset to add additional information + * to estimate the features for every point in the input dataset. This + * is optional, if this is not set, it will only use the data in the + * input cloud to estimate the features. This is useful when you only + * need to compute the features for a downsampled cloud. + * \param[in] cloud a pointer to a PointCloud message + */ + inline void + setSearchSurface (const PointCloudInConstPtr &cloud) + { + surface_ = cloud; + fake_surface_ = false; + //use_surface_ = true; + } + + /** \brief Get a pointer to the surface point cloud dataset. */ + inline PointCloudInConstPtr + getSearchSurface () const + { + return (surface_); + } + + /** \brief Provide a pointer to the search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () const + { + return (tree_); + } + + /** \brief Get the internal search parameter. */ + inline double + getSearchParameter () const + { + return (search_parameter_); + } + + /** \brief Set the number of k nearest neighbors to use for the feature estimation. + * \param[in] k the number of k-nearest neighbors + */ + inline void + setKSearch (int k) { k_ = k; } + + /** \brief get the number of k nearest neighbors used for the feature estimation. */ + inline int + getKSearch () const + { + return (k_); + } + + /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature + * estimation. + * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor + */ + inline void + setRadiusSearch (double radius) + { + search_radius_ = radius; + } + + /** \brief Get the sphere radius used for determining the neighbors. */ + inline double + getRadiusSearch () const + { + return (search_radius_); + } + + /** \brief Base method for feature estimation for all points given in + * using the surface in setSearchSurface () + * and the spatial locator in setSearchMethod () + * \param[out] output the resultant point cloud model dataset containing the estimated features + */ + void + compute (PointCloudOut &output); + + protected: + /** \brief The feature name. */ + std::string feature_name_; + + /** \brief The search method template for points. */ + SearchMethodSurface search_method_surface_; + + /** \brief An input point cloud describing the surface that is to be used + * for nearest neighbors estimation. + */ + PointCloudInConstPtr surface_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The actual search parameter (from either \a search_radius_ or \a k_). */ + double search_parameter_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief The number of K nearest neighbors to use for each point. */ + int k_; + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const { return (feature_name_); } + + /** \brief This method should get called before starting the actual computation. */ + virtual bool + initCompute (); + + /** \brief This method should get called after ending the actual computation. */ + virtual bool + deinitCompute (); + + /** \brief If no surface is given, we use the input PointCloud as the surface. */ + bool fake_surface_; + + /** \brief Search for k-nearest neighbors using the spatial locator from + * \a setSearchmethod, and the given surface from \a setSearchSurface. + * \param[in] index the index of the query point + * \param[in] parameter the search parameter (either k or radius) + * \param[out] indices the resultant vector of indices representing the k-nearest neighbors + * \param[out] distances the resultant vector of distances representing the distances from the query point to the + * k-nearest neighbors + * + * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0. + */ + inline int + searchForNeighbors (std::size_t index, double parameter, + std::vector &indices, std::vector &distances) const + { + return (search_method_surface_ (*input_, index, parameter, indices, distances)); + } + + /** \brief Search for k-nearest neighbors using the spatial locator from + * \a setSearchmethod, and the given surface from \a setSearchSurface. + * \param[in] cloud the query point cloud + * \param[in] index the index of the query point in \a cloud + * \param[in] parameter the search parameter (either k or radius) + * \param[out] indices the resultant vector of indices representing the k-nearest neighbors + * \param[out] distances the resultant vector of distances representing the distances from the query point to the + * k-nearest neighbors + * + * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0. + */ + inline int + searchForNeighbors (const PointCloudIn &cloud, std::size_t index, double parameter, + std::vector &indices, std::vector &distances) const + { + return (search_method_surface_ (cloud, index, parameter, indices, distances)); + } + + private: + /** \brief Abstract feature estimation method. + * \param[out] output the resultant features + */ + virtual void + computeFeature (PointCloudOut &output) = 0; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + template + class FeatureFromNormals : public Feature + { + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + using PointCloudOut = typename Feature::PointCloudOut; + + public: + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Ptr = shared_ptr< FeatureFromNormals >; + using ConstPtr = shared_ptr< const FeatureFromNormals >; + + // Members derived from the base class + using Feature::input_; + using Feature::surface_; + using Feature::getClassName; + + /** \brief Empty constructor. */ + FeatureFromNormals () : normals_ () {} + + /** \brief Empty destructor */ + virtual ~FeatureFromNormals () {} + + /** \brief Provide a pointer to the input dataset that contains the point normals of + * the XYZ dataset. + * In case of search surface is set to be different from the input cloud, + * normals should correspond to the search surface, not the input cloud! + * \param[in] normals the const boost shared pointer to a PointCloud of normals. + * By convention, L2 norm of each normal should be 1. + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; } + + /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */ + inline PointCloudNConstPtr + getInputNormals () const { return (normals_); } + + protected: + /** \brief A pointer to the input dataset that contains the point normals of the XYZ + * dataset. + */ + PointCloudNConstPtr normals_; + + /** \brief This method should get called before starting the actual computation. */ + virtual bool + initCompute (); + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + template + class FeatureFromLabels : public Feature + { + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + using PointCloudOut = typename Feature::PointCloudOut; + + public: + using Ptr = shared_ptr< FeatureFromLabels >; + using ConstPtr = shared_ptr< const FeatureFromLabels >; + + // Members derived from the base class + using Feature::input_; + using Feature::surface_; + using Feature::getClassName; + using Feature::k_; + + /** \brief Empty constructor. */ + FeatureFromLabels () : labels_ () + { + k_ = 1; // Search tree is not always used here. + } + + /** \brief Empty destructor */ + virtual ~FeatureFromLabels () {} + + /** \brief Provide a pointer to the input dataset that contains the point labels of + * the XYZ dataset. + * In case of search surface is set to be different from the input cloud, + * labels should correspond to the search surface, not the input cloud! + * \param[in] labels the const boost shared pointer to a PointCloud of labels. + */ + inline void + setInputLabels (const PointCloudLConstPtr &labels) + { + labels_ = labels; + } + + /** \brief Get a pointer to the labels of the input XYZ point cloud dataset. */ + inline PointCloudLConstPtr + getInputLabels () const + { + return (labels_); + } + + protected: + /** \brief A pointer to the input dataset that contains the point labels of the XYZ + * dataset. + */ + PointCloudLConstPtr labels_; + + /** \brief This method should get called before starting the actual computation. */ + virtual bool + initCompute (); + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief FeatureWithLocalReferenceFrames provides a public interface for descriptor + * extractor classes which need a local reference frame at each input keypoint. + * + * \attention + * This interface is for backward compatibility with existing code and in the future it could be + * merged with pcl::Feature. Subclasses should call the protected method initLocalReferenceFrames () + * to correctly initialize the frames_ member. + * + * \author Nicola Fioraio + * \ingroup features + */ + template + class FeatureWithLocalReferenceFrames + { + public: + using PointCloudLRF = pcl::PointCloud; + using PointCloudLRFPtr = typename PointCloudLRF::Ptr; + using PointCloudLRFConstPtr = typename PointCloudLRF::ConstPtr; + + /** \brief Empty constructor. */ + FeatureWithLocalReferenceFrames () : frames_ (), frames_never_defined_ (true) {} + + /** \brief Empty destructor. */ + virtual ~FeatureWithLocalReferenceFrames () {} + + /** \brief Provide a pointer to the input dataset that contains the local + * reference frames of the XYZ dataset. + * In case of search surface is set to be different from the input cloud, + * local reference frames should correspond to the input cloud, not the search surface! + * \param[in] frames the const boost shared pointer to a PointCloud of reference frames. + */ + inline void + setInputReferenceFrames (const PointCloudLRFConstPtr &frames) + { + frames_ = frames; + frames_never_defined_ = false; + } + + /** \brief Get a pointer to the local reference frames. */ + inline PointCloudLRFConstPtr + getInputReferenceFrames () const + { + return (frames_); + } + + protected: + /** \brief A boost shared pointer to the local reference frames. */ + PointCloudLRFConstPtr frames_; + /** \brief The user has never set the frames. */ + bool frames_never_defined_; + + /** \brief Check if frames_ has been correctly initialized and compute it if needed. + * \param input the subclass' input cloud dataset. + * \param lrf_estimation a pointer to a local reference frame estimation class to be used as default. + * \return true if frames_ has been correctly initialized. + */ + using LRFEstimationPtr = typename Feature::Ptr; + virtual bool + initLocalReferenceFrames (const std::size_t& indices_size, + const LRFEstimationPtr& lrf_estimation = LRFEstimationPtr()); + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/features/flare.h b/deps_install/include/pcl-1.10/pcl/features/flare.h new file mode 100755 index 0000000..b0f37e4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/flare.h @@ -0,0 +1,290 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2016-, Open Perception, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* +*/ + +#pragma once + +#include +#include +#include + + +namespace pcl +{ + + /** \brief FLARELocalReferenceFrameEstimation implements the Fast LocAl Reference framE algorithm + * for local reference frame estimation as described here: + * + * - A. Petrelli, L. Di Stefano, + * "A repeatable and efficient canonical reference for surface matching", + * 3DimPVT, 2012 + * + * FLARE algorithm is deployed in ReLOC algorithm proposed in: + * + * Petrelli A., Di Stefano L., "Pairwise registration by local orientation cues", Computer Graphics Forum, 2015. + * + * \author Alioscia Petrelli + * \ingroup features + */ + template + class FLARELocalReferenceFrameEstimation : public FeatureFromNormals + { + protected: + using Feature::feature_name_; + using Feature::input_; + using Feature::indices_; + using Feature::surface_; + using Feature::tree_; + using Feature::search_parameter_; + using FeatureFromNormals::normals_; + using Feature::fake_surface_; + using Feature::getClassName; + + using typename Feature::PointCloudIn; + using typename Feature::PointCloudOut; + + using typename Feature::PointCloudInConstPtr; + + using typename Feature::KdTreePtr; + + using PointCloudSignedDistance = pcl::PointCloud; + using PointCloudSignedDistancePtr = typename PointCloudSignedDistance::Ptr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + public: + /** \brief Constructor. */ + FLARELocalReferenceFrameEstimation () : + tangent_radius_ (0.0f), + margin_thresh_ (0.85f), + min_neighbors_for_normal_axis_ (6), + min_neighbors_for_tangent_axis_ (6), + sampled_surface_ (), + sampled_tree_ (), + fake_sampled_surface_ (false) + { + feature_name_ = "FLARELocalReferenceFrameEstimation"; + } + + //Getters/Setters + + /** \brief Set the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point. + * + * \param[in] radius The search radius for x axis. + */ + inline void + setTangentRadius (float radius) + { + tangent_radius_ = radius; + } + + /** \brief Get the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point. + * + * \return The search radius for x axis. + */ + inline float + getTangentRadius () const + { + return (tangent_radius_); + } + + /** \brief Set the percentage of the search tangent radius after which a point is considered part of the support. + * + * \param[in] margin_thresh the percentage of the search tangent radius after which a point is considered part of the support. + */ + inline void + setMarginThresh (float margin_thresh) + { + margin_thresh_ = margin_thresh; + } + + /** \brief Get the percentage of the search tangent radius after which a point is considered part of the support. + * + * \return The percentage of the search tangent radius after which a point is considered part of the support. + */ + inline float + getMarginThresh () const + { + return (margin_thresh_); + } + + + /** \brief Set min number of neighbours required for the computation of Z axis. + * + * \param[in] min_neighbors_for_normal_axis min number of neighbours required for the computation of Z axis. + */ + inline void + setMinNeighboursForNormalAxis (int min_neighbors_for_normal_axis) + { + min_neighbors_for_normal_axis_ = min_neighbors_for_normal_axis; + } + + /** \brief Get min number of neighbours required for the computation of Z axis. + * + * \return min number of neighbours required for the computation of Z axis. + */ + inline int + getMinNeighboursForNormalAxis () const + { + return (min_neighbors_for_normal_axis_); + } + + + /** \brief Set min number of neighbours required for the computation of X axis. + * + * \param[in] min_neighbors_for_tangent_axis min number of neighbours required for the computation of X axis. + */ + inline void + setMinNeighboursForTangentAxis (int min_neighbors_for_tangent_axis) + { + min_neighbors_for_tangent_axis_ = min_neighbors_for_tangent_axis; + } + + /** \brief Get min number of neighbours required for the computation of X axis. + * + * \return min number of neighbours required for the computation of X axis. + */ + inline int + getMinNeighboursForTangentAxis () const + { + return (min_neighbors_for_tangent_axis_); + } + + + /** \brief Provide a pointer to the dataset used for the estimation of X axis. + * As the estimation of x axis is negligibly affected by surface downsampling, + * this method lets to consider a downsampled version of surface_ in the estimation of x axis. + * This is optional, if this is not set, it will only use the data in the + * surface_ cloud to estimate the x axis. + * \param[in] cloud a pointer to a PointCloud + */ + inline void + setSearchSampledSurface(const PointCloudInConstPtr &cloud) + { + sampled_surface_ = cloud; + fake_sampled_surface_ = false; + } + + /** \brief Get a pointer to the sampled_surface_ cloud dataset. */ + inline const PointCloudInConstPtr& + getSearchSampledSurface() const + { + return (sampled_surface_); + } + + /** \brief Provide a pointer to the search object linked to sampled_surface. + * \param[in] tree a pointer to the spatial search object linked to sampled_surface. + */ + inline void + setSearchMethodForSampledSurface (const KdTreePtr &tree) { sampled_tree_ = tree; } + + /** \brief Get a pointer to the search method used for the extimation of x axis. */ + inline const KdTreePtr& + getSearchMethodForSampledSurface () const + { + return (sampled_tree_); + } + + /** \brief Get the signed distances of the highest points from the fitted planes. */ + inline const std::vector & + getSignedDistancesFromHighestPoints () const + { + return (signed_distances_from_highest_points_); + } + + protected: + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief This method should get called after the actual computation is ended. */ + bool + deinitCompute () override; + + /** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] index the index of the point in input_ + * \param[out] lrf the resultant local reference frame + * \return signed distance of the highest point from the fitted plane. Max if the lrf is not computable. + */ + SignedDistanceT + computePointLRF (const int index, Eigen::Matrix3f &lrf); + + /** \brief Abstract feature estimation method. + * \param[out] output the resultant features + */ + void + computeFeature (PointCloudOut &output) override; + + + private: + /** \brief Radius used to find tangent axis. */ + float tangent_radius_; + + /** \brief Threshold that define if a support point is near the margins. */ + float margin_thresh_; + + /** \brief Min number of neighbours required for the computation of Z axis. Otherwise, feature point normal is used. */ + int min_neighbors_for_normal_axis_; + + /** \brief Min number of neighbours required for the computation of X axis. Otherwise, a random X axis is set */ + int min_neighbors_for_tangent_axis_; + + /** \brief An input point cloud describing the surface that is to be used + * for nearest neighbor searches for the estimation of X axis. + */ + PointCloudInConstPtr sampled_surface_; + + /** \brief A pointer to the spatial search object used for the estimation of X axis. */ + KdTreePtr sampled_tree_; + + /** \brief Class for normal estimation. */ + NormalEstimation normal_estimation_; + + /** \brief Signed distances of the highest points from the fitted planes.*/ + std::vector signed_distances_from_highest_points_; + + /** \brief If no sampled_surface_ is given, we use surface_ as the sampled surface. */ + bool fake_sampled_surface_; + + }; + +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/fpfh.h b/deps_install/include/pcl-1.10/pcl/features/fpfh.h new file mode 100755 index 0000000..5d3b64b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/fpfh.h @@ -0,0 +1,222 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief FPFHEstimation estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point + * cloud dataset containing points and normals. + * + * A commonly used type for PointOutT is pcl::FPFHSignature33. + * + * \note If you use this code in any academic work, please cite: + * + * - R.B. Rusu, N. Blodow, M. Beetz. + * Fast Point Feature Histograms (FPFH) for 3D Registration. + * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), + * Kobe, Japan, May 12-17 2009. + * - R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. + * Fast Geometric Point Labeling using Conditional Random Fields. + * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * St. Louis, MO, USA, October 11-15 2009. + * + * \attention + * The convention for FPFH features is: + * - if a query point's nearest neighbors cannot be estimated, the FPFH feature will be set to NaN + * (not a number) + * - it is impossible to estimate a FPFH descriptor for a point that + * doesn't have finite 3D coordinates. Therefore, any point that contains + * NaN data on x, y, or z, will have its FPFH feature property set to NaN. + * + * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \ref FPFHEstimationOMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * + * \author Radu B. Rusu + * \ingroup features + */ + template + class FPFHEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::input_; + using Feature::surface_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Empty constructor. */ + FPFHEstimation () : + nr_bins_f1_ (11), nr_bins_f2_ (11), nr_bins_f3_ (11), + d_pi_ (1.0f / (2.0f * static_cast (M_PI))) + { + feature_name_ = "FPFHEstimation"; + }; + + /** \brief Compute the 4-tuple representation containing the three angles and one distance between two points + * represented by Cartesian coordinates and normals. + * \note For explanations about the features, please see the literature mentioned above (the order of the + * features might be different). + * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points + * \param[in] normals the dataset containing the surface normals (assuming normalized vectors) at each point in cloud + * \param[in] p_idx the index of the first point (source) + * \param[in] q_idx the index of the second point (target) + * \param[out] f1 the first angular feature (angle between the projection of nq_idx and u) + * \param[out] f2 the second angular feature (angle between nq_idx and v) + * \param[out] f3 the third angular feature (angle between np_idx and |p_idx - q_idx|) + * \param[out] f4 the distance feature (p_idx - q_idx) + */ + bool + computePairFeatures (const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4); + + /** \brief Estimate the SPFH (Simple Point Feature Histograms) individual signatures of the three angular + * (f1, f2, f3) features for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points + * \param[in] normals the dataset containing the surface normals at each point in \a cloud + * \param[in] p_idx the index of the query point (source) + * \param[in] row the index row in feature histogramms + * \param[in] indices the k-neighborhood point indices in the dataset + * \param[out] hist_f1 the resultant SPFH histogram for feature f1 + * \param[out] hist_f2 the resultant SPFH histogram for feature f2 + * \param[out] hist_f3 the resultant SPFH histogram for feature f3 + */ + void + computePointSPFHSignature (const pcl::PointCloud &cloud, + const pcl::PointCloud &normals, int p_idx, int row, + const std::vector &indices, + Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3); + + /** \brief Weight the SPFH (Simple Point Feature Histograms) individual histograms to create the final FPFH + * (Fast Point Feature Histogram) for a given point based on its 3D spatial neighborhood + * \param[in] hist_f1 the histogram feature vector of \a f1 values over the given patch + * \param[in] hist_f2 the histogram feature vector of \a f2 values over the given patch + * \param[in] hist_f3 the histogram feature vector of \a f3 values over the given patch + * \param[in] indices the point indices of p_idx's k-neighborhood in the point cloud + * \param[in] dists the distances from p_idx to all its k-neighbors + * \param[out] fpfh_histogram the resultant FPFH histogram representing the feature at the query point + */ + void + weightPointSPFHSignature (const Eigen::MatrixXf &hist_f1, + const Eigen::MatrixXf &hist_f2, + const Eigen::MatrixXf &hist_f3, + const std::vector &indices, + const std::vector &dists, + Eigen::VectorXf &fpfh_histogram); + + /** \brief Set the number of subdivisions for each angular feature interval. + * \param[in] nr_bins_f1 number of subdivisions for the first angular feature + * \param[in] nr_bins_f2 number of subdivisions for the second angular feature + * \param[in] nr_bins_f3 number of subdivisions for the third angular feature + */ + inline void + setNrSubdivisions (int nr_bins_f1, int nr_bins_f2, int nr_bins_f3) + { + nr_bins_f1_ = nr_bins_f1; + nr_bins_f2_ = nr_bins_f2; + nr_bins_f3_ = nr_bins_f3; + } + + /** \brief Get the number of subdivisions for each angular feature interval. + * \param[out] nr_bins_f1 number of subdivisions for the first angular feature + * \param[out] nr_bins_f2 number of subdivisions for the second angular feature + * \param[out] nr_bins_f3 number of subdivisions for the third angular feature + */ + inline void + getNrSubdivisions (int &nr_bins_f1, int &nr_bins_f2, int &nr_bins_f3) + { + nr_bins_f1 = nr_bins_f1_; + nr_bins_f2 = nr_bins_f2_; + nr_bins_f3 = nr_bins_f3_; + } + + protected: + + /** \brief Estimate the set of all SPFH (Simple Point Feature Histograms) signatures for the input cloud + * \param[out] spf_hist_lookup a lookup table for all the SPF feature indices + * \param[out] hist_f1 the resultant SPFH histogram for feature f1 + * \param[out] hist_f2 the resultant SPFH histogram for feature f2 + * \param[out] hist_f3 the resultant SPFH histogram for feature f3 + */ + void + computeSPFHSignatures (std::vector &spf_hist_lookup, + Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3); + + /** \brief Estimate the Fast Point Feature Histograms (FPFH) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains the FPFH feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief The number of subdivisions for each angular feature interval. */ + int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_; + + /** \brief Placeholder for the f1 histogram. */ + Eigen::MatrixXf hist_f1_; + + /** \brief Placeholder for the f2 histogram. */ + Eigen::MatrixXf hist_f2_; + + /** \brief Placeholder for the f3 histogram. */ + Eigen::MatrixXf hist_f3_; + + /** \brief Placeholder for a point's FPFH signature. */ + Eigen::VectorXf fpfh_histogram_; + + /** \brief Float constant = 1.0 / (2.0 * M_PI) */ + float d_pi_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/fpfh_omp.h b/deps_install/include/pcl-1.10/pcl/features/fpfh_omp.h new file mode 100755 index 0000000..fd7e49a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/fpfh_omp.h @@ -0,0 +1,130 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point cloud + * dataset containing points and normals, in parallel, using the OpenMP standard. + * + * \note If you use this code in any academic work, please cite: + * + * - R.B. Rusu, N. Blodow, M. Beetz. + * Fast Point Feature Histograms (FPFH) for 3D Registration. + * In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), + * Kobe, Japan, May 12-17 2009. + * - R.B. Rusu, A. Holzbach, N. Blodow, M. Beetz. + * Fast Geometric Point Labeling using Conditional Random Fields. + * In Proceedings of the 22nd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * St. Louis, MO, USA, October 11-15 2009. + * + * \attention + * The convention for FPFH features is: + * - if a query point's nearest neighbors cannot be estimated, the FPFH feature will be set to NaN + * (not a number) + * - it is impossible to estimate a FPFH descriptor for a point that + * doesn't have finite 3D coordinates. Therefore, any point that contains + * NaN data on x, y, or z, will have its FPFH feature property set to NaN. + * + * \author Radu B. Rusu + * \ingroup features + */ + template + class FPFHEstimationOMP : public FPFHEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::input_; + using Feature::surface_; + using FeatureFromNormals::normals_; + using FPFHEstimation::hist_f1_; + using FPFHEstimation::hist_f2_; + using FPFHEstimation::hist_f3_; + using FPFHEstimation::weightPointSPFHSignature; + + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param[in] nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + FPFHEstimationOMP (unsigned int nr_threads = 0) : nr_bins_f1_ (11), nr_bins_f2_ (11), nr_bins_f3_ (11) + { + feature_name_ = "FPFHEstimationOMP"; + + setNumberOfThreads(nr_threads); + } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param[in] nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + private: + /** \brief Estimate the Fast Point Feature Histograms (FPFH) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains the FPFH feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + public: + /** \brief The number of subdivisions for each angular feature interval. */ + int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_; + private: + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/from_meshes.h b/deps_install/include/pcl-1.10/pcl/features/from_meshes.h new file mode 100755 index 0000000..b7e6bab --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/from_meshes.h @@ -0,0 +1,102 @@ +#pragma once + +#include "pcl/features/normal_3d.h" +#include "pcl/Vertices.h" + +namespace pcl +{ + namespace features + { + + /** \brief Compute approximate surface normals on a mesh. + * \param[in] cloud Point cloud containing the XYZ coordinates. + * \param[in] polygons Polygons from the mesh. + * \param[out] normals Point cloud with computed surface normals + */ + template inline void + computeApproximateNormals(const pcl::PointCloud& cloud, const std::vector& polygons, pcl::PointCloud& normals) + { + const auto nr_points = cloud.points.size(); + + normals.header = cloud.header; + normals.width = cloud.width; + normals.height = cloud.height; + normals.points.resize(nr_points); + + for (auto& point: normals.points) + point.getNormalVector3fMap() = Eigen::Vector3f::Zero(); + + // NOTE: for efficiency the weight is computed implicitly by using the + // cross product, this causes inaccurate normals for meshes containing + // non-triangle polygons (quads or other types) + for (const auto& polygon: polygons) + { + if (polygon.vertices.size() < 3) continue; + + // compute normal for triangle + Eigen::Vector3f vec_a_b = cloud.points[polygon.vertices[0]].getVector3fMap() - cloud.points[polygon.vertices[1]].getVector3fMap(); + Eigen::Vector3f vec_a_c = cloud.points[polygon.vertices[0]].getVector3fMap() - cloud.points[polygon.vertices[2]].getVector3fMap(); + Eigen::Vector3f normal = vec_a_b.cross(vec_a_c); + pcl::flipNormalTowardsViewpoint(cloud.points[polygon.vertices[0]], 0.0f, 0.0f, 0.0f, normal(0), normal(1), normal(2)); + + // add normal to all points in polygon + for (const auto& vertex: polygon.vertices) + normals.points[vertex].getNormalVector3fMap() += normal; + } + + for (std::size_t i = 0; i < nr_points; ++i) + { + normals.points[i].getNormalVector3fMap().normalize(); + pcl::flipNormalTowardsViewpoint(cloud.points[i], 0.0f, 0.0f, 0.0f, normals.points[i].normal_x, normals.points[i].normal_y, normals.points[i].normal_z); + } + } + + + /** \brief Compute GICP-style covariance matrices given a point cloud and + * the corresponding surface normals. + * \param[in] cloud Point cloud containing the XYZ coordinates, + * \param[in] normals Point cloud containing the corresponding surface normals. + * \param[out] covariances Vector of computed covariances. + * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001) + */ + template inline void + computeApproximateCovariances(const pcl::PointCloud& cloud, + const pcl::PointCloud& normals, + std::vector >& covariances, + double epsilon = 0.001) + { + assert(cloud.points.size() == normals.points.size()); + + const auto nr_points = cloud.points.size(); + covariances.clear (); + covariances.reserve (nr_points); + for (const auto& point: normals.points) + { + Eigen::Vector3d normal (point.normal_x, + point.normal_y, + point.normal_z); + + // compute rotation matrix + Eigen::Matrix3d rot; + Eigen::Vector3d y; + y << 0, 1, 0; + rot.row(2) = normal; + y -= normal(1) * normal; + y.normalize(); + rot.row(1) = y; + rot.row(0) = normal.cross(rot.row(1)); + + // comnpute approximate covariance + Eigen::Matrix3d cov; + cov << 1, 0, 0, + 0, 1, 0, + 0, 0, epsilon; + covariances.emplace_back (rot.transpose()*cov*rot); + } + } + + } +} + +#define PCL_INSTANTIATE_computeApproximateCovariances(T,NT) template PCL_EXPORTS void pcl::features::computeApproximateCovariances \ + (const pcl::PointCloud&, const pcl::PointCloud&, std::vector>&, double); diff --git a/deps_install/include/pcl-1.10/pcl/features/gasd.h b/deps_install/include/pcl-1.10/pcl/features/gasd.h new file mode 100755 index 0000000..6936fb1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/gasd.h @@ -0,0 +1,363 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2016-, Open Perception, Inc. + * Copyright (c) 2016, Voxar Labs, CIn-UFPE / DEINFO-UFRPE + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /// Different histogram interpolation methods + enum HistogramInterpolationMethod + { + INTERP_NONE, ///< no interpolation + INTERP_TRILINEAR, ///< trilinear interpolation + INTERP_QUADRILINEAR ///< quadrilinear interpolation + }; + + /** \brief GASDEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given + * point cloud dataset given XYZ data. + * + * The suggested PointOutT is pcl::GASDSignature512. + * + * \note If you use this code in any academic work, please cite: + * + * - J. Lima, V. Teichrieb. + * An Efficient Global Point Cloud Descriptor for Object Recognition and Pose Estimation. + * In Proceedings of the 29th SIBGRAPI - Conference on Graphics, Patterns and Images, + * Sao Jose dos Campos, Brazil, October 4-7 2016. + * + * \author Joao Paulo Lima + * + * Voxar Labs, Centro de Informatica, Universidade Federal de Pernambuco, Brazil + * + * Departamento de Estatistica e Informatica, Universidade Federal Rural de Pernambuco, Brazil + * + * \ingroup features + */ + template + class GASDEstimation : public Feature + { + public: + using typename Feature::PointCloudIn; + using typename Feature::PointCloudOut; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + * \param[in] view_direction view direction + * \param[in] shape_half_grid_size shape half grid size + * \param[in] shape_hists_size shape histograms size + * \param[in] shape_interp shape histograms interpolation method + */ + GASDEstimation (const Eigen::Vector3f &view_direction = Eigen::Vector3f (0.0f, 0.0f, 1.0f), + const std::size_t shape_half_grid_size = 4, + const std::size_t shape_hists_size = 1, + const HistogramInterpolationMethod shape_interp = INTERP_TRILINEAR) : + view_direction_ (view_direction), + shape_half_grid_size_ (shape_half_grid_size), + shape_hists_size_ (shape_hists_size), + shape_interp_ (shape_interp) + { + search_radius_ = 0; + k_ = 1; + feature_name_ = "GASDEstimation"; + } + + /** \brief Set the view direction. + * \param[in] dir view direction + */ + inline void + setViewDirection (const Eigen::Vector3f &dir) + { + view_direction_ = dir; + } + + /** \brief Set the shape half grid size. + * \param[in] shgs shape half grid size + */ + inline void + setShapeHalfGridSize (const std::size_t shgs) + { + shape_half_grid_size_ = shgs; + } + + /** \brief Set the shape histograms size. If size is 1, then each histogram bin will store the number + * of points that belong to its correspondent cell in the 3D regular grid. If size > 1, then for each cell + * it will be computed a histogram of normalized distances between each sample and the cloud centroid + * \param[in] shs shape histograms size + */ + inline void + setShapeHistsSize (const std::size_t shs) + { + shape_hists_size_ = shs; + } + + /** \brief Set the shape histograms interpolation method. + * \param[in] interp shape histograms interpolation method + */ + inline void + setShapeHistsInterpMethod (const HistogramInterpolationMethod interp) + { + shape_interp_ = interp; + } + + /** \brief Returns the transformation aligning the point cloud to the canonical coordinate system + * \param[out] trans transformation + */ + const Eigen::Matrix4f& + getTransform () const + { + return transform_; + } + + /** \brief Overloaded computed method from pcl::Feature. + * \param[out] output the resultant point cloud model dataset containing the estimated feature + */ + void + compute (PointCloudOut &output); + + protected: + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_radius_; + using Feature::surface_; + + /** \brief Point cloud aligned to the canonical coordinate system. */ + PointCloudIn shape_samples_; + + /** \brief Normalization factor with respect to axis-aligned bounding cube centered on the origin. */ + float max_coord_; + + /** \brief Normalized sample contribution with respect to the total number of points in the cloud. */ + float hist_incr_; + + /** \brief Current position of output descriptor point cloud. */ + std::size_t pos_; + + /** \brief add a sample to its respective histogram, optionally performing interpolation. + * \param[in] p histogram sample + * \param[in] max_coord normalization factor with respect to axis-aligned bounding cube centered on the origin + * \param[in] half_grid_size half size of the regular grid used to compute the descriptor + * \param[in] interp interpolation method to be used while computing the descriptor + * \param[in] hbin histogram bin + * \param[in] hist_incr normalization factor of sample contribution + * \param[in,out] hists updated histograms + */ + void + addSampleToHistograms (const Eigen::Vector4f &p, + const float max_coord, + const std::size_t half_grid_size, + const HistogramInterpolationMethod interp, + const float hbin, + const float hist_incr, + std::vector &hists); + + /** \brief Estimate GASD descriptor + * + * \param[out] output the resultant point cloud model dataset containing the GASD feature + */ + void + computeFeature (PointCloudOut &output) override; + + private: + /** \brief Transform that aligns the point cloud to the canonical coordinate system. */ + Eigen::Matrix4f transform_; + + /** \brief Viewing direction, default value is (0, 0, 1). */ + Eigen::Vector3f view_direction_; + + /** \brief Half size of the regular grid used to compute the shape descriptor. */ + std::size_t shape_half_grid_size_; + + /** \brief Size of the histograms of normalized distances between each sample and the cloud centroid. */ + std::size_t shape_hists_size_; + + /** \brief Interpolation method to be used while computing the shape descriptor. */ + HistogramInterpolationMethod shape_interp_; + + /** \brief Estimates a reference frame for the point cloud and uses it to compute a transform that aligns the point cloud to the canonical coordinate system. */ + void + computeAlignmentTransform (); + + /** \brief copy computed shape histograms to output descriptor point cloud + * \param[in] grid_size size of the regular grid used to compute the descriptor + * \param[in] hists_size size of the shape histograms + * \param[in] hists shape histograms + * \param[out] output output descriptor point cloud + * \param[in,out] pos current position of output descriptor point cloud + */ + void + copyShapeHistogramsToOutput (const std::size_t grid_size, + const std::size_t hists_size, + const std::vector &hists, + PointCloudOut &output, + std::size_t &pos); + }; + + /** \brief GASDColorEstimation estimates the Globally Aligned Spatial Distribution (GASD) descriptor for a given + * point cloud dataset given XYZ and RGB data. + * + * The suggested PointOutT is pcl::GASDSignature984. + * + * \note If you use this code in any academic work, please cite: + * + * - J. Lima, V. Teichrieb. + * An Efficient Global Point Cloud Descriptor for Object Recognition and Pose Estimation. + * In Proceedings of the 29th SIBGRAPI - Conference on Graphics, Patterns and Images, + * Sao Jose dos Campos, Brazil, October 4-7 2016. + * + * \author Joao Paulo Lima + * + * Voxar Labs, Centro de Informatica, Universidade Federal de Pernambuco, Brazil + * + * Departamento de Estatistica e Informatica, Universidade Federal Rural de Pernambuco, Brazil + * + * \ingroup features + */ + template + class GASDColorEstimation : public GASDEstimation + { + public: + using typename Feature::PointCloudOut; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + * \param[in] view_direction view direction + * \param[in] shape_half_grid_size shape half grid size + * \param[in] shape_hists_size shape histograms size + * \param[in] color_half_grid_size color half grid size + * \param[in] color_hists_size color histograms size + * \param[in] shape_interp shape histograms interpolation method + * \param[in] color_interp color histograms interpolation method + */ + GASDColorEstimation (const Eigen::Vector3f &view_direction = Eigen::Vector3f (0.0f, 0.0f, 1.0f), + const std::size_t shape_half_grid_size = 3, + const std::size_t shape_hists_size = 1, + const std::size_t color_half_grid_size = 2, + const std::size_t color_hists_size = 12, + const HistogramInterpolationMethod shape_interp = INTERP_NONE, + const HistogramInterpolationMethod color_interp = INTERP_NONE) : + GASDEstimation (view_direction, shape_half_grid_size, shape_hists_size, shape_interp), + color_half_grid_size_ (color_half_grid_size), + color_hists_size_ (color_hists_size), + color_interp_ (color_interp) + { + feature_name_ = "GASDColorEstimation"; + } + + /** \brief Set the color half grid size. + * \param[in] chgs color half grid size + */ + inline void + setColorHalfGridSize (const std::size_t chgs) + { + color_half_grid_size_ = chgs; + } + + /** \brief Set the color histograms size (number of bins in the hue histogram for each cell of the 3D regular grid). + * \param[in] chs color histograms size + */ + inline void + setColorHistsSize (const std::size_t chs) + { + color_hists_size_ = chs; + } + + /** \brief Set the color histograms interpolation method. + * \param[in] interp color histograms interpolation method + */ + inline void + setColorHistsInterpMethod (const HistogramInterpolationMethod interp) + { + color_interp_ = interp; + } + + protected: + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_radius_; + using Feature::surface_; + using GASDEstimation::shape_samples_; + using GASDEstimation::max_coord_; + using GASDEstimation::hist_incr_; + using GASDEstimation::pos_; + + private: + /** \brief Half size of the regular grid used to compute the color descriptor. */ + std::size_t color_half_grid_size_; + + /** \brief Size of the hue histograms. */ + std::size_t color_hists_size_; + + /** \brief Interpolation method to be used while computing the color descriptor. */ + HistogramInterpolationMethod color_interp_; + + /** \brief copy computed color histograms to output descriptor point cloud + * \param[in] grid_size size of the regular grid used to compute the descriptor + * \param[in] hists_size size of the color histograms + * \param[in,out] hists color histograms, which are finalized, since they are circular + * \param[out] output output descriptor point cloud + * \param[in,out] pos current position of output descriptor point cloud + */ + void + copyColorHistogramsToOutput (const std::size_t grid_size, + const std::size_t hists_size, + std::vector &hists, + PointCloudOut &output, + std::size_t &pos); + + /** \brief Estimate GASD color descriptor + * + * \param[out] output the resultant point cloud model dataset containing the GASD color feature + */ + void + computeFeature (PointCloudOut &output) override; + }; +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/gfpfh.h b/deps_install/include/pcl-1.10/pcl/features/gfpfh.h new file mode 100755 index 0000000..d42f4b7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/gfpfh.h @@ -0,0 +1,176 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: gfpfh.h 1423 2011-06-21 09:51:32Z bouffa $ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief @b GFPFHEstimation estimates the Global Fast Point Feature Histogram (GFPFH) descriptor for a given point + * cloud dataset containing points and labels. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • R.B. Rusu, A. Holzbach, M. Beetz. + * Detecting and Segmenting Objects for Mobile Manipulation. + * In the S3DV Workshop of the 12th International Conference on Computer Vision (ICCV), + * 2009. + *
    • + *
+ * + * \author Radu B. Rusu + * \ingroup features + */ + template + class GFPFHEstimation : public FeatureFromLabels + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using FeatureFromLabels::feature_name_; + using FeatureFromLabels::getClassName; + using FeatureFromLabels::indices_; + using FeatureFromLabels::k_; + using FeatureFromLabels::search_parameter_; + using FeatureFromLabels::surface_; + + using FeatureFromLabels::input_; + using FeatureFromLabels::labels_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + + /** \brief Empty constructor. */ + GFPFHEstimation () : + octree_leaf_size_ (0.01), + number_of_classes_ (16), + descriptor_size_ (PointOutT::descriptorSize ()) + { + feature_name_ = "GFPFHEstimation"; + } + + /** \brief Set the size of the octree leaves. + */ + inline void + setOctreeLeafSize (double size) { octree_leaf_size_ = size; } + + /** \brief Get the sphere radius used for determining the neighbors. */ + inline double + getOctreeLeafSize () { return (octree_leaf_size_); } + + /** \brief Return the empty label value. */ + inline std::uint32_t + emptyLabel () const { return 0; } + + /** \brief Return the number of different classes. */ + inline std::uint32_t + getNumberOfClasses () const { return number_of_classes_; } + + /** \brief Set the number of different classes. + * \param n number of different classes. + */ + inline void + setNumberOfClasses (std::uint32_t n) { number_of_classes_ = n; } + + /** \brief Return the size of the descriptor. */ + inline int + descriptorSize () const { return descriptor_size_; } + + /** \brief Overloaded computed method from pcl::Feature. + * \param[out] output the resultant point cloud model dataset containing the estimated features + */ + void + compute (PointCloudOut &output); + + protected: + + /** \brief Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param output the resultant point cloud model dataset that contains the PFH feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Return the dominant label of a set of points. */ + std::uint32_t + getDominantLabel (const std::vector& indices); + + /** \brief Compute the fixed-length histograms of transitions. */ + void computeTransitionHistograms (const std::vector< std::vector >& label_histograms, + std::vector< std::vector >& transition_histograms); + + /** \brief Compute the distance of each transition histogram to the mean. */ + void + computeDistancesToMean (const std::vector< std::vector >& transition_histograms, + std::vector& distances); + + /** \brief Return the Intersection Kernel distance between two histograms. */ + float + computeHIKDistance (const std::vector& histogram, + const std::vector& mean_histogram); + + /** \brief Compute the binned histogram of distance values. */ + void + computeDistanceHistogram (const std::vector& distances, + std::vector& histogram); + + /** \brief Compute the mean histogram of the given set of histograms. */ + void + computeMeanHistogram (const std::vector< std::vector >& histograms, + std::vector& mean_histogram); + + private: + /** \brief Size of octree leaves. */ + double octree_leaf_size_; + + /** \brief Number of possible classes/labels. */ + std::uint32_t number_of_classes_; + + /** \brief Dimension of the descriptors. */ + int descriptor_size_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/grsd.h b/deps_install/include/pcl-1.10/pcl/features/grsd.h new file mode 100755 index 0000000..4f9cbfd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/grsd.h @@ -0,0 +1,147 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2014, Willow Garage, Inc. + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b GRSDEstimation estimates the Global Radius-based Surface Descriptor (GRSD) for a given point cloud dataset + * containing points and normals. + * + * @note If you use this code in any academic work, please cite (first for the ray-casting and second for the additive version): + * + *
    + *
  • Z.C. Marton, D. Pangercic, N. Blodow, Michael Beetz. + * Combined 2D-3D Categorization and Classification for Multimodal Perception Systems. + * In The International Journal of Robotics Research, Sage Publications + * pages 1378--1402, Volume 30, Number 11, September 2011. + *
    • + *
  • A. Kanezaki, Z.C. Marton, D. Pangercic, T. Harada, Y. Kuniyoshi, M. Beetz. + * Voxelized Shape and Color Histograms for RGB-D + * In the Workshop on Active Semantic Perception and Object Search in the Real World, in conjunction with the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) + * San Francisco, California, September 25-30, 2011. + *
    • + *
+ * + * @note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \ref FPFHEstimationOMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * \author Zoltan Csaba Marton + * \ingroup features + */ + + template + class GRSDEstimation : public FeatureFromNormals + { + public: + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::search_radius_; + using Feature::search_parameter_; + using Feature::input_; + using FeatureFromNormals::normals_; + using Feature::setSearchSurface; + //using Feature::computeFeature; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudInPtr = typename Feature::PointCloudInPtr; + + /** \brief Constructor. */ + GRSDEstimation () : additive_ (true) + { + feature_name_ = "GRSDEstimation"; + relative_coordinates_all_ = getAllNeighborCellIndices (); + }; + + /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature + * estimation. Same value will be used for the internal voxel grid leaf size. + * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor + */ + inline void + setRadiusSearch (double radius) { width_ = search_radius_ = radius; } + + /** \brief Get the sphere radius used for determining the neighbors. + * \return the sphere radius used as the maximum distance to consider a point a neighbor + */ + inline double + getRadiusSearch () const { return (search_radius_); } + + /** \brief Get the type of the local surface based on the min and max radius computed. + * \return the integer that represents the type of the local surface with values as + * Plane (1), Cylinder (2), Noise or corner (0), Sphere (3) and Edge (4) + */ + static inline int + getSimpleType (float min_radius, float max_radius, + double min_radius_plane = 0.100, + double max_radius_noise = 0.015, + double min_radius_cylinder = 0.175, + double max_min_radius_diff = 0.050); + + protected: + + /** \brief Estimate the Global Radius-based Surface Descriptor (GRSD) for a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param output the resultant point cloud that contains the GRSD feature + */ + void + computeFeature (PointCloudOut &output) override; + + private: + + /** \brief Defines if an additive feature is computed or ray-casting is used to get a more descriptive feature. */ + bool additive_; + + /** \brief Defines the voxel size to be used. */ + double width_; + + /** \brief Pre-computed the relative cell indices of all the 26 neighbors. */ + Eigen::MatrixXi relative_coordinates_all_; + + }; + +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/3dsc.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/3dsc.hpp new file mode 100755 index 0000000..7b9386f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/3dsc.hpp @@ -0,0 +1,275 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_3DSC_HPP_ +#define PCL_FEATURES_IMPL_3DSC_HPP_ + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ShapeContext3DEstimation::initCompute () +{ + if (!FeatureFromNormals::initCompute ()) + { + 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 (azimuth_bins_); + float elevation_interval = 180.0f / static_cast (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 (std::size_t j = 0; j < radius_bins_ + 1; j++) + radii_interval_[j] = static_cast (std::exp (std::log (min_radius_) + ((static_cast (j) / static_cast (radius_bins_)) * std::log (search_radius_ / min_radius_)))); + + // Fill theta divisions of elevation + theta_divisions_.resize (elevation_bins_ + 1, elevation_interval); + theta_divisions_[0] = 0.f; + std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ()); + + // Fill phi didvisions of elevation + phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval); + phi_divisions_[0] = 0.f; + std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ()); + + // 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 (std::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.0f) - (radii_interval_[j] * radii_interval_[j] * radii_interval_[j] / 3.0f); + + for (std::size_t k = 0; k < elevation_bins_; k++) + { + // "theta" term of the volume integral + float integr_theta = std::cos (pcl::deg2rad (theta_divisions_[k])) - std::cos (pcl::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 (std::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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ShapeContext3DEstimation::computePoint ( + std::size_t index, const pcl::PointCloud &normals, float rf[9], std::vector &desc) +{ + // The RF is formed as this x_axis | y_axis | normal + Eigen::Map x_axis (rf); + Eigen::Map y_axis (rf + 3); + Eigen::Map normal (rf + 6); + + // Find every point within specified search_radius_ + std::vector nn_indices; + std::vector nn_dists; + const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists); + if (neighb_cnt == 0) + { + std::fill (desc.begin (), desc.end (), std::numeric_limits::quiet_NaN ()); + std::fill (rf, rf + 9, 0.f); + return (false); + } + + const auto minDistanceIt = std::min_element(nn_dists.begin (), nn_dists.end ()); + const auto minIndex = nn_indices[std::distance (nn_dists.begin (), minDistanceIt)]; + + // Get origin point + Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap (); + // Get origin normal + // Use pre-computed normals + normal = normals[minIndex].getNormalVector3fMap (); + + // Compute and store the RF direction + x_axis[0] = rnd (); + x_axis[1] = rnd (); + x_axis[2] = rnd (); + if (!pcl::utils::equal (normal[2], 0.0f)) + x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2]; + else if (!pcl::utils::equal (normal[1], 0.0f)) + x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1]; + else if (!pcl::utils::equal (normal[0], 0.0f)) + x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0]; + + x_axis.normalize (); + + // Check if the computed x axis is orthogonal to the normal + assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f)); + + // Store the 3rd frame vector + y_axis.matrix () = normal.cross (x_axis); + + // For each point within radius + for (std::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 = pcl::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 (std::acos (std::min (1.0f, std::max (-1.0f, theta)))); + + // Compute the Bin(j, k, l) coordinates of current neighbour + const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r); + const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta); + const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi); + + // Bin (j, k, l) + const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min)); + const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min)); + const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min)); + + // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour + std::vector neighbour_indices; + std::vector neighbour_distances; + int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances); + // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that + if (point_density == 0) + continue; + + float w = (1.0f / static_cast (point_density)) * + volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j]; + + assert (w >= 0.0); + if (w == std::numeric_limits::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 + + // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user + std::fill (rf, rf + 9, 0); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ShapeContext3DEstimation::computeFeature (PointCloudOut &output) +{ + assert (descriptor_length_ == 1980); + + output.is_dense = true; + // Iterate over all points and compute the descriptors + for (std::size_t point_index = 0; point_index < indices_->size (); point_index++) + { + //output[point_index].descriptor.resize (descriptor_length_); + + // If the point is not finite, set the descriptor to NaN and continue + if (!isFinite ((*input_)[(*indices_)[point_index]])) + { + std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_, + std::numeric_limits::quiet_NaN ()); + std::fill (output[point_index].rf, output[point_index].rf + 9, 0); + output.is_dense = false; + continue; + } + + std::vector descriptor (descriptor_length_); + if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor)) + output.is_dense = false; + std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor); + } +} + +#define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/board.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/board.hpp new file mode 100755 index 0000000..fcd1a41 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/board.hpp @@ -0,0 +1,605 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_FEATURES_IMPL_BOARD_H_ +#define PCL_FEATURES_IMPL_BOARD_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::directedOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, + Eigen::Vector3f &directed_ortho_axis) +{ + Eigen::Vector3f projection; + projectPointOnPlane (point, axis_origin, axis, projection); + directed_ortho_axis = projection - axis_origin; + + directed_ortho_axis.normalize (); + + // check if the computed x axis is orthogonal to the normal + //assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::projectPointOnPlane ( + Eigen::Vector3f const &point, + Eigen::Vector3f const &origin_point, + Eigen::Vector3f const &plane_normal, + Eigen::Vector3f &projected_point) +{ + float t; + Eigen::Vector3f xo; + + xo = point - origin_point; + t = plane_normal.dot (xo); + + projected_point = point - (t * plane_normal); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::BOARDLocalReferenceFrameEstimation::getAngleBetweenUnitVectors ( + Eigen::Vector3f const &v1, + Eigen::Vector3f const &v2, + Eigen::Vector3f const &axis) +{ + Eigen::Vector3f angle_orientation; + angle_orientation = v1.cross (v2); + float angle_radians = std::acos (std::max (-1.0f, std::min (1.0f, v1.dot (v2)))); + + angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast (M_PI) - angle_radians) : angle_radians; + + return (angle_radians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::randomOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis) +{ + if (!areEquals (axis.z (), 0.0f)) + { + rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z (); + } + else if (!areEquals (axis.y (), 0.0f)) + { + rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y (); + } + else if (!areEquals (axis.x (), 0.0f)) + { + rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); + } + + rand_ortho_axis.normalize (); + + // check if the computed x axis is orthogonal to the normal + //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::planeFitting ( + Eigen::Matrix const &points, + Eigen::Vector3f ¢er, + Eigen::Vector3f &norm) +{ + // ----------------------------------------------------- + // Plane Fitting using Singular Value Decomposition (SVD) + // ----------------------------------------------------- + + const auto n_points = points.rows (); + if (n_points == 0) + { + return; + } + + //find the center by averaging the points positions + center = points.colwise().mean().transpose(); + + //copy points - average (center) + const Eigen::Matrix A = points.rowwise() - center.transpose(); + + Eigen::JacobiSVD svd (A, Eigen::ComputeFullV); + norm = svd.matrixV ().col (2); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::normalDisambiguation ( + pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal) +{ + Eigen::Vector3f normal_mean; + normal_mean.setZero (); + + for (const int &normal_index : normal_indices) + { + const PointNT& curPt = normal_cloud[normal_index]; + + normal_mean += curPt.getNormalVector3fMap (); + } + + normal_mean.normalize (); + + if (normal.dot (normal_mean) < 0) + { + normal = -normal; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::BOARDLocalReferenceFrameEstimation::computePointLRF (const int &index, + Eigen::Matrix3f &lrf) +{ + //find Z axis + + //extract support points for Rz radius + std::vector neighbours_indices; + std::vector neighbours_distances; + int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); + + //check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis + if (n_neighbours < 6) + { + //PCL_WARN( + // "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n", + // getClassName().c_str(), index); + + //setting lrf to NaN + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + + return (std::numeric_limits::max ()); + } + + //copy neighbours coordinates into eigen matrix + Eigen::Matrix neigh_points_mat (n_neighbours, 3); + for (int i = 0; i < n_neighbours; ++i) + { + neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap (); + } + + Eigen::Vector3f x_axis, y_axis; + //plane fitting to find direction of Z axis + Eigen::Vector3f fitted_normal; //z_axis + Eigen::Vector3f centroid; + planeFitting (neigh_points_mat, centroid, fitted_normal); + + //disambiguate Z axis with normal mean + normalDisambiguation (*normals_, neighbours_indices, fitted_normal); + + //setting LRF Z axis + lrf.row (2).matrix () = fitted_normal; + + ///////////////////////////////////////////////////////////////////////////////////////// + //find X axis + + //extract support points for Rx radius + if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_) + { + n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances); + } + + //find point with the "most different" normal (with respect to fittedNormal) + + float min_normal_cos = std::numeric_limits::max (); + int min_normal_index = -1; + + bool margin_point_found = false; + Eigen::Vector3f best_margin_point; + bool best_point_found_on_margins = false; + + const float radius2 = tangent_radius_ * tangent_radius_; + const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2; + + float max_boundary_angle = 0; + + if (find_holes_) + { + randomOrthogonalAxis (fitted_normal, x_axis); + + lrf.row (0).matrix () = x_axis; + + check_margin_array_.assign(check_margin_array_size_, false); + margin_array_min_angle_.assign(check_margin_array_size_, std::numeric_limits::max ()); + margin_array_max_angle_.assign(check_margin_array_size_, -std::numeric_limits::max ()); + margin_array_min_angle_normal_.assign(check_margin_array_size_, -1.0); + margin_array_max_angle_normal_.assign(check_margin_array_size_, -1.0); + + max_boundary_angle = (2 * static_cast (M_PI)) / static_cast (check_margin_array_size_); + } + + for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh) + { + const int& curr_neigh_idx = neighbours_indices[curr_neigh]; + const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; + if (neigh_distance_sqr <= margin_distance2) + { + continue; + } + + //point normalIndex is inside the ring between marginThresh and Radius + margin_point_found = true; + + Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap (); + + float normal_cos = fitted_normal.dot (normal_mean); + if (normal_cos < min_normal_cos) + { + min_normal_index = curr_neigh_idx; + min_normal_cos = normal_cos; + best_point_found_on_margins = false; + } + + if (find_holes_) + { + //find angle with respect to random axis previously calculated + Eigen::Vector3f indicating_normal_vect; + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), + surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect); + float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal); + + int check_margin_array_idx = std::min (static_cast (std::floor (angle / max_boundary_angle)), check_margin_array_size_ - 1); + check_margin_array_[check_margin_array_idx] = true; + + if (angle < margin_array_min_angle_[check_margin_array_idx]) + { + margin_array_min_angle_[check_margin_array_idx] = angle; + margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos; + } + if (angle > margin_array_max_angle_[check_margin_array_idx]) + { + margin_array_max_angle_[check_margin_array_idx] = angle; + margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos; + } + } + + } //for each neighbor + + if (!margin_point_found) + { + //find among points with neighDistance <= marginThresh*radius + for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++) + { + const int& curr_neigh_idx = neighbours_indices[curr_neigh]; + const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; + + if (neigh_distance_sqr > margin_distance2) + continue; + + Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap (); + + float normal_cos = fitted_normal.dot (normal_mean); + + if (normal_cos < min_normal_cos) + { + min_normal_index = curr_neigh_idx; + min_normal_cos = normal_cos; + } + }//for each neighbor + + // Check if we are not in a degenerate case (all the neighboring normals are NaNs) + if (min_normal_index == -1) + { + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), + surface_->at (min_normal_index).getVector3fMap (), x_axis); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + + return (min_normal_cos); + } + + if (!find_holes_) + { + if (best_point_found_on_margins) + { + //if most inclined normal is on support margin + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + return (min_normal_cos); + } + + // Check if we are not in a degenerate case (all the neighboring normals are NaNs) + if (min_normal_index == -1) + { + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), + surface_->at (min_normal_index).getVector3fMap (), x_axis); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + return (min_normal_cos); + }// if(!find_holes_) + + //check if there is at least a hole + bool is_hole_present = false; + for (const auto check_margin: check_margin_array_) + { + if (!check_margin) + { + is_hole_present = true; + break; + } + } + + if (!is_hole_present) + { + if (best_point_found_on_margins) + { + //if most inclined normal is on support margin + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + return (min_normal_cos); + } + + // Check if we are not in a degenerate case (all the neighboring normals are NaNs) + if (min_normal_index == -1) + { + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + + //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), + surface_->at (min_normal_index).getVector3fMap (), x_axis); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + return (min_normal_cos); + }//if (!is_hole_present) + + //case hole found + //find missing region + float angle = 0.0; + int hole_end; + int hole_first; + + const auto find_first_no_border_pie = [](const auto& array) -> std::size_t { + if (array.back()) + { + return 0; + } + const auto result = std::find_if(array.cbegin (), array.cend (), + [](const auto& x) -> bool { return x;}); + return std::distance(array.cbegin (), result); + }; + const auto first_no_border = find_first_no_border_pie(check_margin_array_); + + //float steep_prob = 0.0; + float max_hole_prob = -std::numeric_limits::max (); + + //find holes + for (auto ch = first_no_border; ch < check_margin_array_size_; ch++) + { + if (!check_margin_array_[ch]) + { + //border beginning found + hole_first = ch; + hole_end = hole_first + 1; + while (!check_margin_array_[hole_end % check_margin_array_size_]) + { + ++hole_end; + } + //border end found, find angle + + if ((hole_end - hole_first) > 0) + { + //check if hole can be a shapeness hole + int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1)) + % check_margin_array_size_; + int following_hole = (hole_end) % check_margin_array_size_; + float normal_begin = margin_array_max_angle_normal_[previous_hole]; + float normal_end = margin_array_min_angle_normal_[following_hole]; + normal_begin -= min_normal_cos; + normal_end -= min_normal_cos; + normal_begin = normal_begin / (1.0f - min_normal_cos); + normal_end = normal_end / (1.0f - min_normal_cos); + normal_begin = 1.0f - normal_begin; + normal_end = 1.0f - normal_end; + + //evaluate P(Hole); + float hole_width = 0.0f; + if (following_hole < previous_hole) + { + hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast (M_PI) + - margin_array_max_angle_[previous_hole]; + } + else + { + hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole]; + } + float hole_prob = hole_width / (2 * static_cast (M_PI)); + + //evaluate P(zmin|Hole) + float steep_prob = (normal_end + normal_begin) / 2.0f; + + //check hole prob and after that, check steepThresh + + if (hole_prob > hole_size_prob_thresh_) + { + if (steep_prob > steep_thresh_) + { + if (hole_prob > max_hole_prob) + { + max_hole_prob = hole_prob; + + float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f; + if (following_hole < previous_hole) + { + angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2 + * static_cast (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight; + } + else + { + angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + - margin_array_max_angle_[previous_hole]) * angle_weight; + } + } + } + } + } //(hole_end-hole_first) > 0 + + if (hole_end >= check_margin_array_size_) + { + break; + } + ch = hole_end - 1; + } + } + + if (max_hole_prob > -std::numeric_limits::max ()) + { + //hole found + Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal); + x_axis = rotation * x_axis; + + min_normal_cos -= 10.0f; + } + else + { + if (best_point_found_on_margins) + { + //if most inclined normal is on support margin + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis); + } + else + { + // Check if we are not in a degenerate case (all the neighboring normals are NaNs) + if (min_normal_index == -1) + { + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + + //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), + surface_->at (min_normal_index).getVector3fMap (), x_axis); + } + } + + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + return (min_normal_cos); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::computeFeature (PointCloudOut &output) +{ + //check whether used with search radius or search k-neighbors + if (this->getKSearch () != 0) + { + PCL_ERROR( + "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", + getClassName().c_str()); + return; + } + + this->resetData (); + for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx) + { + Eigen::Matrix3f currentLrf; + PointOutT &rf = output[point_idx]; + + //rf.confidence = computePointLRF (*indices_[point_idx], currentLrf); + //if (rf.confidence == std::numeric_limits::max ()) + if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits::max ()) + { + output.is_dense = false; + } + + for (int d = 0; d < 3; ++d) + { + rf.x_axis[d] = currentLrf (0, d); + rf.y_axis[d] = currentLrf (1, d); + rf.z_axis[d] = currentLrf (2, d); + } + } +} + +#define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation; + +#endif // PCL_FEATURES_IMPL_BOARD_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/boundary.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/boundary.hpp new file mode 100755 index 0000000..9410a5b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/boundary.hpp @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_BOUNDARY_H_ +#define PCL_FEATURES_IMPL_BOUNDARY_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::BoundaryEstimation::isBoundaryPoint ( + const pcl::PointCloud &cloud, int q_idx, + const std::vector &indices, + const Eigen::Vector4f &u, const Eigen::Vector4f &v, + const float angle_threshold) +{ + return (isBoundaryPoint (cloud, cloud.points[q_idx], indices, u, v, angle_threshold)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::BoundaryEstimation::isBoundaryPoint ( + const pcl::PointCloud &cloud, const PointInT &q_point, + const std::vector &indices, + const Eigen::Vector4f &u, const Eigen::Vector4f &v, + const float angle_threshold) +{ + if (indices.size () < 3) + return (false); + + if (!std::isfinite (q_point.x) || !std::isfinite (q_point.y) || !std::isfinite (q_point.z)) + return (false); + + // Compute the angles between each neighboring point and the query point itself + std::vector angles (indices.size ()); + float max_dif = FLT_MIN, dif; + int cp = 0; + + for (const int &index : indices) + { + if (!std::isfinite (cloud.points[index].x) || + !std::isfinite (cloud.points[index].y) || + !std::isfinite (cloud.points[index].z)) + continue; + + Eigen::Vector4f delta = cloud.points[index].getVector4fMap () - q_point.getVector4fMap (); + if (delta == Eigen::Vector4f::Zero()) + continue; + + angles[cp++] = std::atan2 (v.dot (delta), u.dot (delta)); // the angles are fine between -PI and PI too + } + if (cp == 0) + return (false); + + angles.resize (cp); + std::sort (angles.begin (), angles.end ()); + + // Compute the maximal angle difference between two consecutive angles + for (std::size_t i = 0; i < angles.size () - 1; ++i) + { + dif = angles[i + 1] - angles[i]; + if (max_dif < dif) + max_dif = dif; + } + // Get the angle difference between the last and the first + dif = 2 * static_cast (M_PI) - angles[angles.size () - 1] + angles[0]; + if (max_dif < dif) + max_dif = dif; + + // Check results + return (max_dif > angle_threshold); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BoundaryEstimation::computeFeature (PointCloudOut &output) +{ + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero (); + + output.is_dense = true; + // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense + if (input_->is_dense) + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + output.points[idx].boundary_point = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + // Obtain a coordinate system on the least-squares plane + //v = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal (); + //u = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v); + getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v); + + // Estimate whether the point is lying on a boundary surface or not + output.points[idx].boundary_point = isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_); + } + } + else + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + output.points[idx].boundary_point = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + // Obtain a coordinate system on the least-squares plane + //v = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal (); + //u = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v); + getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v); + + // Estimate whether the point is lying on a boundary surface or not + output.points[idx].boundary_point = isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_); + } + } +} + +#define PCL_INSTANTIATE_BoundaryEstimation(PointInT,PointNT,PointOutT) template class PCL_EXPORTS pcl::BoundaryEstimation; + +#endif // PCL_FEATURES_IMPL_BOUNDARY_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/brisk_2d.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/brisk_2d.hpp new file mode 100755 index 0000000..0ee753e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/brisk_2d.hpp @@ -0,0 +1,676 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception , Inc. + * Copyright (C) 2011 The Autonomous Systems Lab (ASL), ETH Zurich, + * Stefan Leutenegger, Simon Lynen and Margarita Chli. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_ +#define PCL_FEATURES_IMPL_BRISK_2D_HPP_ + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::BRISK2DEstimation::BRISK2DEstimation () + : rotation_invariance_enabled_ (true) + , scale_invariance_enabled_ (true) + , pattern_scale_ (1.0f) + , input_cloud_ (), keypoints_ (), scale_range_ (), pattern_points_ (), points_ () + , n_rot_ (1024), scale_list_ (nullptr), size_list_ (nullptr) + , scales_ (64) + , scalerange_ (30) + , basic_size_ (12.0) + , strings_ (0), d_max_ (0.0f), d_min_ (0.0f), short_pairs_ (), long_pairs_ () + , no_short_pairs_ (0), no_long_pairs_ (0) + , intensity_ () + , name_ ("BRISK2Destimation") +{ + // Since we do not assume pattern_scale_ should be changed by the user, we + // can initialize the kernel in the constructor + std::vector r_list; + std::vector n_list; + + // this is the standard pattern found to be suitable also + r_list.resize (5); + n_list.resize (5); + const float f = 0.85f * pattern_scale_; + + r_list[0] = f * 0.0f; + r_list[1] = f * 2.9f; + r_list[2] = f * 4.9f; + r_list[3] = f * 7.4f; + r_list[4] = f * 10.8f; + + n_list[0] = 1; + n_list[1] = 10; + n_list[2] = 14; + n_list[3] = 15; + n_list[4] = 20; + + generateKernel (r_list, n_list, 5.85f * pattern_scale_, 8.2f * pattern_scale_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::BRISK2DEstimation::~BRISK2DEstimation () +{ + if (pattern_points_) delete [] pattern_points_; + if (short_pairs_) delete [] short_pairs_; + if (long_pairs_) delete [] long_pairs_; + if (scale_list_) delete [] scale_list_; + if (size_list_) delete [] size_list_; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BRISK2DEstimation::generateKernel ( + std::vector &radius_list, + std::vector &number_list, float d_max, float d_min, + std::vector index_change) +{ + d_max_ = d_max; + d_min_ = d_min; + + // get the total number of points + const auto rings = radius_list.size (); + assert (radius_list.size () != 0 && radius_list.size () == number_list.size ()); + points_ = 0; // remember the total number of points + for (const auto number: number_list) + points_ += number; + + // set up the patterns + pattern_points_ = new BriskPatternPoint[points_*scales_*n_rot_]; + BriskPatternPoint* pattern_iterator = pattern_points_; + + // define the scale discretization: + static const float lb_scale = std::log (scalerange_) / std::log (2.0); + static const float lb_scale_step = lb_scale / (float (scales_)); + + scale_list_ = new float[scales_]; + size_list_ = new unsigned int[scales_]; + + const float sigma_scale = 1.3f; + + for (unsigned int scale = 0; scale < scales_; ++scale) + { + scale_list_[scale] = static_cast (pow (double (2.0), static_cast (float (scale) * lb_scale_step))); + size_list_[scale] = 0; + + // generate the pattern points look-up + for (std::size_t rot = 0; rot < n_rot_; ++rot) + { + // this is the rotation of the feature + double theta = double (rot) * 2 * M_PI / double (n_rot_); + for (int ring = 0; ring < rings; ++ring) + { + for (int num = 0; num < number_list[ring]; ++num) + { + // the actual coordinates on the circle + double alpha = double (num) * 2 * M_PI / double (number_list[ring]); + + // feature rotation plus angle of the point + pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast (std::cos (alpha + theta)); + pattern_iterator->y = scale_list_[scale] * radius_list[ring] * static_cast (sin (alpha + theta)); + // and the gaussian kernel sigma + if (ring == 0) + pattern_iterator->sigma = sigma_scale * scale_list_[scale] * 0.5f; + else + pattern_iterator->sigma = static_cast (sigma_scale * scale_list_[scale] * (double (radius_list[ring])) * sin (M_PI / double (number_list[ring]))); + + // adapt the sizeList if necessary + const unsigned int size = static_cast (std::ceil (((scale_list_[scale] * radius_list[ring]) + pattern_iterator->sigma)) + 1); + + if (size_list_[scale] < size) + size_list_[scale] = size; + + // increment the iterator + ++pattern_iterator; + } + } + } + } + + // now also generate pairings + short_pairs_ = new BriskShortPair[points_ * (points_ - 1) / 2]; + long_pairs_ = new BriskLongPair[points_ * (points_ - 1) / 2]; + no_short_pairs_ = 0; + no_long_pairs_ = 0; + + // fill index_change with 0..n if empty + if (index_change.empty ()) + { + index_change.resize (points_ * (points_ - 1) / 2); + } + std::iota(index_change.begin (), index_change.end (), 0); + + const float d_min_sq = d_min_ * d_min_; + const float d_max_sq = d_max_ * d_max_; + for (unsigned int i = 1; i < points_; i++) + { + for (unsigned int j = 0; j < i; j++) + { //(find all the pairs) + // point pair distance: + const float dx = pattern_points_[j].x - pattern_points_[i].x; + const float dy = pattern_points_[j].y - pattern_points_[i].y; + const float norm_sq = (dx*dx+dy*dy); + if (norm_sq > d_min_sq) + { + // save to long pairs + BriskLongPair& longPair = long_pairs_[no_long_pairs_]; + longPair.weighted_dx = int ((dx / (norm_sq)) * 2048.0 + 0.5); + longPair.weighted_dy = int ((dy / (norm_sq)) * 2048.0 + 0.5); + longPair.i = i; + longPair.j = j; + ++no_long_pairs_; + } + else if (norm_sq < d_max_sq) + { + // save to short pairs + assert (no_short_pairs_ < index_change.size ()); // make sure the user passes something sensible + BriskShortPair& shortPair = short_pairs_[index_change[no_short_pairs_]]; + shortPair.j = j; + shortPair.i = i; + ++no_short_pairs_; + } + } + } + + // no bits: + strings_ = int (std::ceil ((float (no_short_pairs_)) / 128.0)) * 4 * 4; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline int +pcl::BRISK2DEstimation::smoothedIntensity ( + const std::vector &image, + int image_width, int, + //const Stefan& integral, + const std::vector &integral_image, + const float key_x, const float key_y, const unsigned int scale, + const unsigned int rot, const unsigned int point) const +{ + // get the float position + const BriskPatternPoint& brisk_point = pattern_points_[scale * n_rot_*points_ + rot * points_ + point]; + const float xf = brisk_point.x + key_x; + const float yf = brisk_point.y + key_y; + const int x = int (xf); + const int y = int (yf); + const int& imagecols = image_width; + + // get the sigma: + const float sigma_half = brisk_point.sigma; + const float area = 4.0f * sigma_half * sigma_half; + + // Get the point step + + // calculate output: + int ret_val; + if (sigma_half < 0.5) + { + // interpolation multipliers: + const int r_x = static_cast ((xf - float (x)) * 1024); + const int r_y = static_cast ((yf - float (y)) * 1024); + const int r_x_1 = (1024 - r_x); + const int r_y_1 = (1024 - r_y); + + //+const unsigned char* ptr = static_cast (&image.points[0].r) + x + y * imagecols; + const unsigned char* ptr = static_cast(&image[0]) + x + y * imagecols; + + // just interpolate: + ret_val = (r_x_1 * r_y_1 * int (*ptr)); + + //+ptr += sizeof (PointInT); + ptr++; + + ret_val += (r_x * r_y_1 * int (*ptr)); + + //+ptr += (imagecols * sizeof (PointInT)); + ptr += imagecols; + + ret_val += (r_x * r_y * int (*ptr)); + + //+ptr -= sizeof (PointInT); + ptr--; + + ret_val += (r_x_1 * r_y * int (*ptr)); + return (ret_val + 512) / 1024; + } + + // this is the standard case (simple, not speed optimized yet): + + // scaling: + const int scaling = static_cast (4194304.0f / area); + const int scaling2 = static_cast (float (scaling) * area / 1024.0f); + + // the integral image is larger: + const int integralcols = imagecols + 1; + + // calculate borders + const float x_1 = xf - sigma_half; + const float x1 = xf + sigma_half; + const float y_1 = yf - sigma_half; + const float y1 = yf + sigma_half; + + const int x_left = int (x_1 + 0.5); + const int y_top = int (y_1 + 0.5); + const int x_right = int (x1 + 0.5); + const int y_bottom = int (y1 + 0.5); + + // overlap area - multiplication factors: + const float r_x_1 = float (x_left) - x_1 + 0.5f; + const float r_y_1 = float (y_top) - y_1 + 0.5f; + const float r_x1 = x1 - float (x_right) + 0.5f; + const float r_y1 = y1 - float (y_bottom) + 0.5f; + const int dx = x_right - x_left - 1; + const int dy = y_bottom - y_top - 1; + const int A = static_cast ((r_x_1 * r_y_1) * float (scaling)); + const int B = static_cast ((r_x1 * r_y_1) * float (scaling)); + const int C = static_cast ((r_x1 * r_y1) * float (scaling)); + const int D = static_cast ((r_x_1 * r_y1) * float (scaling)); + const int r_x_1_i = static_cast (r_x_1 * float (scaling)); + const int r_y_1_i = static_cast (r_y_1 * float (scaling)); + const int r_x1_i = static_cast (r_x1 * float (scaling)); + const int r_y1_i = static_cast (r_y1 * float (scaling)); + + if (dx + dy > 2) + { + // now the calculation: + + //+const unsigned char* ptr = static_cast (&image.points[0].r) + x_left + imagecols * y_top; + const unsigned char* ptr = static_cast(&image[0]) + x_left + imagecols * y_top; + + // first the corners: + ret_val = A * int (*ptr); + + //+ptr += (dx + 1) * sizeof (PointInT); + ptr += dx + 1; + + ret_val += B * int (*ptr); + + //+ptr += (dy * imagecols + 1) * sizeof (PointInT); + ptr += dy * imagecols + 1; + + ret_val += C * int (*ptr); + + //+ptr -= (dx + 1) * sizeof (PointInT); + ptr -= dx + 1; + + ret_val += D * int (*ptr); + + // next the edges: + //+int* ptr_integral;// = static_cast (integral.data) + x_left + integralcols * y_top + 1; + const int* ptr_integral = static_cast (&integral_image[0]) + x_left + integralcols * y_top + 1; + + // find a simple path through the different surface corners + const int tmp1 = (*ptr_integral); + ptr_integral += dx; + const int tmp2 = (*ptr_integral); + ptr_integral += integralcols; + const int tmp3 = (*ptr_integral); + ptr_integral++; + const int tmp4 = (*ptr_integral); + ptr_integral += dy * integralcols; + const int tmp5 = (*ptr_integral); + ptr_integral--; + const int tmp6 = (*ptr_integral); + ptr_integral += integralcols; + const int tmp7 = (*ptr_integral); + ptr_integral -= dx; + const int tmp8 = (*ptr_integral); + ptr_integral -= integralcols; + const int tmp9 = (*ptr_integral); + ptr_integral--; + const int tmp10 = (*ptr_integral); + ptr_integral -= dy * integralcols; + const int tmp11 = (*ptr_integral); + ptr_integral++; + const int tmp12 = (*ptr_integral); + + // assign the weighted surface integrals: + const int upper = (tmp3 -tmp2 +tmp1 -tmp12) * r_y_1_i; + const int middle = (tmp6 -tmp3 +tmp12 -tmp9) * scaling; + const int left = (tmp9 -tmp12 +tmp11 -tmp10) * r_x_1_i; + const int right = (tmp5 -tmp4 +tmp3 -tmp6) * r_x1_i; + const int bottom = (tmp7 -tmp6 +tmp9 -tmp8) * r_y1_i; + + return (ret_val + upper + middle + left + right + bottom + scaling2 / 2) / scaling2; + } + + // now the calculation: + + //const unsigned char* ptr = static_cast (&image.points[0].r) + x_left + imagecols * y_top; + const unsigned char* ptr = static_cast(&image[0]) + x_left + imagecols * y_top; + + // first row: + ret_val = A * int (*ptr); + + //+ptr += sizeof (PointInT); + ptr++; + + //+const unsigned char* end1 = ptr + (dx * sizeof (PointInT)); + const unsigned char* end1 = ptr + dx; + + //+for (; ptr < end1; ptr += sizeof (PointInT)) + for (; ptr < end1; ptr++) + ret_val += r_y_1_i * int (*ptr); + ret_val += B * int (*ptr); + + // middle ones: + //+ptr += (imagecols - dx - 1) * sizeof (PointInT); + ptr += imagecols - dx - 1; + + //+const unsigned char* end_j = ptr + (dy * imagecols) * sizeof (PointInT); + const unsigned char* end_j = ptr + dy * imagecols; + + //+for (; ptr < end_j; ptr += (imagecols - dx - 1) * sizeof (PointInT)) + for (; ptr < end_j; ptr += imagecols - dx - 1) + { + ret_val += r_x_1_i * int (*ptr); + + //+ptr += sizeof (PointInT); + ptr++; + + //+const unsigned char* end2 = ptr + (dx * sizeof (PointInT)); + const unsigned char* end2 = ptr + dx; + + //+for (; ptr < end2; ptr += sizeof (PointInT)) + for (; ptr < end2; ptr++) + ret_val += int (*ptr) * scaling; + + ret_val += r_x1_i * int (*ptr); + } + // last row: + ret_val += D * int (*ptr); + + //+ptr += sizeof (PointInT); + ptr++; + + //+const unsigned char* end3 = ptr + (dx * sizeof (PointInT)); + const unsigned char* end3 = ptr + dx; + + //+for (; ptr bool +pcl::BRISK2DEstimation::RoiPredicate ( + const float min_x, const float min_y, + const float max_x, const float max_y, const KeypointT& pt) +{ + return ((pt.x < min_x) || (pt.x >= max_x) || (pt.y < min_y) || (pt.y >= max_y)); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BRISK2DEstimation::compute ( + PointCloudOutT &output) +{ + if (!input_cloud_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); + return; + } + + // image size + const int width = int (input_cloud_->width); + const int height = int (input_cloud_->height); + + // destination for intensity data; will be forwarded to BRISK + std::vector image_data (width*height); + + for (std::size_t i = 0; i < image_data.size (); ++i) + image_data[i] = static_cast (intensity_ ((*input_cloud_)[i])); + + // Remove keypoints very close to the border + std::size_t ksize = keypoints_->points.size (); + std::vector kscales; // remember the scale per keypoint + kscales.resize (ksize); + + // initialize constants + static const float lb_scalerange = std::log2 (scalerange_); + + typename std::vector >::iterator beginning = keypoints_->points.begin (); + std::vector::iterator beginningkscales = kscales.begin (); + + static const float basic_size_06 = basic_size_ * 0.6f; + unsigned int basicscale = 0; + + if (!scale_invariance_enabled_) + basicscale = std::max (static_cast (float (scales_) / lb_scalerange * (std::log2 (1.45f * basic_size_ / (basic_size_06))) + 0.5f), 0); + + for (std::size_t k = 0; k < ksize; k++) + { + unsigned int scale; + if (scale_invariance_enabled_) + { + scale = std::max (static_cast (float (scales_) / lb_scalerange * (std::log2 (keypoints_->points[k].size / (basic_size_06))) + 0.5f), 0); + // saturate + if (scale >= scales_) scale = scales_ - 1; + kscales[k] = scale; + } + else + { + scale = basicscale; + kscales[k] = scale; + } + + const int border = size_list_[scale]; + const int border_x = width - border; + const int border_y = height - border; + + if (RoiPredicate (float (border), float (border), float (border_x), float (border_y), keypoints_->points[k])) + { + //std::cerr << "remove keypoint" << std::endl; + keypoints_->points.erase (beginning + k); + kscales.erase (beginningkscales + k); + if (k == 0) + { + beginning = keypoints_->points.begin (); + beginningkscales = kscales.begin (); + } + ksize--; + k--; + } + } + + keypoints_->width = std::uint32_t (keypoints_->size ()); + keypoints_->height = 1; + + // first, calculate the integral image over the whole image: + // current integral image + std::vector integral ((width+1)*(height+1), 0); // the integral image + + for (std::size_t row_index = 1; row_index < height; ++row_index) + { + for (std::size_t col_index = 1; col_index < width; ++col_index) + { + const std::size_t index = row_index*width+col_index; + const std::size_t index2 = (row_index)*(width+1)+(col_index); + + integral[index2] = static_cast (image_data[index]) + - integral[index2-1-(width+1)] + + integral[index2-(width+1)] + + integral[index2-1]; + } + } + + int* values = new int[points_]; // for temporary use + + // resize the descriptors: + //output = zeros (ksize, strings_); + + // now do the extraction for all keypoints: + + // temporary variables containing gray values at sample points: + int t1; + int t2; + + // the feature orientation + int direction0; + int direction1; + + output.resize (ksize); + //output.width = ksize; + //output.height = 1; + for (std::size_t k = 0; k < ksize; k++) + { + unsigned char* ptr = &output.points[k].descriptor[0]; + + int theta; + KeypointT &kp = keypoints_->points[k]; + const int& scale = kscales[k]; + int shifter = 0; + int* pvalues = values; + const float& x = float (kp.x); + const float& y = float (kp.y); + if (true) // kp.angle==-1 + { + if (!rotation_invariance_enabled_) + // don't compute the gradient direction, just assign a rotation of 0 degree + theta = 0; + else + { + // get the gray values in the unrotated pattern + for (unsigned int i = 0; i < points_; i++) + *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, 0, i); + + direction0 = 0; + direction1 = 0; + // now iterate through the long pairings + const BriskLongPair* max = long_pairs_ + no_long_pairs_; + + for (BriskLongPair* iter = long_pairs_; iter < max; ++iter) + { + t1 = *(values + iter->i); + t2 = *(values + iter->j); + const int delta_t = (t1 - t2); + + // update the direction: + const int tmp0 = delta_t * (iter->weighted_dx) / 1024; + const int tmp1 = delta_t * (iter->weighted_dy) / 1024; + direction0 += tmp0; + direction1 += tmp1; + } + kp.angle = std::atan2 (float (direction1), float (direction0)) / float (M_PI) * 180.0f; + theta = static_cast ((float (n_rot_) * kp.angle) / (360.0f) + 0.5f); + if (theta < 0) + theta += n_rot_; + if (theta >= int (n_rot_)) + theta -= n_rot_; + } + } + else + { + // figure out the direction: + //int theta=rotationInvariance*round((_n_rot*std::atan2(direction.at(0,0),direction.at(1,0)))/(2*M_PI)); + if (!rotation_invariance_enabled_) + theta = 0; + else + { + theta = static_cast (n_rot_ * (kp.angle / (360.0)) + 0.5); + if (theta < 0) + theta += n_rot_; + if (theta >= int (n_rot_)) + theta -= n_rot_; + } + } + + // now also extract the stuff for the actual direction: + // let us compute the smoothed values + shifter = 0; + + //unsigned int mean=0; + pvalues = values; + // get the gray values in the rotated pattern + for (unsigned int i = 0; i < points_; i++) + *(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, theta, i); + +#ifdef __GNUC__ + using UINT32_ALIAS = std::uint32_t; +#endif +#ifdef _MSC_VER + // Todo: find the equivalent to may_alias + #define UCHAR_ALIAS std::uint32_t //__declspec(noalias) + #define UINT32_ALIAS std::uint32_t //__declspec(noalias) +#endif + + // now iterate through all the pairings + UINT32_ALIAS* ptr2 = reinterpret_cast (ptr); + const BriskShortPair* max = short_pairs_ + no_short_pairs_; + + for (BriskShortPair* iter = short_pairs_; iter < max; ++iter) + { + t1 = *(values + iter->i); + t2 = *(values + iter->j); + + if (t1 > t2) + *ptr2 |= ((1) << shifter); + + // else already initialized with zero + // take care of the iterators: + ++shifter; + + if (shifter == 32) + { + shifter = 0; + ++ptr2; + } + } + + //ptr += strings_; + + //// Account for the scale + orientation; + //ptr += sizeof (output.points[0].scale); + //ptr += sizeof (output.points[0].orientation); + } + + // we do not change the denseness + output.width = int (output.points.size ()); + output.height = 1; + output.is_dense = true; + + // clean-up + delete [] values; +} + + +#endif //#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/cppf.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/cppf.hpp new file mode 100755 index 0000000..74782d4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/cppf.hpp @@ -0,0 +1,123 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2013, Martin Szarski + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_CPPF_H_ +#define PCL_FEATURES_IMPL_CPPF_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::CPPFEstimation::CPPFEstimation () + : FeatureFromNormals () +{ + feature_name_ = "CPPFEstimation"; + // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () + Feature::tree_.reset (new pcl::search::KdTree ()); + Feature::search_radius_ = 1.0f; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CPPFEstimation::computeFeature (PointCloudOut &output) +{ + // Initialize output container + output.points.clear (); + output.points.reserve (indices_->size () * input_->points.size ()); + output.is_dense = true; + // Compute point pair features for every pair of points in the cloud + for (const auto& i: *indices_) + { + for (std::size_t j = 0 ; j < input_->points.size (); ++j) + { + PointOutT p; + // No need to calculate feature for identity pair (i, j) as they aren't used in future calculations + if (i != j) + { + if ( + pcl::computeCPPFPairFeature (input_->points[i].getVector4fMap (), + normals_->points[i].getNormalVector4fMap (), + input_->points[i].getRGBVector4i (), + input_->points[j].getVector4fMap (), + normals_->points[j].getNormalVector4fMap (), + input_->points[j].getRGBVector4i (), + p.f1, p.f2, p.f3, p.f4, p.f5, p.f6, p.f7, p.f8, p.f9, p.f10)) + { + // Calculate alpha_m angle + Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), + model_reference_normal = normals_->points[i].getNormalVector3fMap (), + model_point = input_->points[j].getVector3fMap (); + Eigen::AngleAxisf rotation_mg (std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), + model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); + Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; + + Eigen::Vector3f model_point_transformed = transform_mg * model_point; + float angle = std::atan2 ( -model_point_transformed(2), model_point_transformed(1)); + if (std::sin (angle) * model_point_transformed(2) < 0.0f) + angle *= (-1); + p.alpha_m = -angle; + } + else + { + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j); + p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + } + } + else + { + p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + } + + output.points.push_back (p); + } + } + // overwrite the sizes done by Feature::initCompute () + output.height = 1; + output.width = static_cast (output.points.size ()); +} + +#define PCL_INSTANTIATE_CPPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CPPFEstimation; + + +#endif // PCL_FEATURES_IMPL_CPPF_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/crh.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/crh.hpp new file mode 100755 index 0000000..4146c44 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/crh.hpp @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: cvfh.hpp 5311 2012-03-26 22:02:04Z aaldoma $ + * + */ + +#ifndef PCL_FEATURES_IMPL_CRH_H_ +#define PCL_FEATURES_IMPL_CRH_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::CRHEstimation::computeFeature (PointCloudOut &output) +{ + // Check if input was set + if (!normals_) + { + PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + if (normals_->points.size () != surface_->points.size ()) + { + PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + Eigen::Vector3f plane_normal; + plane_normal[0] = -centroid_[0]; + plane_normal[1] = -centroid_[1]; + plane_normal[2] = -centroid_[2]; + Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ (); + plane_normal.normalize (); + Eigen::Vector3f axis = plane_normal.cross (z_vector); + double rotation = -asin (axis.norm ()); + axis.normalize (); + + int nbins = nbins_; + int bin_angle = 360 / nbins; + + Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast (rotation), axis)); + + pcl::PointCloud grid; + grid.points.resize (indices_->size ()); + + for (std::size_t i = 0; i < indices_->size (); i++) + { + grid.points[i].getVector4fMap () = surface_->points[(*indices_)[i]].getVector4fMap (); + grid.points[i].getNormalVector4fMap () = normals_->points[(*indices_)[i]].getNormalVector4fMap (); + } + + pcl::transformPointCloudWithNormals (grid, grid, transformPC); + + //fill spatial data vector and the zero-initialize or "value-initialize" an array on c++, + // the initialization is made with () after the [nbins] + std::vector spatial_data(nbins); + + float sum_w = 0; + for (const auto &point : grid.points) + { + int bin = static_cast ((((std::atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins; + float w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x); + sum_w += w; + spatial_data[bin] += w; + } + + for (auto& data: spatial_data) + data /= sum_w; + + std::vector freq_data(nbins / 2 + 1); + kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0, nullptr, nullptr); + kiss_fftr (mycfg, spatial_data.data (), freq_data.data ()); + + for (auto& data: freq_data) + { + data.r /= freq_data[0].r; + data.i /= freq_data[0].r; + } + + output.points.resize (1); + output.width = output.height = 1; + + output.points[0].histogram[0] = freq_data[0].r; //dc + int k = 1; + for (int i = 1; i < (nbins / 2); i++, k += 2) + { + output.points[0].histogram[k] = freq_data[i].r; + output.points[0].histogram[k + 1] = freq_data[i].i; + } + + output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].r; //nyquist +} + +#define PCL_INSTANTIATE_CRHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CRHEstimation; + +#endif // PCL_FEATURES_IMPL_CRH_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/cvfh.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/cvfh.hpp new file mode 100755 index 0000000..499d78e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/cvfh.hpp @@ -0,0 +1,332 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_CVFH_H_ +#define PCL_FEATURES_IMPL_CVFH_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CVFHEstimation::compute (PointCloudOut &output) +{ + if (!Feature::initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + // Resize the output dataset + // Important! We should only allocate precisely how many elements we will need, otherwise + // we risk at pre-allocating too much memory which could lead to bad_alloc + // (see http://dev.pointclouds.org/issues/657) + output.width = output.height = 1; + output.points.resize (1); + + // Perform the actual feature computation + computeFeature (output); + + Feature::deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CVFHEstimation::extractEuclideanClustersSmooth ( + const pcl::PointCloud &cloud, + const pcl::PointCloud &normals, + float tolerance, + const pcl::search::Search::Ptr &tree, + std::vector &clusters, + double eps_angle, + unsigned int min_pts_per_cluster, + unsigned int max_pts_per_cluster) +{ + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + if (cloud.points.size () != normals.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); + return; + } + + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + // Process all points in the indices vector + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + if (processed[i]) + continue; + processed[i] = true; + + pcl::PointIndices r; + r.header = cloud.header; + auto& seed_queue = r.indices; + + seed_queue.push_back (i); + + // loop has an emplace_back, making it difficult to use modern loops + for (std::size_t idx = 0; idx != seed_queue.size (); ++idx) + { + // Search for seed_queue[index] + if (!tree->radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances)) + { + continue; + } + + // skip index 0, since nn_indices[0] == idx, worth it? + for (std::size_t j = 1; j < nn_indices.size (); ++j) + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + //processed[nn_indices[j]] = true; + // [-1;1] + const double dot_p = normals.points[seed_queue[idx]].getNormalVector3fMap().dot( + normals.points[nn_indices[j]].getNormalVector3fMap()); + + if (std::acos (dot_p) < eps_angle) + { + processed[nn_indices[j]] = true; + seed_queue.emplace_back (nn_indices[j]); + } + } + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + // Might be better to work directly in the cluster somehow + clusters.emplace_back (std::move(r)); // Trying to avoid a copy by moving + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CVFHEstimation::filterNormalsWithHighCurvature ( + const pcl::PointCloud & cloud, + std::vector &indices_to_use, + std::vector &indices_out, + std::vector &indices_in, + float threshold) +{ + indices_out.resize (cloud.points.size ()); + indices_in.resize (cloud.points.size ()); + + std::size_t in, out; + in = out = 0; + + for (const int &index : indices_to_use) + { + if (cloud.points[index].curvature > threshold) + { + indices_out[out] = index; + out++; + } + else + { + indices_in[in] = index; + in++; + } + } + + indices_out.resize (out); + indices_in.resize (in); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CVFHEstimation::computeFeature (PointCloudOut &output) +{ + // Check if input was set + if (!normals_) + { + PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + if (normals_->points.size () != surface_->points.size ()) + { + PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + centroids_dominant_orientations_.clear (); + + // ---[ Step 0: remove normals with high curvature + std::vector indices_out; + std::vector indices_in; + filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_); + + pcl::PointCloud::Ptr normals_filtered_cloud (new pcl::PointCloud ()); + normals_filtered_cloud->width = static_cast (indices_in.size ()); + normals_filtered_cloud->height = 1; + normals_filtered_cloud->points.resize (normals_filtered_cloud->width); + + for (std::size_t i = 0; i < indices_in.size (); ++i) + { + normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x; + normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y; + normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z; + } + + std::vector clusters; + + if(normals_filtered_cloud->points.size() >= min_points_) + { + //recompute normals and use them for clustering + KdTreePtr normals_tree_filtered (new pcl::search::KdTree (false)); + normals_tree_filtered->setInputCloud (normals_filtered_cloud); + + + pcl::NormalEstimation n3d; + n3d.setRadiusSearch (radius_normals_); + n3d.setSearchMethod (normals_tree_filtered); + n3d.setInputCloud (normals_filtered_cloud); + n3d.compute (*normals_filtered_cloud); + + KdTreePtr normals_tree (new pcl::search::KdTree (false)); + normals_tree->setInputCloud (normals_filtered_cloud); + + extractEuclideanClustersSmooth (*normals_filtered_cloud, + *normals_filtered_cloud, + cluster_tolerance_, + normals_tree, + clusters, + eps_angle_threshold_, + static_cast (min_points_)); + + } + + VFHEstimator vfh; + vfh.setInputCloud (surface_); + vfh.setInputNormals (normals_); + vfh.setIndices(indices_); + vfh.setSearchMethod (this->tree_); + vfh.setUseGivenNormal (true); + vfh.setUseGivenCentroid (true); + vfh.setNormalizeBins (normalize_bins_); + vfh.setNormalizeDistance (true); + vfh.setFillSizeComponent (true); + output.height = 1; + + // ---[ Step 1b : check if any dominant cluster was found + if (!clusters.empty ()) + { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information + + for (const auto &cluster : clusters) //for each cluster + { + Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero (); + Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero (); + + for (const auto &index : cluster.indices) + { + avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap (); + avg_centroid += normals_filtered_cloud->points[index].getVector4fMap (); + } + + avg_normal /= static_cast (cluster.indices.size ()); + avg_centroid /= static_cast (cluster.indices.size ()); + + Eigen::Vector4f centroid_test; + pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test); + avg_normal.normalize (); + + Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]); + Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); + + //append normal and centroid for the clusters + dominant_normals_.push_back (avg_norm); + centroids_dominant_orientations_.push_back (avg_dominant_centroid); + } + + //compute modified VFH for all dominant clusters and add them to the list! + output.points.resize (dominant_normals_.size ()); + output.width = static_cast (dominant_normals_.size ()); + + for (std::size_t i = 0; i < dominant_normals_.size (); ++i) + { + //configure VFH computation for CVFH + vfh.setNormalToUse (dominant_normals_[i]); + vfh.setCentroidToUse (centroids_dominant_orientations_[i]); + pcl::PointCloud vfh_signature; + vfh.compute (vfh_signature); + output.points[i] = vfh_signature.points[0]; + } + } + else + { // ---[ Step 1b.1 : If no, compute CVFH using all the object points + Eigen::Vector4f avg_centroid; + pcl::compute3DCentroid (*surface_, avg_centroid); + Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); + centroids_dominant_orientations_.push_back (cloud_centroid); + + //configure VFH computation for CVFH using all object points + vfh.setCentroidToUse (cloud_centroid); + vfh.setUseGivenNormal (false); + + pcl::PointCloud vfh_signature; + vfh.compute (vfh_signature); + + output.points.resize (1); + output.width = 1; + + output.points[0] = vfh_signature.points[0]; + } +} + +#define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation; + +#endif // PCL_FEATURES_IMPL_VFH_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/don.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/don.hpp new file mode 100755 index 0000000..e333d66 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/don.hpp @@ -0,0 +1,104 @@ + /* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Yani Ioannou + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef PCL_FILTERS_DON_IMPL_H_ +#define PCL_FILTERS_DON_IMPL_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::DifferenceOfNormalsEstimation::initCompute () +{ + // Check if input normals are set + if (!input_normals_small_) + { + PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing small support radius normals was given!\n", getClassName().c_str ()); + Feature::deinitCompute(); + return (false); + } + + if (!input_normals_large_) + { + PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing large support radius normals was given!\n", getClassName().c_str ()); + Feature::deinitCompute(); + return (false); + } + + // Check if the size of normals is the same as the size of the surface + if (input_normals_small_->points.size () != input_->points.size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] ", getClassName().c_str ()); + PCL_ERROR ("The number of points in the input dataset differs from "); + PCL_ERROR ("the number of points in the dataset containing the small support radius normals!\n"); + Feature::deinitCompute (); + return (false); + } + + if (input_normals_large_->points.size () != input_->points.size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] ", getClassName().c_str ()); + PCL_ERROR ("The number of points in the input dataset differs from "); + PCL_ERROR ("the number of points in the dataset containing the large support radius normals!\n"); + Feature::deinitCompute (); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::DifferenceOfNormalsEstimation::computeFeature (PointCloudOut &output) +{ + //perform DoN subtraction and return results + for (std::size_t point_id = 0; point_id < input_->points.size (); ++point_id) + { + output.points[point_id].getNormalVector3fMap () = (input_normals_small_->points[point_id].getNormalVector3fMap () + - input_normals_large_->points[point_id].getNormalVector3fMap ()) / 2.0; + if(!std::isfinite (output.points[point_id].normal_x) || + !std::isfinite (output.points[point_id].normal_y) || + !std::isfinite (output.points[point_id].normal_z)){ + output.points[point_id].getNormalVector3fMap () = Eigen::Vector3f(0,0,0); + } + output.points[point_id].curvature = output.points[point_id].getNormalVector3fMap ().norm(); + } +} + + +#define PCL_INSTANTIATE_DifferenceOfNormalsEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::DifferenceOfNormalsEstimation; + +#endif // PCL_FILTERS_DON_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/esf.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/esf.hpp new file mode 100755 index 0000000..58ef720 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/esf.hpp @@ -0,0 +1,555 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: pfh.hpp 5027 2012-03-12 03:10:45Z rusu $ + * + */ + +#ifndef PCL_FEATURES_IMPL_ESF_H_ +#define PCL_FEATURES_IMPL_ESF_H_ + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ESFEstimation::computeESF ( + PointCloudIn &pc, std::vector &hist) +{ + const int binsize = 64; + unsigned int sample_size = 20000; + // @TODO: Replace with c++ stdlib uniform_random_generator + srand (static_cast (time (nullptr))); + int maxindex = static_cast (pc.points.size ()); + + std::vector d2v, d1v, d3v, wt_d3; + std::vector wt_d2; + d1v.reserve (sample_size); + d2v.reserve (sample_size * 3); + d3v.reserve (sample_size); + wt_d2.reserve (sample_size * 3); + wt_d3.reserve (sample_size); + + float h_in[binsize] = {0}; + float h_out[binsize] = {0}; + float h_mix[binsize] = {0}; + float h_mix_ratio[binsize] = {0}; + + float h_a3_in[binsize] = {0}; + float h_a3_out[binsize] = {0}; + float h_a3_mix[binsize] = {0}; + + float h_d3_in[binsize] = {0}; + float h_d3_out[binsize] = {0}; + float h_d3_mix[binsize] = {0}; + + float ratio=0.0; + float pih = static_cast(M_PI) / 2.0f; + float a,b,c,s; + int th1,th2,th3; + int vxlcnt = 0; + int pcnt1,pcnt2,pcnt3; + for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx) + { + // get a new random point + int index1 = rand()%maxindex; + int index2 = rand()%maxindex; + int index3 = rand()%maxindex; + + if (index1==index2 || index1 == index3 || index2 == index3) + { + nn_idx--; + continue; + } + + Eigen::Vector4f p1 = pc.points[index1].getVector4fMap (); + Eigen::Vector4f p2 = pc.points[index2].getVector4fMap (); + Eigen::Vector4f p3 = pc.points[index3].getVector4fMap (); + + // A3 + Eigen::Vector4f v21 (p2 - p1); + Eigen::Vector4f v31 (p3 - p1); + Eigen::Vector4f v23 (p2 - p3); + a = v21.norm (); b = v31.norm (); c = v23.norm (); s = (a+b+c) * 0.5f; + if (s * (s-a) * (s-b) * (s-c) <= 0.001f) + { + nn_idx--; + continue; + } + + v21.normalize (); + v31.normalize (); + v23.normalize (); + + //TODO: .dot gives nan's + th1 = static_cast (pcl_round (std::acos (std::abs (v21.dot (v31))) / pih * (binsize-1))); + th2 = static_cast (pcl_round (std::acos (std::abs (v23.dot (v31))) / pih * (binsize-1))); + th3 = static_cast (pcl_round (std::acos (std::abs (v23.dot (v21))) / pih * (binsize-1))); + if (th1 < 0 || th1 >= binsize) + { + nn_idx--; + continue; + } + if (th2 < 0 || th2 >= binsize) + { + nn_idx--; + continue; + } + if (th3 < 0 || th3 >= binsize) + { + nn_idx--; + continue; + } + + // D2 + d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index2])); + d2v.push_back (pcl::euclideanDistance (pc.points[index1], pc.points[index3])); + d2v.push_back (pcl::euclideanDistance (pc.points[index2], pc.points[index3])); + + int vxlcnt_sum = 0; + int p_cnt = 0; + // IN, OUT, MIXED, Ratio line tracing, index1->index2 + { + const int xs = p1[0] < 0.0? static_cast(std::floor(p1[0])+GRIDSIZE_H): static_cast(std::ceil(p1[0])+GRIDSIZE_H-1); + const int ys = p1[1] < 0.0? static_cast(std::floor(p1[1])+GRIDSIZE_H): static_cast(std::ceil(p1[1])+GRIDSIZE_H-1); + const int zs = p1[2] < 0.0? static_cast(std::floor(p1[2])+GRIDSIZE_H): static_cast(std::ceil(p1[2])+GRIDSIZE_H-1); + const int xt = p2[0] < 0.0? static_cast(std::floor(p2[0])+GRIDSIZE_H): static_cast(std::ceil(p2[0])+GRIDSIZE_H-1); + const int yt = p2[1] < 0.0? static_cast(std::floor(p2[1])+GRIDSIZE_H): static_cast(std::ceil(p2[1])+GRIDSIZE_H-1); + const int zt = p2[2] < 0.0? static_cast(std::floor(p2[2])+GRIDSIZE_H): static_cast(std::ceil(p2[2])+GRIDSIZE_H-1); + wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt1)); + if (wt_d2.back () == 2) + h_mix_ratio[static_cast (pcl_round (ratio * (binsize-1)))]++; + vxlcnt_sum += vxlcnt; + p_cnt += pcnt1; + } + // IN, OUT, MIXED, Ratio line tracing, index1->index3 + { + const int xs = p1[0] < 0.0? static_cast(std::floor(p1[0])+GRIDSIZE_H): static_cast(std::ceil(p1[0])+GRIDSIZE_H-1); + const int ys = p1[1] < 0.0? static_cast(std::floor(p1[1])+GRIDSIZE_H): static_cast(std::ceil(p1[1])+GRIDSIZE_H-1); + const int zs = p1[2] < 0.0? static_cast(std::floor(p1[2])+GRIDSIZE_H): static_cast(std::ceil(p1[2])+GRIDSIZE_H-1); + const int xt = p3[0] < 0.0? static_cast(std::floor(p3[0])+GRIDSIZE_H): static_cast(std::ceil(p3[0])+GRIDSIZE_H-1); + const int yt = p3[1] < 0.0? static_cast(std::floor(p3[1])+GRIDSIZE_H): static_cast(std::ceil(p3[1])+GRIDSIZE_H-1); + const int zt = p3[2] < 0.0? static_cast(std::floor(p3[2])+GRIDSIZE_H): static_cast(std::ceil(p3[2])+GRIDSIZE_H-1); + wt_d2.push_back (this->lci (xs, ys, zs, xt, yt, zt, ratio, vxlcnt, pcnt2)); + if (wt_d2.back () == 2) + h_mix_ratio[static_cast(pcl_round (ratio * (binsize-1)))]++; + vxlcnt_sum += vxlcnt; + p_cnt += pcnt2; + } + // IN, OUT, MIXED, Ratio line tracing, index2->index3 + { + const int xs = p2[0] < 0.0? static_cast(std::floor(p2[0])+GRIDSIZE_H): static_cast(std::ceil(p2[0])+GRIDSIZE_H-1); + const int ys = p2[1] < 0.0? static_cast(std::floor(p2[1])+GRIDSIZE_H): static_cast(std::ceil(p2[1])+GRIDSIZE_H-1); + const int zs = p2[2] < 0.0? static_cast(std::floor(p2[2])+GRIDSIZE_H): static_cast(std::ceil(p2[2])+GRIDSIZE_H-1); + const int xt = p3[0] < 0.0? static_cast(std::floor(p3[0])+GRIDSIZE_H): static_cast(std::ceil(p3[0])+GRIDSIZE_H-1); + const int yt = p3[1] < 0.0? static_cast(std::floor(p3[1])+GRIDSIZE_H): static_cast(std::ceil(p3[1])+GRIDSIZE_H-1); + const int zt = p3[2] < 0.0? static_cast(std::floor(p3[2])+GRIDSIZE_H): static_cast(std::ceil(p3[2])+GRIDSIZE_H-1); + wt_d2.push_back (this->lci (xs,ys,zs,xt,yt,zt,ratio,vxlcnt,pcnt3)); + if (wt_d2.back () == 2) + h_mix_ratio[static_cast(pcl_round(ratio * (binsize-1)))]++; + vxlcnt_sum += vxlcnt; + p_cnt += pcnt3; + } + + // D3 ( herons formula ) + d3v.push_back (std::sqrt (std::sqrt (s * (s-a) * (s-b) * (s-c)))); + if (vxlcnt_sum <= 21) + { + wt_d3.push_back (0); + h_a3_out[th1] += static_cast (pcnt3) / 32.0f; + h_a3_out[th2] += static_cast (pcnt1) / 32.0f; + h_a3_out[th3] += static_cast (pcnt2) / 32.0f; + } + else + if (p_cnt - vxlcnt_sum < 4) + { + h_a3_in[th1] += static_cast (pcnt3) / 32.0f; + h_a3_in[th2] += static_cast (pcnt1) / 32.0f; + h_a3_in[th3] += static_cast (pcnt2) / 32.0f; + wt_d3.push_back (1); + } + else + { + h_a3_mix[th1] += static_cast (pcnt3) / 32.0f; + h_a3_mix[th2] += static_cast (pcnt1) / 32.0f; + h_a3_mix[th3] += static_cast (pcnt2) / 32.0f; + wt_d3.push_back (static_cast (vxlcnt_sum) / static_cast (p_cnt)); + } + } + // Normalizing, get max + float maxd2 = 0; + float maxd3 = 0; + + for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx) + { + // get max of Dx + if (d2v[nn_idx] > maxd2) + maxd2 = d2v[nn_idx]; + if (d2v[sample_size + nn_idx] > maxd2) + maxd2 = d2v[sample_size + nn_idx]; + if (d2v[sample_size*2 +nn_idx] > maxd2) + maxd2 = d2v[sample_size*2 +nn_idx]; + if (d3v[nn_idx] > maxd3) + maxd3 = d3v[nn_idx]; + } + + // Normalize and create histogram + int index; + for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx) + { + if (wt_d3[nn_idx] >= 0.999) // IN + { + index = static_cast(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1))); + if (index >= 0 && index < binsize) + h_d3_in[index]++; + } + else + { + if (wt_d3[nn_idx] <= 0.001) // OUT + { + index = static_cast(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1))); + if (index >= 0 && index < binsize) + h_d3_out[index]++ ; + } + else + { + index = static_cast(pcl_round (d3v[nn_idx] / maxd3 * (binsize-1))); + if (index >= 0 && index < binsize) + h_d3_mix[index]++; + } + } + } + //normalize and create histogram + for (std::size_t nn_idx = 0; nn_idx < d2v.size(); ++nn_idx ) + { + if (wt_d2[nn_idx] == 0) + h_in[static_cast(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ; + if (wt_d2[nn_idx] == 1) + h_out[static_cast(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++; + if (wt_d2[nn_idx] == 2) + h_mix[static_cast(pcl_round (d2v[nn_idx] / maxd2 * (binsize-1)))]++ ; + } + + //float weights[10] = {1, 1, 1, 1, 1, 1, 1, 1 , 1 , 1}; + float weights[10] = {0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 1.0f, 1.0f, 2.0f, 2.0f, 2.0f}; + + hist.reserve (binsize * 10); + for (const float &i : h_a3_in) + hist.push_back (i * weights[0]); + for (const float &i : h_a3_out) + hist.push_back (i * weights[1]); + for (const float &i : h_a3_mix) + hist.push_back (i * weights[2]); + + for (const float &i : h_d3_in) + hist.push_back (i * weights[3]); + for (const float &i : h_d3_out) + hist.push_back (i * weights[4]); + for (const float &i : h_d3_mix) + hist.push_back (i * weights[5]); + + for (const float &i : h_in) + hist.push_back (i*0.5f * weights[6]); + for (const float &i : h_out) + hist.push_back (i * weights[7]); + for (const float &i : h_mix) + hist.push_back (i * weights[8]); + for (const float &i : h_mix_ratio) + hist.push_back (i*0.5f * weights[9]); + + float sm = 0; + for (const float &i : hist) + sm += i; + + for (float &i : hist) + i /= sm; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::ESFEstimation::lci ( + const int x1, const int y1, const int z1, + const int x2, const int y2, const int z2, + float &ratio, int &incnt, int &pointcount) +{ + int voxelcount = 0; + int voxel_in = 0; + int act_voxel[3]; + act_voxel[0] = x1; + act_voxel[1] = y1; + act_voxel[2] = z1; + int x_inc, y_inc, z_inc; + int dx = x2 - x1; + int dy = y2 - y1; + int dz = z2 - z1; + if (dx < 0) + x_inc = -1; + else + x_inc = 1; + int l = std::abs (dx); + if (dy < 0) + y_inc = -1 ; + else + y_inc = 1; + int m = std::abs (dy); + if (dz < 0) + z_inc = -1 ; + else + z_inc = 1; + int n = std::abs (dz); + int dx2 = 2 * l; + int dy2 = 2 * m; + int dz2 = 2 * n; + if ((l >= m) & (l >= n)) + { + int err_1 = dy2 - l; + int err_2 = dz2 - l; + for (int i = 1; i(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); + if (err_1 > 0) + { + act_voxel[1] += y_inc; + err_1 -= dx2; + } + if (err_2 > 0) + { + act_voxel[2] += z_inc; + err_2 -= dx2; + } + err_1 += dy2; + err_2 += dz2; + act_voxel[0] += x_inc; + } + } + else if ((m >= l) & (m >= n)) + { + int err_1 = dx2 - m; + int err_2 = dz2 - m; + for (int i=1; i(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); + if (err_1 > 0) + { + act_voxel[0] += x_inc; + err_1 -= dy2; + } + if (err_2 > 0) + { + act_voxel[2] += z_inc; + err_2 -= dy2; + } + err_1 += dx2; + err_2 += dz2; + act_voxel[1] += y_inc; + } + } + else + { + int err_1 = dy2 - n; + int err_2 = dx2 - n; + for (int i=1; i(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); + if (err_1 > 0) + { + act_voxel[1] += y_inc; + err_1 -= dz2; + } + if (err_2 > 0) + { + act_voxel[0] += x_inc; + err_2 -= dz2; + } + err_1 += dy2; + err_2 += dx2; + act_voxel[2] += z_inc; + } + } + voxelcount++; + voxel_in += static_cast(lut_[act_voxel[0]][act_voxel[1]][act_voxel[2]] == 1); + incnt = voxel_in; + pointcount = voxelcount; + + if (voxel_in >= voxelcount-1) + return (0); + + if (voxel_in <= 7) + return (1); + + ratio = static_cast(voxel_in) / static_cast(voxelcount); + return (2); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ESFEstimation::voxelize9 (PointCloudIn &cluster) +{ + for (std::size_t i = 0; i < cluster.points.size (); ++i) + { + int xx = cluster.points[i].x<0.0? static_cast(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1); + int yy = cluster.points[i].y<0.0? static_cast(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1); + int zz = cluster.points[i].z<0.0? static_cast(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1); + + for (int x = -1; x < 2; x++) + for (int y = -1; y < 2; y++) + for (int z = -1; z < 2; z++) + { + int xi = xx + x; + int yi = yy + y; + int zi = zz + z; + + if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0) + { + ; + } + else + this->lut_[xi][yi][zi] = 1; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ESFEstimation::cleanup9 (PointCloudIn &cluster) +{ + for (std::size_t i = 0; i < cluster.points.size (); ++i) + { + int xx = cluster.points[i].x<0.0? static_cast(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1); + int yy = cluster.points[i].y<0.0? static_cast(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1); + int zz = cluster.points[i].z<0.0? static_cast(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1); + + for (int x = -1; x < 2; x++) + for (int y = -1; y < 2; y++) + for (int z = -1; z < 2; z++) + { + int xi = xx + x; + int yi = yy + y; + int zi = zz + z; + + if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0) + { + ; + } + else + this->lut_[xi][yi][zi] = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ESFEstimation::scale_points_unit_sphere ( + const pcl::PointCloud &pc, float scalefactor, Eigen::Vector4f& centroid) +{ + pcl::compute3DCentroid (pc, centroid); + pcl::demeanPointCloud (pc, centroid, local_cloud_); + + float max_distance = 0; + pcl::PointXYZ cog (0, 0, 0); + + for (std::size_t i = 0; i < local_cloud_.points.size (); ++i) + { + float d = pcl::euclideanDistance(cog,local_cloud_.points[i]); + if (d > max_distance) + max_distance = d; + } + + float scale_factor = 1.0f / max_distance * scalefactor; + + Eigen::Affine3f matrix = Eigen::Affine3f::Identity(); + matrix.scale (scale_factor); + pcl::transformPointCloud (local_cloud_, local_cloud_, matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ESFEstimation::compute (PointCloudOut &output) +{ + if (!Feature::initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + // Copy the header + output.header = input_->header; + + // Resize the output dataset + // Important! We should only allocate precisely how many elements we will need, otherwise + // we risk at pre-allocating too much memory which could lead to bad_alloc + // (see http://dev.pointclouds.org/issues/657) + output.width = output.height = 1; + output.is_dense = input_->is_dense; + output.points.resize (1); + + // Perform the actual feature computation + computeFeature (output); + + Feature::deinitCompute (); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ESFEstimation::computeFeature (PointCloudOut &output) +{ + Eigen::Vector4f xyz_centroid; + std::vector hist; + scale_points_unit_sphere (*surface_, static_cast(GRIDSIZE_H), xyz_centroid); + this->voxelize9 (local_cloud_); + this->computeESF (local_cloud_, hist); + this->cleanup9 (local_cloud_); + + // We only output _1_ signature + output.points.resize (1); + output.width = 1; + output.height = 1; + + for (std::size_t d = 0; d < hist.size (); ++d) + output.points[0].histogram[d] = hist[d]; +} + +#define PCL_INSTANTIATE_ESFEstimation(T,OutT) template class PCL_EXPORTS pcl::ESFEstimation; + +#endif // PCL_FEATURES_IMPL_ESF_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/feature.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/feature.hpp new file mode 100755 index 0000000..5e3a467 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/feature.hpp @@ -0,0 +1,338 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_FEATURE_H_ +#define PCL_FEATURES_IMPL_FEATURE_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +inline void +pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + const Eigen::Vector4f &point, + Eigen::Vector4f &plane_parameters, float &curvature) +{ + solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature); + + plane_parameters[3] = 0; + // Hessian form (D = nc . p_plane (centroid here) + p) + plane_parameters[3] = -1 * plane_parameters.dot (point); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +inline void +pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + float &nx, float &ny, float &nz, float &curvature) +{ + // Avoid getting hung on Eigen's optimizers +// for (int i = 0; i < 9; ++i) +// if (!std::isfinite (covariance_matrix.coeff (i))) +// { +// //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n"); +// nx = ny = nz = curvature = std::numeric_limits::quiet_NaN (); +// return; +// } + // Extract the smallest eigenvalue and its eigenvector + EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + + nx = eigen_vector [0]; + ny = eigen_vector [1]; + nz = eigen_vector [2]; + + // Compute the curvature surface change + float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8); + if (eig_sum != 0) + curvature = std::abs (eigen_value / eig_sum); + else + curvature = 0; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Feature::initCompute () +{ + if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // If the dataset is empty, just return + if (input_->points.empty ()) + { + PCL_ERROR ("[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ()); + // Cleanup + deinitCompute (); + return (false); + } + + // If no search surface has been defined, use the input dataset as the search surface itself + if (!surface_) + { + fake_surface_ = true; + surface_ = input_; + } + + // Check if a space search locator was given + if (!tree_) + { + if (surface_->isOrganized () && input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + if (tree_->getInputCloud () != surface_) // Make sure the tree searches the surface + tree_->setInputCloud (surface_); + + + // Do a fast check to see if the search parameters are well defined + if (search_radius_ != 0.0) + { + if (k_ != 0) + { + PCL_ERROR ("[pcl::%s::compute] ", getClassName ().c_str ()); + PCL_ERROR ("Both radius (%f) and K (%d) defined! ", search_radius_, k_); + PCL_ERROR ("Set one of them to zero first and then re-run compute ().\n"); + // Cleanup + deinitCompute (); + return (false); + } + else // Use the radiusSearch () function + { + search_parameter_ = search_radius_; + // Declare the search locator definition + search_method_surface_ = [this] (const PointCloudIn &cloud, int index, double radius, + std::vector &k_indices, std::vector &k_distances) + { + return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0); + }; + } + } + else + { + if (k_ != 0) // Use the nearestKSearch () function + { + search_parameter_ = k_; + // Declare the search locator definition + search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, std::vector &k_indices, + std::vector &k_distances) + { + return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances); + }; + } + else + { + PCL_ERROR ("[pcl::%s::compute] Neither radius nor K defined! ", getClassName ().c_str ()); + PCL_ERROR ("Set one of them to a positive number first and then re-run compute ().\n"); + // Cleanup + deinitCompute (); + return (false); + } + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Feature::deinitCompute () +{ + // Reset the surface + if (fake_surface_) + { + surface_.reset (); + fake_surface_ = false; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::Feature::compute (PointCloudOut &output) +{ + if (!initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Copy the header + output.header = input_->header; + + // Resize the output dataset + if (output.points.size () != indices_->size ()) + output.points.resize (indices_->size ()); + + // Check if the output will be computed for all points or only a subset + // If the input width or height are not set, set output width as size + if (indices_->size () != input_->points.size () || input_->width * input_->height == 0) + { + output.width = static_cast (indices_->size ()); + output.height = 1; + } + else + { + output.width = input_->width; + output.height = input_->height; + } + output.is_dense = input_->is_dense; + + // Perform the actual feature computation + computeFeature (output); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::FeatureFromNormals::initCompute () +{ + if (!Feature::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // Check if input normals are set + if (!normals_) + { + PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ()); + Feature::deinitCompute (); + return (false); + } + + // Check if the size of normals is the same as the size of the surface + if (normals_->points.size () != surface_->points.size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ()); + PCL_ERROR ("The number of points in the input dataset (%u) differs from ", surface_->points.size ()); + PCL_ERROR ("the number of points in the dataset containing the normals (%u)!\n", normals_->points.size ()); + Feature::deinitCompute (); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::FeatureFromLabels::initCompute () +{ + if (!Feature::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // Check if input normals are set + if (!labels_) + { + PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing labels was given!\n", getClassName ().c_str ()); + Feature::deinitCompute (); + return (false); + } + + // Check if the size of normals is the same as the size of the surface + if (labels_->points.size () != surface_->points.size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] The number of points in the input dataset differs from the number of points in the dataset containing the labels!\n", getClassName ().c_str ()); + Feature::deinitCompute (); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::FeatureWithLocalReferenceFrames::initLocalReferenceFrames (const std::size_t& indices_size, + const LRFEstimationPtr& lrf_estimation) +{ + if (frames_never_defined_) + frames_.reset (); + + // Check if input frames are set + if (!frames_) + { + if (!lrf_estimation) + { + PCL_ERROR ("[initLocalReferenceFrames] No input dataset containing reference frames was given!\n"); + return (false); + } else + { + //PCL_WARN ("[initLocalReferenceFrames] No input dataset containing reference frames was given! Proceed using default\n"); + PointCloudLRFPtr default_frames (new PointCloudLRF()); + lrf_estimation->compute (*default_frames); + frames_ = default_frames; + } + } + + // Check if the size of frames is the same as the size of the input cloud + if (frames_->points.size () != indices_size) + { + if (!lrf_estimation) + { + PCL_ERROR ("[initLocalReferenceFrames] The number of points in the input dataset differs from the number of points in the dataset containing the reference frames!\n"); + return (false); + } else + { + //PCL_WARN ("[initLocalReferenceFrames] The number of points in the input dataset differs from the number of points in the dataset containing the reference frames! Proceed using default\n"); + PointCloudLRFPtr default_frames (new PointCloudLRF()); + lrf_estimation->compute (*default_frames); + frames_ = default_frames; + } + } + + return (true); +} + +#endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/flare.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/flare.hpp new file mode 100755 index 0000000..fa53eff --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/flare.hpp @@ -0,0 +1,265 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2016-, Open Perception, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* +*/ + +#ifndef PCL_FEATURES_IMPL_FLARE_H_ +#define PCL_FEATURES_IMPL_FLARE_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool + pcl::FLARELocalReferenceFrameEstimation::initCompute () +{ + if (!FeatureFromNormals::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + if (tangent_radius_ == 0.0f) + { + PCL_ERROR ("[pcl::%s::initCompute] tangent_radius_ not set.\n", getClassName ().c_str ()); + return (false); + } + + // If no search sampled_surface_ has been defined, use the surface_ dataset as the search sampled_surface_ itself + if (!sampled_surface_) + { + fake_sampled_surface_ = true; + sampled_surface_ = surface_; + + if (sampled_tree_) + { + PCL_WARN ("[pcl::%s::initCompute] sampled_surface_ is not set even if sampled_tree_ is already set.", getClassName ().c_str ()); + PCL_WARN ("sampled_tree_ will be rebuilt from surface_. Use sampled_surface_.\n"); + } + } + + // Check if a space search locator was given for sampled_surface_ + if (!sampled_tree_) + { + if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ()) + sampled_tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + sampled_tree_.reset (new pcl::search::KdTree (false)); + } + + if (sampled_tree_->getInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface + sampled_tree_->setInputCloud (sampled_surface_); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool + pcl::FLARELocalReferenceFrameEstimation::deinitCompute () +{ + // Reset the surface + if (fake_surface_) + { + surface_.reset (); + fake_surface_ = false; + } + // Reset the sampled surface + if (fake_sampled_surface_) + { + sampled_surface_.reset (); + fake_sampled_surface_ = false; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template SignedDistanceT + pcl::FLARELocalReferenceFrameEstimation::computePointLRF (const int index, + Eigen::Matrix3f &lrf) +{ + Eigen::Vector3f x_axis, y_axis; + Eigen::Vector3f fitted_normal; //z_axis + + //find Z axis + + //extract support points for the computation of Z axis + std::vector neighbours_indices; + std::vector neighbours_distances; + + const std::size_t n_normal_neighbours = + this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); + if (n_normal_neighbours < min_neighbors_for_normal_axis_) + { + if (!pcl::isFinite ((*normals_)[index])) + { + //normal is invalid + //setting lrf to NaN + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + + //set z_axis as the normal of index point + fitted_normal = (*normals_)[index].getNormalVector3fMap (); + } + else + { + float plane_curvature; + normal_estimation_.computePointNormal (*surface_, neighbours_indices, fitted_normal (0), fitted_normal (1), fitted_normal (2), plane_curvature); + + //disambiguate Z axis with normal mean + if (!pcl::flipNormalTowardsNormalsMean (*normals_, neighbours_indices, fitted_normal)) + { + //all normals in the neighbourood are invalid + //setting lrf to NaN + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + } + + //setting LRF Z axis + lrf.row (2).matrix () = fitted_normal; + + //find X axis + + //extract support points for Rx radius + const std::size_t n_tangent_neighbours = + sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances); + + if (n_tangent_neighbours < min_neighbors_for_tangent_axis_) + { + //set X axis as a random axis + x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + + return (std::numeric_limits::max ()); + } + + //find point with the largest signed distance from tangent plane + + SignedDistanceT shape_score; + SignedDistanceT best_shape_score = -std::numeric_limits::max (); + int best_shape_index = -1; + + Eigen::Vector3f best_margin_point; + + const float radius2 = tangent_radius_ * tangent_radius_; + const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2; + + Vector3fMapConst feature_point = (*input_)[index].getVector3fMap (); + + for (std::size_t curr_neigh = 0; curr_neigh < n_tangent_neighbours; ++curr_neigh) + { + const int& curr_neigh_idx = neighbours_indices[curr_neigh]; + const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; + + if (neigh_distance_sqr <= margin_distance2) + { + continue; + } + + //point curr_neigh_idx is inside the ring between marginThresh and Radius + + shape_score = fitted_normal.dot ((*sampled_surface_)[curr_neigh_idx].getVector3fMap ()); + + if (shape_score > best_shape_score) + { + best_shape_index = curr_neigh_idx; + best_shape_score = shape_score; + } + } //for each neighbor + + if (best_shape_index == -1) + { + x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + + return (std::numeric_limits::max ()); + } + + //find orthogonal axis directed to best_shape_index point projection on plane with fittedNormal as axis + x_axis = pcl::geometry::projectedAsUnitVector (sampled_surface_->at (best_shape_index).getVector3fMap (), feature_point, fitted_normal); + + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + best_shape_score -= fitted_normal.dot (feature_point); + return (best_shape_score); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void + pcl::FLARELocalReferenceFrameEstimation::computeFeature (PointCloudOut &output) +{ + //check whether used with search radius or search k-neighbors + if (this->getKSearch () != 0) + { + PCL_ERROR ( + "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch (0) and setRadiusSearch (radius) to use this class.\n", + getClassName ().c_str ()); + return; + } + + signed_distances_from_highest_points_.resize (indices_->size ()); + + for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx) + { + Eigen::Matrix3f currentLrf; + PointOutT &rf = output[point_idx]; + + signed_distances_from_highest_points_[point_idx] = computePointLRF ((*indices_)[point_idx], currentLrf); + if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits::max ()) + { + output.is_dense = false; + } + + rf.getXAxisVector3fMap () = currentLrf.row (0); + rf.getYAxisVector3fMap () = currentLrf.row (1); + rf.getZAxisVector3fMap () = currentLrf.row (2); + } +} + +#define PCL_INSTANTIATE_FLARELocalReferenceFrameEstimation(T,NT,OutT,SdT) template class PCL_EXPORTS pcl::FLARELocalReferenceFrameEstimation; + +#endif // PCL_FEATURES_IMPL_FLARE_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/fpfh.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/fpfh.hpp new file mode 100755 index 0000000..e2bf64d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/fpfh.hpp @@ -0,0 +1,305 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_FPFH_H_ +#define PCL_FEATURES_IMPL_FPFH_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::FPFHEstimation::computePairFeatures ( + const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4) +{ + pcl::computePairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (), + cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (), + f1, f2, f3, f4); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FPFHEstimation::computePointSPFHSignature ( + const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + int p_idx, int row, const std::vector &indices, + Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3) +{ + Eigen::Vector4f pfh_tuple; + // Get the number of bins from the histograms size + // @TODO: use arrays + int nr_bins_f1 = static_cast (hist_f1.cols ()); + int nr_bins_f2 = static_cast (hist_f2.cols ()); + int nr_bins_f3 = static_cast (hist_f3.cols ()); + + // Factorization constant + float hist_incr = 100.0f / static_cast(indices.size () - 1); + + // Iterate over all the points in the neighborhood + for (const auto &index : indices) + { + // Avoid unnecessary returns + if (p_idx == index) + continue; + + // Compute the pair P to NNi + if (!computePairFeatures (cloud, normals, p_idx, index, pfh_tuple[0], pfh_tuple[1], pfh_tuple[2], pfh_tuple[3])) + continue; + + // Normalize the f1, f2, f3 features and push them in the histogram + int h_index = static_cast (std::floor (nr_bins_f1 * ((pfh_tuple[0] + M_PI) * d_pi_))); + if (h_index < 0) h_index = 0; + if (h_index >= nr_bins_f1) h_index = nr_bins_f1 - 1; + hist_f1 (row, h_index) += hist_incr; + + h_index = static_cast (std::floor (nr_bins_f2 * ((pfh_tuple[1] + 1.0) * 0.5))); + if (h_index < 0) h_index = 0; + if (h_index >= nr_bins_f2) h_index = nr_bins_f2 - 1; + hist_f2 (row, h_index) += hist_incr; + + h_index = static_cast (std::floor (nr_bins_f3 * ((pfh_tuple[2] + 1.0) * 0.5))); + if (h_index < 0) h_index = 0; + if (h_index >= nr_bins_f3) h_index = nr_bins_f3 - 1; + hist_f3 (row, h_index) += hist_incr; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FPFHEstimation::weightPointSPFHSignature ( + const Eigen::MatrixXf &hist_f1, const Eigen::MatrixXf &hist_f2, const Eigen::MatrixXf &hist_f3, + const std::vector &indices, const std::vector &dists, Eigen::VectorXf &fpfh_histogram) +{ + assert (indices.size () == dists.size ()); + // @TODO: use arrays + double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0; + float weight = 0.0, val_f1, val_f2, val_f3; + + // Get the number of bins from the histograms size + const auto nr_bins_f1 = hist_f1.cols (); + const auto nr_bins_f2 = hist_f2.cols (); + const auto nr_bins_f3 = hist_f3.cols (); + const auto nr_bins_f12 = nr_bins_f1 + nr_bins_f2; + + // Clear the histogram + fpfh_histogram.setZero (nr_bins_f1 + nr_bins_f2 + nr_bins_f3); + + // Use the entire patch + for (std::size_t idx = 0; idx < indices.size (); ++idx) + { + // Minus the query point itself + if (dists[idx] == 0) + continue; + + // Standard weighting function used + weight = 1.0f / dists[idx]; + + // Weight the SPFH of the query point with the SPFH of its neighbors + for (std::size_t f1_i = 0; f1_i < nr_bins_f1; ++f1_i) + { + val_f1 = hist_f1 (indices[idx], f1_i) * weight; + sum_f1 += val_f1; + fpfh_histogram[f1_i] += val_f1; + } + + for (std::size_t f2_i = 0; f2_i < nr_bins_f2; ++f2_i) + { + val_f2 = hist_f2 (indices[idx], f2_i) * weight; + sum_f2 += val_f2; + fpfh_histogram[f2_i + nr_bins_f1] += val_f2; + } + + for (std::size_t f3_i = 0; f3_i < nr_bins_f3; ++f3_i) + { + val_f3 = hist_f3 (indices[idx], f3_i) * weight; + sum_f3 += val_f3; + fpfh_histogram[f3_i + nr_bins_f12] += val_f3; + } + } + + if (sum_f1 != 0) + sum_f1 = 100.0 / sum_f1; // histogram values sum up to 100 + if (sum_f2 != 0) + sum_f2 = 100.0 / sum_f2; // histogram values sum up to 100 + if (sum_f3 != 0) + sum_f3 = 100.0 / sum_f3; // histogram values sum up to 100 + + // Adjust final FPFH values + const auto denormalize_with = [](auto factor) + { + return [=](const auto& data) { return data * factor; }; + }; + + auto last = fpfh_histogram.data (); + last = std::transform(last, last + nr_bins_f1, last, denormalize_with (sum_f1)); + last = std::transform(last, last + nr_bins_f2, last, denormalize_with (sum_f2)); + std::transform(last, last + nr_bins_f3, last, denormalize_with (sum_f3)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FPFHEstimation::computeSPFHSignatures (std::vector &spfh_hist_lookup, + Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3) +{ + // Allocate enough space to hold the NN search results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + std::set spfh_indices; + spfh_hist_lookup.resize (surface_->points.size ()); + + // Build a list of (unique) indices for which we will need to compute SPFH signatures + // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_]) + if (surface_ != input_ || + indices_->size () != surface_->points.size ()) + { + for (const auto& p_idx: *indices_) + { + if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0) + continue; + + spfh_indices.insert (nn_indices.begin (), nn_indices.end ()); + } + } + else + { + // Special case: When a feature must be computed at every point, there is no need for a neighborhood search + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + spfh_indices.insert (static_cast (idx)); + } + + // Initialize the arrays that will store the SPFH signatures + std::size_t data_size = spfh_indices.size (); + hist_f1.setZero (data_size, nr_bins_f1_); + hist_f2.setZero (data_size, nr_bins_f2_); + hist_f3.setZero (data_size, nr_bins_f3_); + + // Compute SPFH signatures for every point that needs them + std::size_t i = 0; + for (const auto& p_idx: spfh_indices) + { + // Find the neighborhood around p_idx + if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0) + continue; + + // Estimate the SPFH signature around p_idx + computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1, hist_f2, hist_f3); + + // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices + spfh_hist_lookup[p_idx] = i; + i++; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FPFHEstimation::computeFeature (PointCloudOut &output) +{ + // Allocate enough space to hold the NN search results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + std::vector spfh_hist_lookup; + computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_); + + output.is_dense = true; + // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense + if (input_->is_dense) + { + // Iterate over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d) + output.points[idx].histogram[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices + // instead of indices into surface_->points + for (auto &nn_index : nn_indices) + nn_index = spfh_hist_lookup[nn_index]; + + // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ... + weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_); + + // ...and copy it into the output cloud + std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output.points[idx].histogram); + } + } + else + { + // Iterate over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d) + output.points[idx].histogram[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices + // instead of indices into surface_->points + for (auto &nn_index : nn_indices) + nn_index = spfh_hist_lookup[nn_index]; + + // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ... + weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_); + + // ...and copy it into the output cloud + std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output.points[idx].histogram); + } + } +} + +#define PCL_INSTANTIATE_FPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimation; + +#endif // PCL_FEATURES_IMPL_FPFH_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/fpfh_omp.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/fpfh_omp.hpp new file mode 100755 index 0000000..c0015a7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/fpfh_omp.hpp @@ -0,0 +1,172 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_ +#define PCL_FEATURES_IMPL_FPFH_OMP_H_ + +#include + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FPFHEstimationOMP::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FPFHEstimationOMP::computeFeature (PointCloudOut &output) +{ + std::vector spfh_indices_vec; + std::vector spfh_hist_lookup (surface_->points.size ()); + + // Build a list of (unique) indices for which we will need to compute SPFH signatures + // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_]) + if (surface_ != input_ || + indices_->size () != surface_->points.size ()) + { + std::vector nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch (). + std::vector nn_dists (k_); + + std::set spfh_indices_set; + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + int p_idx = (*indices_)[idx]; + if (!isFinite ((*input_)[p_idx]) || + this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0) + continue; + + spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ()); + } + spfh_indices_vec.resize (spfh_indices_set.size ()); + std::copy (spfh_indices_set.cbegin (), spfh_indices_set.cend (), spfh_indices_vec.begin ()); + } + else + { + // Special case: When a feature must be computed at every point, there is no need for a neighborhood search + spfh_indices_vec.resize (indices_->size ()); + std::iota(spfh_indices_vec.begin (), spfh_indices_vec.end (), + static_cast(0)); + } + + // Initialize the arrays that will store the SPFH signatures + const auto data_size = spfh_indices_vec.size (); + hist_f1_.setZero (data_size, nr_bins_f1_); + hist_f2_.setZero (data_size, nr_bins_f2_); + hist_f3_.setZero (data_size, nr_bins_f3_); + + std::vector nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch (). + std::vector nn_dists (k_); + + // Compute SPFH signatures for every point that needs them + +#ifdef _OPENMP +#pragma omp parallel for shared (spfh_hist_lookup) private (nn_indices, nn_dists) num_threads(threads_) +#endif + for (std::ptrdiff_t i = 0; i < static_cast (spfh_indices_vec.size ()); ++i) + { + // Get the next point index + int p_idx = spfh_indices_vec[i]; + + // Find the neighborhood around p_idx + if (!isFinite ((*input_)[p_idx]) || + this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0) + continue; + + // Estimate the SPFH signature around p_idx + this->computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_); + + // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices + spfh_hist_lookup[p_idx] = i; + } + + // Initialize the array that will store the FPFH signature + int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_; + + nn_indices.clear(); + nn_dists.clear(); + + // Iterate over the entire index vector +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) +#endif + for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) + { + // Find the indices of point idx's neighbors... + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + for (int d = 0; d < nr_bins; ++d) + output.points[idx].histogram[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + + // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices + // instead of indices into surface_->points + for (int &nn_index : nn_indices) + nn_index = spfh_hist_lookup[nn_index]; + + // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ... + Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins); + weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram); + + // ...and copy it into the output cloud + for (int d = 0; d < nr_bins; ++d) + output.points[idx].histogram[d] = fpfh_histogram[d]; + } + +} + +#define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP; + +#endif // PCL_FEATURES_IMPL_FPFH_OMP_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/gasd.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/gasd.hpp new file mode 100755 index 0000000..02223d6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/gasd.hpp @@ -0,0 +1,401 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2016-, Open Perception, Inc. + * Copyright (c) 2016, Voxar Labs, CIn-UFPE / DEINFO-UFRPE + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_GASD_H_ +#define PCL_FEATURES_IMPL_GASD_H_ + +#include +#include +#include + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GASDEstimation::compute (PointCloudOut &output) +{ + if (!Feature::initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Resize the output dataset + output.resize (1); + + // Copy header and is_dense flag from input + output.header = surface_->header; + output.is_dense = surface_->is_dense; + + // Perform the actual feature computation + computeFeature (output); + + Feature::deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GASDEstimation::computeAlignmentTransform () +{ + Eigen::Vector4f centroid; + Eigen::Matrix3f covariance_matrix; + + // compute centroid of the object's partial view + pcl::compute3DCentroid (*surface_, *indices_, centroid); + + // compute covariance matrix from points and centroid of the object's partial view + pcl::computeCovarianceMatrix (*surface_, *indices_, centroid, covariance_matrix); + + Eigen::Matrix3f eigenvectors; + Eigen::Vector3f eigenvalues; + + // compute eigenvalues and eigenvectors of the covariance matrix + pcl::eigen33 (covariance_matrix, eigenvectors, eigenvalues); + + // z axis of the reference frame is the eigenvector associated with the minimal eigenvalue + Eigen::Vector3f z_axis = eigenvectors.col (0); + + // if angle between z axis and viewing direction is in the [-90 deg, 90 deg] range, then z axis is negated + if (z_axis.dot (view_direction_) > 0) + { + z_axis = -z_axis; + } + + // x axis of the reference frame is the eigenvector associated with the maximal eigenvalue + const Eigen::Vector3f x_axis = eigenvectors.col (2); + + // y axis is the cross product of z axis and x axis + const Eigen::Vector3f y_axis = z_axis.cross (x_axis); + + const Eigen::Vector3f centroid_xyz = centroid.head<3> (); + + // compute alignment transform from axes and centroid + transform_ << x_axis.transpose (), -x_axis.dot (centroid_xyz), + y_axis.transpose (), -y_axis.dot (centroid_xyz), + z_axis.transpose (), -z_axis.dot (centroid_xyz), + 0.0f, 0.0f, 0.0f, 1.0f; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GASDEstimation::addSampleToHistograms (const Eigen::Vector4f &p, + const float max_coord, + const std::size_t half_grid_size, + const HistogramInterpolationMethod interp, + const float hbin, + const float hist_incr, + std::vector &hists) +{ + const std::size_t grid_size = half_grid_size * 2; + + // compute normalized coordinates with respect to axis-aligned bounding cube centered on the origin + const Eigen::Vector3f scaled ( (p[0] / max_coord) * half_grid_size, (p[1] / max_coord) * half_grid_size, (p[2] / max_coord) * half_grid_size); + + // compute histograms array coords + Eigen::Vector4f coords (scaled[0] + half_grid_size, scaled[1] + half_grid_size, scaled[2] + half_grid_size, hbin); + + // if using histogram interpolation, subtract 0.5 so samples with the central value of the bin have full weight in it + if (interp != INTERP_NONE) + { + coords -= Eigen::Vector4f (0.5f, 0.5f, 0.5f, 0.5f); + } + + // compute histograms bins indices + const Eigen::Vector4f bins (std::floor (coords[0]), std::floor (coords[1]), std::floor (coords[2]), std::floor (coords[3])); + + // compute indices of the bin where the sample falls into + const std::size_t grid_idx = ( (bins[0] + 1) * (grid_size + 2) + bins[1] + 1) * (grid_size + 2) + bins[2] + 1; + const std::size_t h_idx = bins[3] + 1; + + if (interp == INTERP_NONE) + { + // no interpolation + hists[grid_idx][h_idx] += hist_incr; + } + else + { + // if using histogram interpolation, compute trilinear interpolation + coords -= Eigen::Vector4f (bins[0], bins[1], bins[2], 0.0f); + + const float v_x1 = hist_incr * coords[0]; + const float v_x0 = hist_incr - v_x1; + + const float v_xy11 = v_x1 * coords[1]; + const float v_xy10 = v_x1 - v_xy11; + const float v_xy01 = v_x0 * coords[1]; + const float v_xy00 = v_x0 - v_xy01; + + const float v_xyz111 = v_xy11 * coords[2]; + const float v_xyz110 = v_xy11 - v_xyz111; + const float v_xyz101 = v_xy10 * coords[2]; + const float v_xyz100 = v_xy10 - v_xyz101; + const float v_xyz011 = v_xy01 * coords[2]; + const float v_xyz010 = v_xy01 - v_xyz011; + const float v_xyz001 = v_xy00 * coords[2]; + const float v_xyz000 = v_xy00 - v_xyz001; + + if (interp == INTERP_TRILINEAR) + { + // trilinear interpolation + hists[grid_idx][h_idx] += v_xyz000; + hists[grid_idx + 1][h_idx] += v_xyz001; + hists[grid_idx + (grid_size + 2)][h_idx] += v_xyz010; + hists[grid_idx + (grid_size + 3)][h_idx] += v_xyz011; + hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx] += v_xyz100; + hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx] += v_xyz101; + hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx] += v_xyz110; + hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx] += v_xyz111; + } + else + { + // quadrilinear interpolation + coords[3] -= bins[3]; + + const float v_xyzh1111 = v_xyz111 * coords[3]; + const float v_xyzh1110 = v_xyz111 - v_xyzh1111; + const float v_xyzh1101 = v_xyz110 * coords[3]; + const float v_xyzh1100 = v_xyz110 - v_xyzh1101; + const float v_xyzh1011 = v_xyz101 * coords[3]; + const float v_xyzh1010 = v_xyz101 - v_xyzh1011; + const float v_xyzh1001 = v_xyz100 * coords[3]; + const float v_xyzh1000 = v_xyz100 - v_xyzh1001; + const float v_xyzh0111 = v_xyz011 * coords[3]; + const float v_xyzh0110 = v_xyz011 - v_xyzh0111; + const float v_xyzh0101 = v_xyz010 * coords[3]; + const float v_xyzh0100 = v_xyz010 - v_xyzh0101; + const float v_xyzh0011 = v_xyz001 * coords[3]; + const float v_xyzh0010 = v_xyz001 - v_xyzh0011; + const float v_xyzh0001 = v_xyz000 * coords[3]; + const float v_xyzh0000 = v_xyz000 - v_xyzh0001; + + hists[grid_idx][h_idx] += v_xyzh0000; + hists[grid_idx][h_idx + 1] += v_xyzh0001; + hists[grid_idx + 1][h_idx] += v_xyzh0010; + hists[grid_idx + 1][h_idx + 1] += v_xyzh0011; + hists[grid_idx + (grid_size + 2)][h_idx] += v_xyzh0100; + hists[grid_idx + (grid_size + 2)][h_idx + 1] += v_xyzh0101; + hists[grid_idx + (grid_size + 3)][h_idx] += v_xyzh0110; + hists[grid_idx + (grid_size + 3)][h_idx + 1] += v_xyzh0111; + hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx] += v_xyzh1000; + hists[grid_idx + (grid_size + 2) * (grid_size + 2)][h_idx + 1] += v_xyzh1001; + hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx] += v_xyzh1010; + hists[grid_idx + (grid_size + 2) * (grid_size + 2) + 1][h_idx + 1] += v_xyzh1011; + hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx] += v_xyzh1100; + hists[grid_idx + (grid_size + 3) * (grid_size + 2)][h_idx + 1] += v_xyzh1101; + hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx] += v_xyzh1110; + hists[grid_idx + (grid_size + 3) * (grid_size + 2) + 1][h_idx + 1] += v_xyzh1111; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GASDEstimation::copyShapeHistogramsToOutput (const std::size_t grid_size, + const std::size_t hists_size, + const std::vector &hists, + PointCloudOut &output, + std::size_t &pos) +{ + for (std::size_t i = 0; i < grid_size; ++i) + { + for (std::size_t j = 0; j < grid_size; ++j) + { + for (std::size_t k = 0; k < grid_size; ++k) + { + const std::size_t idx = ( (i + 1) * (grid_size + 2) + (j + 1)) * (grid_size + 2) + (k + 1); + + std::copy (hists[idx].data () + 1, hists[idx].data () + hists_size + 1, output.points[0].histogram + pos); + pos += hists_size; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GASDEstimation::computeFeature (PointCloudOut &output) +{ + // compute alignment transform using reference frame + computeAlignmentTransform (); + + // align point cloud + pcl::transformPointCloud (*surface_, *indices_, shape_samples_, transform_); + + const std::size_t shape_grid_size = shape_half_grid_size_ * 2; + + // each histogram dimension has 2 additional bins, 1 in each boundary, for performing interpolation + std::vector shape_hists ((shape_grid_size + 2) * (shape_grid_size + 2) * (shape_grid_size + 2), + Eigen::VectorXf::Zero (shape_hists_size_ + 2)); + + Eigen::Vector4f centroid_p = Eigen::Vector4f::Zero (); + + // compute normalization factor for distances between samples and centroid + Eigen::Vector4f far_pt; + pcl::getMaxDistance (shape_samples_, centroid_p, far_pt); + far_pt[3] = 0; + const float distance_normalization_factor = (centroid_p - far_pt).norm (); + + // compute normalization factor with respect to axis-aligned bounding cube centered on the origin + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (shape_samples_, min_pt, max_pt); + + max_coord_ = std::max (min_pt.head<3> ().cwiseAbs ().maxCoeff (), max_pt.head<3> ().cwiseAbs ().maxCoeff ()); + + // normalize sample contribution with respect to the total number of points in the cloud + hist_incr_ = 100.0f / static_cast (shape_samples_.size () - 1); + + // for each sample + for (const auto& sample: shape_samples_) + { + // compute shape histogram array coord based on distance between sample and centroid + const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f); + const float d = p.norm (); + + const float shape_grid_step = distance_normalization_factor / shape_half_grid_size_; + + float integral; + const float dist_hist_val = std::modf(d / shape_grid_step, &integral); + + const float dbin = dist_hist_val * shape_hists_size_; + + // add sample to shape histograms, optionally performing interpolation + addSampleToHistograms (p, max_coord_, shape_half_grid_size_, shape_interp_, dbin, hist_incr_, shape_hists); + } + + pos_ = 0; + + // copy shape histograms to output + copyShapeHistogramsToOutput (shape_grid_size, shape_hists_size_, shape_hists, output, pos_); + + // set remaining values of the descriptor to zero (if any) + std::fill (output.points[0].histogram + pos_, output.points[0].histogram + output.points[0].descriptorSize (), 0.0f); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GASDColorEstimation::copyColorHistogramsToOutput (const std::size_t grid_size, + const std::size_t hists_size, + std::vector &hists, + PointCloudOut &output, + std::size_t &pos) +{ + for (std::size_t i = 0; i < grid_size; ++i) + { + for (std::size_t j = 0; j < grid_size; ++j) + { + for (std::size_t k = 0; k < grid_size; ++k) + { + const std::size_t idx = ( (i + 1) * (grid_size + 2) + (j + 1)) * (grid_size + 2) + (k + 1); + + hists[idx][1] += hists[idx][hists_size + 1]; + hists[idx][hists_size] += hists[idx][0]; + + std::copy (hists[idx].data () + 1, hists[idx].data () + hists_size + 1, output.points[0].histogram + pos); + pos += hists_size; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GASDColorEstimation::computeFeature (PointCloudOut &output) +{ + // call shape feature computation + GASDEstimation::computeFeature (output); + + const std::size_t color_grid_size = color_half_grid_size_ * 2; + + // each histogram dimension has 2 additional bins, 1 in each boundary, for performing interpolation + std::vector color_hists ((color_grid_size + 2) * (color_grid_size + 2) * (color_grid_size + 2), + Eigen::VectorXf::Zero (color_hists_size_ + 2)); + + // for each sample + for (const auto& sample: shape_samples_) + { + // compute shape histogram array coord based on distance between sample and centroid + const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f); + + // compute hue value + float hue = 0.f; + + const unsigned char max = std::max (sample.r, std::max (sample.g, sample.b)); + const unsigned char min = std::min (sample.r, std::min (sample.g, sample.b)); + + const float diff_inv = 1.f / static_cast (max - min); + + if (std::isfinite (diff_inv)) + { + if (max == sample.r) + { + hue = 60.f * (static_cast (sample.g - sample.b) * diff_inv); + } + else if (max == sample.g) + { + hue = 60.f * (2.f + static_cast (sample.b - sample.r) * diff_inv); + } + else + { + hue = 60.f * (4.f + static_cast (sample.r - sample.g) * diff_inv); // max == b + } + + if (hue < 0.f) + { + hue += 360.f; + } + } + + // compute color histogram array coord based on hue value + const float hbin = (hue / 360) * color_hists_size_; + + // add sample to color histograms, optionally performing interpolation + GASDEstimation::addSampleToHistograms (p, max_coord_, color_half_grid_size_, color_interp_, hbin, hist_incr_, color_hists); + } + + // copy color histograms to output + copyColorHistogramsToOutput (color_grid_size, color_hists_size_, color_hists, output, pos_); + + // set remaining values of the descriptor to zero (if any) + std::fill (output.points[0].histogram + pos_, output.points[0].histogram + output.points[0].descriptorSize (), 0.0f); +} + +#define PCL_INSTANTIATE_GASDEstimation(InT, OutT) template class PCL_EXPORTS pcl::GASDEstimation; +#define PCL_INSTANTIATE_GASDColorEstimation(InT, OutT) template class PCL_EXPORTS pcl::GASDColorEstimation; + +#endif // PCL_FEATURES_IMPL_GASD_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/gfpfh.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/gfpfh.hpp new file mode 100755 index 0000000..08a9921 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/gfpfh.hpp @@ -0,0 +1,272 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: gfpfh.hpp 2218 2011-08-25 20:27:15Z rusu $ + * + */ + +#ifndef PCL_FEATURES_IMPL_GFPFH_H_ +#define PCL_FEATURES_IMPL_GFPFH_H_ + +#include +#include +#include + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GFPFHEstimation::compute (PointCloudOut &output) +{ + if (!Feature::initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + // Copy the header + output.header = input_->header; + + // Resize the output dataset + // Important! We should only allocate precisely how many elements we will need, otherwise + // we risk at pre-allocating too much memory which could lead to bad_alloc + // (see http://dev.pointclouds.org/issues/657) + output.width = output.height = 1; + output.is_dense = input_->is_dense; + output.points.resize (1); + + // Perform the actual feature computation + computeFeature (output); + + Feature::deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GFPFHEstimation::computeFeature (PointCloudOut &output) +{ + pcl::octree::OctreePointCloudSearch octree (octree_leaf_size_); + octree.setInputCloud (input_); + octree.addPointsFromInputCloud (); + + typename pcl::PointCloud::VectorType occupied_cells; + octree.getOccupiedVoxelCenters (occupied_cells); + + // Determine the voxels crosses along the line segments + // formed by every pair of occupied cells. + std::vector< std::vector > line_histograms; + for (std::size_t i = 0; i < occupied_cells.size (); ++i) + { + Eigen::Vector3f origin = occupied_cells[i].getVector3fMap (); + + for (std::size_t j = i+1; j < occupied_cells.size (); ++j) + { + typename pcl::PointCloud::VectorType intersected_cells; + Eigen::Vector3f end = occupied_cells[j].getVector3fMap (); + octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f); + + // Intersected cells are ordered from closest to furthest w.r.t. the origin. + std::vector histogram; + for (std::size_t k = 0; k < intersected_cells.size (); ++k) + { + std::vector indices; + octree.voxelSearch (intersected_cells[k], indices); + int label = emptyLabel (); + if (!indices.empty ()) + { + label = getDominantLabel (indices); + } + histogram.push_back (label); + } + + line_histograms.push_back(histogram); + } + } + + std::vector< std::vector > transition_histograms; + computeTransitionHistograms (line_histograms, transition_histograms); + + std::vector distances; + computeDistancesToMean (transition_histograms, distances); + + std::vector gfpfh_histogram; + computeDistanceHistogram (distances, gfpfh_histogram); + + output.clear (); + output.width = 1; + output.height = 1; + output.points.resize (1); + std::copy (gfpfh_histogram.cbegin (), gfpfh_histogram.cend (), output.points[0].histogram); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GFPFHEstimation::computeTransitionHistograms (const std::vector< std::vector >& label_histograms, + std::vector< std::vector >& transition_histograms) +{ + transition_histograms.resize (label_histograms.size ()); + + for (std::size_t i = 0; i < label_histograms.size (); ++i) + { + transition_histograms[i].resize ((getNumberOfClasses () + 2) * (getNumberOfClasses () + 1) / 2, 0); + + std::vector< std::vector > transitions (getNumberOfClasses () + 1); + for (auto &transition : transitions) + { + transition.resize (getNumberOfClasses () + 1, 0); + } + + for (std::size_t k = 1; k < label_histograms[i].size (); ++k) + { + std::uint32_t first_class = label_histograms[i][k-1]; + std::uint32_t second_class = label_histograms[i][k]; + // Order has no influence. + if (second_class < first_class) + std::swap (first_class, second_class); + + transitions[first_class][second_class] += 1; + } + + // Build a one-dimension histogram out of it. + std::size_t flat_index = 0; + for (std::size_t m = 0; m < transitions.size (); ++m) + for (std::size_t n = m; n < transitions[m].size (); ++n) + { + transition_histograms[i][flat_index] = transitions[m][n]; + ++flat_index; + } + + assert (flat_index == transition_histograms[i].size ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GFPFHEstimation::computeDistancesToMean (const std::vector< std::vector >& transition_histograms, + std::vector& distances) +{ + distances.resize (transition_histograms.size ()); + + std::vector mean_histogram; + computeMeanHistogram (transition_histograms, mean_histogram); + + for (std::size_t i = 0; i < transition_histograms.size (); ++i) + { + float d = computeHIKDistance (transition_histograms[i], mean_histogram); + distances[i] = d; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GFPFHEstimation::computeDistanceHistogram (const std::vector& distances, + std::vector& histogram) +{ + std::vector::const_iterator min_it, max_it; + std::tie (min_it, max_it) = std::minmax_element (distances.cbegin (), distances.cend ()); + assert (min_it != distances.cend ()); + assert (max_it != distances.cend ()); + + const float min_value = *min_it; + const float max_value = *max_it; + + histogram.resize (descriptorSize (), 0); + + const float range = max_value - min_value; + + using binSizeT = decltype(descriptorSize()); + const binSizeT max_bin = descriptorSize () - 1; + for (const float &distance : distances) + { + const auto raw_bin = descriptorSize () * (distance - min_value) / range; + const auto bin = std::min (max_bin, static_cast (std::floor (raw_bin))); + histogram[bin] += 1; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GFPFHEstimation::computeMeanHistogram (const std::vector< std::vector >& histograms, + std::vector& mean_histogram) +{ + assert (histograms.size () > 0); + + mean_histogram.resize (histograms[0].size (), 0); + for (const auto &histogram : histograms) + for (std::size_t j = 0; j < histogram.size (); ++j) + mean_histogram[j] += static_cast (histogram[j]); + + for (float &i : mean_histogram) + i /= static_cast (histograms.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::GFPFHEstimation::computeHIKDistance (const std::vector& histogram, + const std::vector& mean_histogram) +{ + assert (histogram.size () == mean_histogram.size ()); + + float norm = 0.f; + for (std::size_t i = 0; i < histogram.size (); ++i) + norm += std::min (static_cast (histogram[i]), mean_histogram[i]); + + norm /= static_cast (histogram.size ()); + return (norm); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::uint32_t +pcl::GFPFHEstimation::getDominantLabel (const std::vector& indices) +{ + std::vector counts (getNumberOfClasses () + 1, 0); + for (const int &nn_index : indices) + { + std::uint32_t label = labels_->points[nn_index].label; + counts[label] += 1; + } + + const auto max_it = std::max_element (counts.cbegin (), counts.cend ()); + if (max_it == counts.end ()) + return (emptyLabel ()); + + return std::distance(counts.cbegin (), max_it); +} + +#define PCL_INSTANTIATE_GFPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GFPFHEstimation; + +#endif // PCL_FEATURES_IMPL_GFPFH_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/grsd.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/grsd.hpp new file mode 100755 index 0000000..daf15ec --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/grsd.hpp @@ -0,0 +1,126 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2014, Willow Garage, Inc. + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_GRSD_H_ +#define PCL_FEATURES_IMPL_GRSD_H_ + +#include +///////// STATIC ///////// +template int +pcl::GRSDEstimation::getSimpleType (float min_radius, float max_radius, + double min_radius_plane, + double max_radius_noise, + double min_radius_cylinder, + double max_min_radius_diff) +{ + if (min_radius > min_radius_plane) + return (1); // plane + if (max_radius > min_radius_cylinder) + return (2); // cylinder (rim) + if (min_radius < max_radius_noise) + return (0); // noise/corner + if (max_radius - min_radius < max_min_radius_diff) + return (3); // sphere/corner + return (4); // edge +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GRSDEstimation::computeFeature (PointCloudOut &output) +{ + // Check if search_radius_ was set + if (width_ < 0) + { + PCL_ERROR ("[pcl::%s::computeFeature] A voxel cell width needs to be set!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Create the voxel grid + PointCloudInPtr cloud_downsampled (new PointCloudIn()); + pcl::VoxelGrid grid; + grid.setLeafSize (width_, width_, width_); + grid.setInputCloud (input_); + grid.setSaveLeafLayout (true); // TODO maybe avoid this using nearest neighbor search + grid.filter (*cloud_downsampled); + + // Compute RSD + pcl::PointCloud::Ptr radii (new pcl::PointCloud()); + pcl::RSDEstimation rsd; + rsd.setInputCloud (cloud_downsampled); + rsd.setSearchSurface (input_); + rsd.setInputNormals (normals_); + rsd.setRadiusSearch (std::max (search_radius_, std::sqrt (3.0) * width_ / 2)); + rsd.compute (*radii); + + // Save the type of each point + int NR_CLASS = 5; // TODO make this nicer + std::vector types (radii->points.size ()); + std::transform(radii->points.cbegin (), radii->points.cend (), types.begin (), + [](const auto& point) { + // GCC 5.4 can't find unqualified getSimpleType + return GRSDEstimation::getSimpleType(point.r_min, point.r_max); }); + + // Get the transitions between surface types between neighbors of occupied cells + Eigen::MatrixXi transition_matrix = Eigen::MatrixXi::Zero (NR_CLASS + 1, NR_CLASS + 1); + for (std::size_t idx = 0; idx < cloud_downsampled->points.size (); ++idx) + { + const int source_type = types[idx]; + std::vector neighbors = grid.getNeighborCentroidIndices (cloud_downsampled->points[idx], relative_coordinates_all_); + for (const int &neighbor : neighbors) + { + int neighbor_type = NR_CLASS; + if (neighbor != -1) // not empty + neighbor_type = types[neighbor]; + transition_matrix (source_type, neighbor_type)++; + } + } + + // Save feature values + output.points.resize (1); + output.height = output.width = 1; + int nrf = 0; + for (int i = 0; i < NR_CLASS + 1; i++) + for (int j = i; j < NR_CLASS + 1; j++) + output.points[0].histogram[nrf++] = transition_matrix (i, j) + transition_matrix (j, i); +} + +#define PCL_INSTANTIATE_GRSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GRSDEstimation; + +#endif /* PCL_FEATURES_IMPL_GRSD_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/integral_image2D.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/integral_image2D.hpp new file mode 100755 index 0000000..abf594b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/integral_image2D.hpp @@ -0,0 +1,385 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: feature.h 2784 2011-10-15 22:05:38Z aichim $ + */ + +#ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_ +#define PCL_INTEGRAL_IMAGE2D_IMPL_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImage2D::setSecondOrderComputation (bool compute_second_order_integral_images) +{ + compute_second_order_integral_images_ = compute_second_order_integral_images; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImage2D::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) +{ + if ((width + 1) * (height + 1) > first_order_integral_image_.size () ) + { + width_ = width; + height_ = height; + first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); + finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); + if (compute_second_order_integral_images_) + second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); + } + computeIntegralImages (data, row_stride, element_stride); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::ElementType +pcl::IntegralImage2D::getFirstOrderSum ( + unsigned start_x, unsigned start_y, unsigned width, unsigned height) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = upper_left_idx + width; + const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; + const unsigned lower_right_idx = lower_left_idx + width; + + return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] - + first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::SecondOrderType +pcl::IntegralImage2D::getSecondOrderSum ( + unsigned start_x, unsigned start_y, unsigned width, unsigned height) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = upper_left_idx + width; + const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; + const unsigned lower_right_idx = lower_left_idx + width; + + return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] - + second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::IntegralImage2D::getFiniteElementsCount ( + unsigned start_x, unsigned start_y, unsigned width, unsigned height) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = upper_left_idx + width; + const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; + const unsigned lower_right_idx = lower_left_idx + width; + + return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] - + finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::ElementType +pcl::IntegralImage2D::getFirstOrderSumSE ( + unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = start_y * (width_ + 1) + end_x; + const unsigned lower_left_idx = end_y * (width_ + 1) + start_x; + const unsigned lower_right_idx = end_y * (width_ + 1) + end_x; + + return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] - + first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::SecondOrderType +pcl::IntegralImage2D::getSecondOrderSumSE ( + unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = start_y * (width_ + 1) + end_x; + const unsigned lower_left_idx = end_y * (width_ + 1) + start_x; + const unsigned lower_right_idx = end_y * (width_ + 1) + end_x; + + return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] - + second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::IntegralImage2D::getFiniteElementsCountSE ( + unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = start_y * (width_ + 1) + end_x; + const unsigned lower_left_idx = end_y * (width_ + 1) + start_x; + const unsigned lower_right_idx = end_y * (width_ + 1) + end_x; + + return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] - + finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImage2D::computeIntegralImages ( + const DataType *data, unsigned row_stride, unsigned element_stride) +{ + ElementType* previous_row = &first_order_integral_image_[0]; + ElementType* current_row = previous_row + (width_ + 1); + memset (previous_row, 0, sizeof (ElementType) * (width_ + 1)); + + unsigned* count_previous_row = &finite_values_integral_image_[0]; + unsigned* count_current_row = count_previous_row + (width_ + 1); + memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1)); + + if (!compute_second_order_integral_images_) + { + for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, + previous_row = current_row, current_row += (width_ + 1), + count_previous_row = count_current_row, count_current_row += (width_ + 1)) + { + current_row [0].setZero (); + count_current_row [0] = 0; + for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) + { + current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; + count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; + const InputType* element = reinterpret_cast (&data [valIdx]); + if (std::isfinite (element->sum ())) + { + current_row [colIdx + 1] += element->template cast::IntegralType>(); + ++(count_current_row [colIdx + 1]); + } + } + } + } + else + { + SecondOrderType* so_previous_row = &second_order_integral_image_[0]; + SecondOrderType* so_current_row = so_previous_row + (width_ + 1); + memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1)); + + SecondOrderType so_element; + for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, + previous_row = current_row, current_row += (width_ + 1), + count_previous_row = count_current_row, count_current_row += (width_ + 1), + so_previous_row = so_current_row, so_current_row += (width_ + 1)) + { + current_row [0].setZero (); + so_current_row [0].setZero (); + count_current_row [0] = 0; + for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) + { + current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; + so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx]; + count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; + + const InputType* element = reinterpret_cast (&data [valIdx]); + if (std::isfinite (element->sum ())) + { + current_row [colIdx + 1] += element->template cast::IntegralType>(); + ++(count_current_row [colIdx + 1]); + for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx) + for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx) + so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx]; + } + } + } + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +template void +pcl::IntegralImage2D::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) +{ + if ((width + 1) * (height + 1) > first_order_integral_image_.size () ) + { + width_ = width; + height_ = height; + first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); + finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); + if (compute_second_order_integral_images_) + second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); + } + computeIntegralImages (data, row_stride, element_stride); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::ElementType +pcl::IntegralImage2D::getFirstOrderSum ( + unsigned start_x, unsigned start_y, unsigned width, unsigned height) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = upper_left_idx + width; + const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; + const unsigned lower_right_idx = lower_left_idx + width; + + return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] - + first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::SecondOrderType +pcl::IntegralImage2D::getSecondOrderSum ( + unsigned start_x, unsigned start_y, unsigned width, unsigned height) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = upper_left_idx + width; + const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; + const unsigned lower_right_idx = lower_left_idx + width; + + return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] - + second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::IntegralImage2D::getFiniteElementsCount ( + unsigned start_x, unsigned start_y, unsigned width, unsigned height) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = upper_left_idx + width; + const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; + const unsigned lower_right_idx = lower_left_idx + width; + + return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] - + finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::ElementType +pcl::IntegralImage2D::getFirstOrderSumSE ( + unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = start_y * (width_ + 1) + end_x; + const unsigned lower_left_idx = end_y * (width_ + 1) + start_x; + const unsigned lower_right_idx = end_y * (width_ + 1) + end_x; + + return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] - + first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::IntegralImage2D::SecondOrderType +pcl::IntegralImage2D::getSecondOrderSumSE ( + unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = start_y * (width_ + 1) + end_x; + const unsigned lower_left_idx = end_y * (width_ + 1) + start_x; + const unsigned lower_right_idx = end_y * (width_ + 1) + end_x; + + return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] - + second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::IntegralImage2D::getFiniteElementsCountSE ( + unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const +{ + const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; + const unsigned upper_right_idx = start_y * (width_ + 1) + end_x; + const unsigned lower_left_idx = end_y * (width_ + 1) + start_x; + const unsigned lower_right_idx = end_y * (width_ + 1) + end_x; + + return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] - + finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImage2D::computeIntegralImages ( + const DataType *data, unsigned row_stride, unsigned element_stride) +{ + ElementType* previous_row = &first_order_integral_image_[0]; + ElementType* current_row = previous_row + (width_ + 1); + memset (previous_row, 0, sizeof (ElementType) * (width_ + 1)); + + unsigned* count_previous_row = &finite_values_integral_image_[0]; + unsigned* count_current_row = count_previous_row + (width_ + 1); + memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1)); + + if (!compute_second_order_integral_images_) + { + for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, + previous_row = current_row, current_row += (width_ + 1), + count_previous_row = count_current_row, count_current_row += (width_ + 1)) + { + current_row [0] = 0.0; + count_current_row [0] = 0; + for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) + { + current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; + count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; + if (std::isfinite (data [valIdx])) + { + current_row [colIdx + 1] += data [valIdx]; + ++(count_current_row [colIdx + 1]); + } + } + } + } + else + { + SecondOrderType* so_previous_row = &second_order_integral_image_[0]; + SecondOrderType* so_current_row = so_previous_row + (width_ + 1); + memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1)); + + for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, + previous_row = current_row, current_row += (width_ + 1), + count_previous_row = count_current_row, count_current_row += (width_ + 1), + so_previous_row = so_current_row, so_current_row += (width_ + 1)) + { + current_row [0] = 0.0; + so_current_row [0] = 0.0; + count_current_row [0] = 0; + for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) + { + current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; + so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx]; + count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; + if (std::isfinite (data[valIdx])) + { + current_row [colIdx + 1] += data[valIdx]; + so_current_row [colIdx + 1] += data[valIdx] * data[valIdx]; + ++(count_current_row [colIdx + 1]); + } + } + } + } +} +#endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/integral_image_normal.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/integral_image_normal.hpp new file mode 100755 index 0000000..8dd6560 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/integral_image_normal.hpp @@ -0,0 +1,1205 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_ +#define PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::IntegralImageNormalEstimation::~IntegralImageNormalEstimation () +{ + delete[] diff_x_; + delete[] diff_y_; + delete[] depth_data_; + delete[] distance_map_; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::initData () +{ + if (border_policy_ != BORDER_POLICY_IGNORE && + border_policy_ != BORDER_POLICY_MIRROR) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::IntegralImageNormalEstimation::initData] unknown border policy."); + + if (normal_estimation_method_ != COVARIANCE_MATRIX && + normal_estimation_method_ != AVERAGE_3D_GRADIENT && + normal_estimation_method_ != AVERAGE_DEPTH_CHANGE && + normal_estimation_method_ != SIMPLE_3D_GRADIENT) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method."); + + // compute derivatives + delete[] diff_x_; + delete[] diff_y_; + delete[] depth_data_; + delete[] distance_map_; + diff_x_ = nullptr; + diff_y_ = nullptr; + depth_data_ = nullptr; + distance_map_ = nullptr; + + if (normal_estimation_method_ == COVARIANCE_MATRIX) + initCovarianceMatrixMethod (); + else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) + initAverage3DGradientMethod (); + else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) + initAverageDepthChangeMethod (); + else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) + initSimple3DGradientMethod (); +} + + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::setRectSize (const int width, const int height) +{ + rect_width_ = width; + rect_width_2_ = width/2; + rect_width_4_ = width/4; + rect_height_ = height; + rect_height_2_ = height/2; + rect_height_4_ = height/4; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::initSimple3DGradientMethod () +{ + // number of DataType entries per element (equal or bigger than dimensions) + int element_stride = sizeof (PointInT) / sizeof (float); + // number of DataType entries per row (equal or bigger than element_stride number of elements per row) + int row_stride = element_stride * input_->width; + + const float *data_ = reinterpret_cast (&input_->points[0]); + + integral_image_XYZ_.setSecondOrderComputation (false); + integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride); + + init_simple_3d_gradient_ = true; + init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::initCovarianceMatrixMethod () +{ + // number of DataType entries per element (equal or bigger than dimensions) + int element_stride = sizeof (PointInT) / sizeof (float); + // number of DataType entries per row (equal or bigger than element_stride number of elements per row) + int row_stride = element_stride * input_->width; + + const float *data_ = reinterpret_cast (&input_->points[0]); + + integral_image_XYZ_.setSecondOrderComputation (true); + integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride); + + init_covariance_matrix_ = true; + init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::initAverage3DGradientMethod () +{ + std::size_t data_size = (input_->points.size () << 2); + diff_x_ = new float[data_size]; + diff_y_ = new float[data_size]; + + memset (diff_x_, 0, sizeof(float) * data_size); + memset (diff_y_, 0, sizeof(float) * data_size); + + // x u x + // l x r + // x d x + const PointInT* point_up = &(input_->points [1]); + const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]); + const PointInT* point_lf = &(input_->points [input_->width]); + const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]); + float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2); + float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2); + unsigned diff_skip = 8; // skip last element in row and the first in the next row + + for (std::size_t ri = 1; ri < input_->height - 1; ++ri + , point_up += input_->width + , point_dn += input_->width + , point_lf += input_->width + , point_rg += input_->width + , diff_x_ptr += diff_skip + , diff_y_ptr += diff_skip) + { + for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4) + { + diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x; + diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y; + diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z; + + diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x; + diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y; + diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z; + } + } + + // Compute integral images + integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2); + integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2); + init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false; + init_average_3d_gradient_ = true; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::initAverageDepthChangeMethod () +{ + // number of DataType entries per element (equal or bigger than dimensions) + int element_stride = sizeof (PointInT) / sizeof (float); + // number of DataType entries per row (equal or bigger than element_stride number of elements per row) + int row_stride = element_stride * input_->width; + + const float *data_ = reinterpret_cast (&input_->points[0]); + + // integral image over the z - value + integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride); + init_depth_change_ = true; + init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::computePointNormal ( + const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal) +{ + float bad_point = std::numeric_limits::quiet_NaN (); + + if (normal_estimation_method_ == COVARIANCE_MATRIX) + { + if (!init_covariance_matrix_) + initCovarianceMatrixMethod (); + + unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_); + + // no valid points within the rectangular region? + if (count == 0) + { + normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; + return; + } + + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector3f center; + typename IntegralImage2D::SecondOrderType so_elements; + center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast (); + so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); + + covariance_matrix.coeffRef (0) = static_cast (so_elements [0]); + covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast (so_elements [1]); + covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast (so_elements [2]); + covariance_matrix.coeffRef (4) = static_cast (so_elements [3]); + covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast (so_elements [4]); + covariance_matrix.coeffRef (8) = static_cast (so_elements [5]); + covariance_matrix -= (center * center.transpose ()) / static_cast (count); + float eigen_value; + Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]); + normal.getNormalVector3fMap () = eigen_vector; + + // Compute the curvature surface change + if (eigen_value > 0.0) + normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8))); + else + normal.curvature = 0; + + return; + } + if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) + { + if (!init_average_3d_gradient_) + initAverage3DGradientMethod (); + + unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); + unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); + if (count_x == 0 || count_y == 0) + { + normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; + return; + } + Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); + Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_); + + Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); + double normal_length = normal_vector.squaredNorm (); + if (normal_length == 0.0f) + { + normal.getNormalVector3fMap ().setConstant (bad_point); + normal.curvature = bad_point; + return; + } + + normal_vector /= sqrt (normal_length); + + float nx = static_cast (normal_vector [0]); + float ny = static_cast (normal_vector [1]); + float nz = static_cast (normal_vector [2]); + + flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); + + normal.normal_x = nx; + normal.normal_y = ny; + normal.normal_z = nz; + normal.curvature = bad_point; + return; + } + if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) + { + if (!init_depth_change_) + initAverageDepthChangeMethod (); + + // width and height are at least 3 x 3 + unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_); + unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_); + unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_); + unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_); + + if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0) + { + normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; + return; + } + + float mean_L_z = static_cast (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_L_z); + float mean_R_z = static_cast (integral_image_depth_.getFirstOrderSum (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_R_z); + float mean_U_z = static_cast (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_) / count_U_z); + float mean_D_z = static_cast (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_) / count_D_z); + + PointInT pointL = input_->points[point_index - rect_width_4_ - 1]; + PointInT pointR = input_->points[point_index + rect_width_4_ + 1]; + PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1]; + PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1]; + + const float mean_x_z = mean_R_z - mean_L_z; + const float mean_y_z = mean_D_z - mean_U_z; + + const float mean_x_x = pointR.x - pointL.x; + const float mean_x_y = pointR.y - pointL.y; + const float mean_y_x = pointD.x - pointU.x; + const float mean_y_y = pointD.y - pointU.y; + + float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; + float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; + float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; + + const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); + + if (normal_length == 0.0f) + { + normal.getNormalVector3fMap ().setConstant (bad_point); + normal.curvature = bad_point; + return; + } + + flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z); + + const float scale = 1.0f / std::sqrt (normal_length); + + normal.normal_x = normal_x * scale; + normal.normal_y = normal_y * scale; + normal.normal_z = normal_z * scale; + normal.curvature = bad_point; + + return; + } + if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) + { + if (!init_simple_3d_gradient_) + initSimple3DGradientMethod (); + + // this method does not work if lots of NaNs are in the neighborhood of the point + Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) - + integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_); + + Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) - + integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1); + Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); + double normal_length = normal_vector.squaredNorm (); + if (normal_length == 0.0f) + { + normal.getNormalVector3fMap ().setConstant (bad_point); + normal.curvature = bad_point; + return; + } + + normal_vector /= sqrt (normal_length); + + float nx = static_cast (normal_vector [0]); + float ny = static_cast (normal_vector [1]); + float nz = static_cast (normal_vector [2]); + + flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); + + normal.normal_x = nx; + normal.normal_y = ny; + normal.normal_z = nz; + normal.curvature = bad_point; + return; + } + + normal.getNormalVector3fMap ().setConstant (bad_point); + normal.curvature = bad_point; + return; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template +void +sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height, + const std::function &f, + T & result) +{ + if (start_x < 0) + { + if (start_y < 0) + { + result += f (0, 0, end_x, end_y); + result += f (0, 0, -start_x, -start_y); + result += f (0, 0, -start_x, end_y); + result += f (0, 0, end_x, -start_y); + } + else if (end_y >= height) + { + result += f (0, start_y, end_x, height-1); + result += f (0, start_y, -start_x, height-1); + result += f (0, height-(end_y-(height-1)), end_x, height-1); + result += f (0, height-(end_y-(height-1)), -start_x, height-1); + } + else + { + result += f (0, start_y, end_x, end_y); + result += f (0, start_y, -start_x, end_y); + } + } + else if (start_y < 0) + { + if (end_x >= width) + { + result += f (start_x, 0, width-1, end_y); + result += f (start_x, 0, width-1, -start_y); + result += f (width-(end_x-(width-1)), 0, width-1, end_y); + result += f (width-(end_x-(width-1)), 0, width-1, -start_y); + } + else + { + result += f (start_x, 0, end_x, end_y); + result += f (start_x, 0, end_x, -start_y); + } + } + else if (end_x >= width) + { + if (end_y >= height) + { + result += f (start_x, start_y, width-1, height-1); + result += f (start_x, height-(end_y-(height-1)), width-1, height-1); + result += f (width-(end_x-(width-1)), start_y, width-1, height-1); + result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1); + } + else + { + result += f (start_x, start_y, width-1, end_y); + result += f (width-(end_x-(width-1)), start_y, width-1, end_y); + } + } + else if (end_y >= height) + { + result += f (start_x, start_y, end_x, height-1); + result += f (start_x, height-(end_y-(height-1)), end_x, height-1); + } + else + { + result += f (start_x, start_y, end_x, end_y); + } +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::computePointNormalMirror ( + const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal) +{ + float bad_point = std::numeric_limits::quiet_NaN (); + + const int width = input_->width; + const int height = input_->height; + + // ============================================================== + if (normal_estimation_method_ == COVARIANCE_MATRIX) + { + if (!init_covariance_matrix_) + initCovarianceMatrixMethod (); + + const int start_x = pos_x - rect_width_2_; + const int start_y = pos_y - rect_height_2_; + const int end_x = start_x + rect_width_; + const int end_y = start_y + rect_height_; + + unsigned count = 0; + auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); }; + sumArea (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count); + + // no valid points within the rectangular region? + if (count == 0) + { + normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; + return; + } + + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector3f center; + typename IntegralImage2D::SecondOrderType so_elements; + typename IntegralImage2D::ElementType tmp_center; + typename IntegralImage2D::SecondOrderType tmp_so_elements; + + center[0] = 0; + center[1] = 0; + center[2] = 0; + tmp_center[0] = 0; + tmp_center[1] = 0; + tmp_center[2] = 0; + so_elements[0] = 0; + so_elements[1] = 0; + so_elements[2] = 0; + so_elements[3] = 0; + so_elements[4] = 0; + so_elements[5] = 0; + + auto cb_xyz_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFirstOrderSumSE (p1, p2, p3, p4); }; + sumArea::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center); + auto cb_xyz_sosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); }; + sumArea::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements); + + center[0] = float (tmp_center[0]); + center[1] = float (tmp_center[1]); + center[2] = float (tmp_center[2]); + + covariance_matrix.coeffRef (0) = static_cast (so_elements [0]); + covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast (so_elements [1]); + covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast (so_elements [2]); + covariance_matrix.coeffRef (4) = static_cast (so_elements [3]); + covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast (so_elements [4]); + covariance_matrix.coeffRef (8) = static_cast (so_elements [5]); + covariance_matrix -= (center * center.transpose ()) / static_cast (count); + float eigen_value; + Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]); + normal.getNormalVector3fMap () = eigen_vector; + + // Compute the curvature surface change + if (eigen_value > 0.0) + normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8))); + else + normal.curvature = 0; + + return; + } + // ======================================================= + if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) + { + if (!init_average_3d_gradient_) + initAverage3DGradientMethod (); + + const int start_x = pos_x - rect_width_2_; + const int start_y = pos_y - rect_height_2_; + const int end_x = start_x + rect_width_; + const int end_y = start_y + rect_height_; + + unsigned count_x = 0; + unsigned count_y = 0; + + auto cb_dx_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); }; + sumArea(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x); + auto cb_dy_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); }; + sumArea(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y); + + + if (count_x == 0 || count_y == 0) + { + normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; + return; + } + Eigen::Vector3d gradient_x (0, 0, 0); + Eigen::Vector3d gradient_y (0, 0, 0); + + auto cb_dx_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); }; + sumArea::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x); + auto cb_dy_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); }; + sumArea::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y); + + + Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x); + double normal_length = normal_vector.squaredNorm (); + if (normal_length == 0.0f) + { + normal.getNormalVector3fMap ().setConstant (bad_point); + normal.curvature = bad_point; + return; + } + + normal_vector /= sqrt (normal_length); + + float nx = static_cast (normal_vector [0]); + float ny = static_cast (normal_vector [1]); + float nz = static_cast (normal_vector [2]); + + flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz); + + normal.normal_x = nx; + normal.normal_y = ny; + normal.normal_z = nz; + normal.curvature = bad_point; + return; + } + // ====================================================== + if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) + { + if (!init_depth_change_) + initAverageDepthChangeMethod (); + + int point_index_L_x = pos_x - rect_width_4_ - 1; + int point_index_L_y = pos_y; + int point_index_R_x = pos_x + rect_width_4_ + 1; + int point_index_R_y = pos_y; + int point_index_U_x = pos_x - 1; + int point_index_U_y = pos_y - rect_height_4_; + int point_index_D_x = pos_x + 1; + int point_index_D_y = pos_y + rect_height_4_; + + if (point_index_L_x < 0) + point_index_L_x = -point_index_L_x; + if (point_index_U_x < 0) + point_index_U_x = -point_index_U_x; + if (point_index_U_y < 0) + point_index_U_y = -point_index_U_y; + + if (point_index_R_x >= width) + point_index_R_x = width-(point_index_R_x-(width-1)); + if (point_index_D_x >= width) + point_index_D_x = width-(point_index_D_x-(width-1)); + if (point_index_D_y >= height) + point_index_D_y = height-(point_index_D_y-(height-1)); + + const int start_x_L = pos_x - rect_width_2_; + const int start_y_L = pos_y - rect_height_4_; + const int end_x_L = start_x_L + rect_width_2_; + const int end_y_L = start_y_L + rect_height_2_; + + const int start_x_R = pos_x + 1; + const int start_y_R = pos_y - rect_height_4_; + const int end_x_R = start_x_R + rect_width_2_; + const int end_y_R = start_y_R + rect_height_2_; + + const int start_x_U = pos_x - rect_width_4_; + const int start_y_U = pos_y - rect_height_2_; + const int end_x_U = start_x_U + rect_width_2_; + const int end_y_U = start_y_U + rect_height_2_; + + const int start_x_D = pos_x - rect_width_4_; + const int start_y_D = pos_y + 1; + const int end_x_D = start_x_D + rect_width_2_; + const int end_y_D = start_y_D + rect_height_2_; + + unsigned count_L_z = 0; + unsigned count_R_z = 0; + unsigned count_U_z = 0; + unsigned count_D_z = 0; + + auto cb_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); }; + sumArea(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z); + sumArea(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z); + sumArea(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z); + sumArea(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z); + + if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0) + { + normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point; + return; + } + + float mean_L_z = 0; + float mean_R_z = 0; + float mean_U_z = 0; + float mean_D_z = 0; + + auto cb_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); }; + sumArea(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z); + sumArea(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z); + sumArea(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z); + sumArea(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z); + + mean_L_z /= float (count_L_z); + mean_R_z /= float (count_R_z); + mean_U_z /= float (count_U_z); + mean_D_z /= float (count_D_z); + + + PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x]; + PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x]; + PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x]; + PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x]; + + const float mean_x_z = mean_R_z - mean_L_z; + const float mean_y_z = mean_D_z - mean_U_z; + + const float mean_x_x = pointR.x - pointL.x; + const float mean_x_y = pointR.y - pointL.y; + const float mean_y_x = pointD.x - pointU.x; + const float mean_y_y = pointD.y - pointU.y; + + float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y; + float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z; + float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x; + + const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z); + + if (normal_length == 0.0f) + { + normal.getNormalVector3fMap ().setConstant (bad_point); + normal.curvature = bad_point; + return; + } + + flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z); + + const float scale = 1.0f / std::sqrt (normal_length); + + normal.normal_x = normal_x * scale; + normal.normal_y = normal_y * scale; + normal.normal_z = normal_z * scale; + normal.curvature = bad_point; + + return; + } + // ======================================================== + if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) + { + PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT"); + } + + normal.getNormalVector3fMap ().setConstant (bad_point); + normal.curvature = bad_point; + return; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::computeFeature (PointCloudOut &output) +{ + output.sensor_origin_ = input_->sensor_origin_; + output.sensor_orientation_ = input_->sensor_orientation_; + + float bad_point = std::numeric_limits::quiet_NaN (); + + // compute depth-change map + unsigned char * depthChangeMap = new unsigned char[input_->points.size ()]; + memset (depthChangeMap, 255, input_->points.size ()); + + unsigned index = 0; + for (unsigned int ri = 0; ri < input_->height-1; ++ri) + { + for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index) + { + index = ri * input_->width + ci; + + const float depth = input_->points [index].z; + const float depthR = input_->points [index + 1].z; + const float depthD = input_->points [index + input_->width].z; + + //const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs(depth)+1.0f))/(500.0f*0.001f); + const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f); + + if (std::fabs (depth - depthR) > depthDependendDepthChange + || !std::isfinite (depth) || !std::isfinite (depthR)) + { + depthChangeMap[index] = 0; + depthChangeMap[index+1] = 0; + } + if (std::fabs (depth - depthD) > depthDependendDepthChange + || !std::isfinite (depth) || !std::isfinite (depthD)) + { + depthChangeMap[index] = 0; + depthChangeMap[index + input_->width] = 0; + } + } + } + + // compute distance map + //float *distanceMap = new float[input_->points.size ()]; + delete[] distance_map_; + distance_map_ = new float[input_->points.size ()]; + float *distanceMap = distance_map_; + for (std::size_t index = 0; index < input_->points.size (); ++index) + { + if (depthChangeMap[index] == 0) + distanceMap[index] = 0.0f; + else + distanceMap[index] = static_cast (input_->width + input_->height); + } + + // first pass + float* previous_row = distanceMap; + float* current_row = previous_row + input_->width; + for (std::size_t ri = 1; ri < input_->height; ++ri) + { + for (std::size_t ci = 1; ci < input_->width; ++ci) + { + const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f; + const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f; + const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f; + const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f; + const float center = current_row [ci]; //distanceMap[ri*input_->width + ci]; + + const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight)); + + if (minValue < center) + current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue; + } + previous_row = current_row; + current_row += input_->width; + } + + float* next_row = distanceMap + input_->width * (input_->height - 1); + current_row = next_row - input_->width; + // second pass + for (int ri = input_->height-2; ri >= 0; --ri) + { + for (int ci = input_->width-2; ci >= 0; --ci) + { + const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f; + const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f; + const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f; + const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f; + const float center = current_row [ci]; //distanceMap[ri*input_->width + ci]; + + const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight)); + + if (minValue < center) + current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue; + } + next_row = current_row; + current_row -= input_->width; + } + + if (indices_->size () < input_->size ()) + computeFeaturePart (distanceMap, bad_point, output); + else + computeFeatureFull (distanceMap, bad_point, output); + + delete[] depthChangeMap; +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::computeFeatureFull (const float *distanceMap, + const float &bad_point, + PointCloudOut &output) +{ + unsigned index = 0; + + if (border_policy_ == BORDER_POLICY_IGNORE) + { + // Set all normals that we do not touch to NaN + // top and bottom borders + // That sets the output density to false! + output.is_dense = false; + unsigned border = int(normal_smoothing_size_); + PointOutT* vec1 = &output [0]; + PointOutT* vec2 = vec1 + input_->width * (input_->height - border); + + std::size_t count = border * input_->width; + for (std::size_t idx = 0; idx < count; ++idx) + { + vec1 [idx].getNormalVector3fMap ().setConstant (bad_point); + vec1 [idx].curvature = bad_point; + vec2 [idx].getNormalVector3fMap ().setConstant (bad_point); + vec2 [idx].curvature = bad_point; + } + + // left and right borders actually columns + vec1 = &output [border * input_->width]; + vec2 = vec1 + input_->width - border; + for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width) + { + for (std::size_t ci = 0; ci < border; ++ci) + { + vec1 [ci].getNormalVector3fMap ().setConstant (bad_point); + vec1 [ci].curvature = bad_point; + vec2 [ci].getNormalVector3fMap ().setConstant (bad_point); + vec2 [ci].curvature = bad_point; + } + } + + if (use_depth_dependent_smoothing_) + { + index = border + input_->width * border; + unsigned skip = (border << 1); + for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) + { + for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) + { + index = ri * input_->width + ci; + + const float depth = input_->points[index].z; + if (!std::isfinite (depth)) + { + output[index].getNormalVector3fMap ().setConstant (bad_point); + output[index].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast(depth)/10.0f); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormal (ci, ri, index, output [index]); + } + else + { + output[index].getNormalVector3fMap ().setConstant (bad_point); + output[index].curvature = bad_point; + } + } + } + } + else + { + float smoothing_constant = normal_smoothing_size_; + + index = border + input_->width * border; + unsigned skip = (border << 1); + for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) + { + for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) + { + index = ri * input_->width + ci; + + if (!std::isfinite (input_->points[index].z)) + { + output [index].getNormalVector3fMap ().setConstant (bad_point); + output [index].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[index], smoothing_constant); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormal (ci, ri, index, output [index]); + } + else + { + output [index].getNormalVector3fMap ().setConstant (bad_point); + output [index].curvature = bad_point; + } + } + } + } + } + else if (border_policy_ == BORDER_POLICY_MIRROR) + { + output.is_dense = false; + + if (use_depth_dependent_smoothing_) + { + //index = 0; + //unsigned skip = 0; + //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip) + for (unsigned ri = 0; ri < input_->height; ++ri) + { + //for (unsigned ci = 0; ci < input_->width; ++ci, ++index) + for (unsigned ci = 0; ci < input_->width; ++ci) + { + index = ri * input_->width + ci; + + const float depth = input_->points[index].z; + if (!std::isfinite (depth)) + { + output[index].getNormalVector3fMap ().setConstant (bad_point); + output[index].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast(depth)/10.0f); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormalMirror (ci, ri, index, output [index]); + } + else + { + output[index].getNormalVector3fMap ().setConstant (bad_point); + output[index].curvature = bad_point; + } + } + } + } + else + { + float smoothing_constant = normal_smoothing_size_; + + //index = border + input_->width * border; + //unsigned skip = (border << 1); + //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip) + for (unsigned ri = 0; ri < input_->height; ++ri) + { + //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index) + for (unsigned ci = 0; ci < input_->width; ++ci) + { + index = ri * input_->width + ci; + + if (!std::isfinite (input_->points[index].z)) + { + output [index].getNormalVector3fMap ().setConstant (bad_point); + output [index].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[index], smoothing_constant); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormalMirror (ci, ri, index, output [index]); + } + else + { + output [index].getNormalVector3fMap ().setConstant (bad_point); + output [index].curvature = bad_point; + } + } + } + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntegralImageNormalEstimation::computeFeaturePart (const float *distanceMap, + const float &bad_point, + PointCloudOut &output) +{ + if (border_policy_ == BORDER_POLICY_IGNORE) + { + output.is_dense = false; + unsigned border = int(normal_smoothing_size_); + unsigned bottom = input_->height > border ? input_->height - border : 0; + unsigned right = input_->width > border ? input_->width - border : 0; + if (use_depth_dependent_smoothing_) + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + if (v < border || v > bottom) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + if (u < border || u > right) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + const float depth = input_->points[pt_index].z; + if (!std::isfinite (depth)) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast(depth)/10.0f); + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormal (u, v, pt_index, output [idx]); + } + else + { + output[idx].getNormalVector3fMap ().setConstant (bad_point); + output[idx].curvature = bad_point; + } + } + } + else + { + float smoothing_constant = normal_smoothing_size_; + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + if (v < border || v > bottom) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + if (u < border || u > right) + { + output.points[idx].getNormalVector3fMap ().setConstant (bad_point); + output.points[idx].curvature = bad_point; + continue; + } + + if (!std::isfinite (input_->points[pt_index].z)) + { + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormal (u, v, pt_index, output [idx]); + } + else + { + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; + } + } + } + }// border_policy_ == BORDER_POLICY_IGNORE + else if (border_policy_ == BORDER_POLICY_MIRROR) + { + output.is_dense = false; + + if (use_depth_dependent_smoothing_) + { + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + + const float depth = input_->points[pt_index].z; + if (!std::isfinite (depth)) + { + output[idx].getNormalVector3fMap ().setConstant (bad_point); + output[idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast(depth)/10.0f); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormalMirror (u, v, pt_index, output [idx]); + } + else + { + output[idx].getNormalVector3fMap ().setConstant (bad_point); + output[idx].curvature = bad_point; + } + } + } + else + { + float smoothing_constant = normal_smoothing_size_; + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + unsigned pt_index = (*indices_)[idx]; + unsigned u = pt_index % input_->width; + unsigned v = pt_index / input_->width; + + if (!std::isfinite (input_->points[pt_index].z)) + { + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; + continue; + } + + float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant); + + if (smoothing > 2.0f) + { + setRectSize (static_cast (smoothing), static_cast (smoothing)); + computePointNormalMirror (u, v, pt_index, output [idx]); + } + else + { + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; + } + } + } + } // border_policy_ == BORDER_POLICY_MIRROR +} + +////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::IntegralImageNormalEstimation::initCompute () +{ + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n"); + return (false); + } + return (Feature::initCompute ()); +} + +#define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation; + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/intensity_gradient.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/intensity_gradient.hpp new file mode 100755 index 0000000..3afbee4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/intensity_gradient.hpp @@ -0,0 +1,237 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_ +#define PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntensityGradientEstimation ::computePointIntensityGradient ( + const pcl::PointCloud &cloud, const std::vector &indices, + const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient) +{ + if (indices.size () < 3) + { + gradient[0] = gradient[1] = gradient[2] = std::numeric_limits::quiet_NaN (); + return; + } + + Eigen::Matrix3f A = Eigen::Matrix3f::Zero (); + Eigen::Vector3f b = Eigen::Vector3f::Zero (); + + for (const int &nn_index : indices) + { + PointInT p = cloud.points[nn_index]; + if (!std::isfinite (p.x) || + !std::isfinite (p.y) || + !std::isfinite (p.z) || + !std::isfinite (intensity_ (p))) + continue; + + p.x -= point[0]; + p.y -= point[1]; + p.z -= point[2]; + intensity_.demean (p, mean_intensity); + + A (0, 0) += p.x * p.x; + A (0, 1) += p.x * p.y; + A (0, 2) += p.x * p.z; + + A (1, 1) += p.y * p.y; + A (1, 2) += p.y * p.z; + + A (2, 2) += p.z * p.z; + + b[0] += p.x * intensity_ (p); + b[1] += p.y * intensity_ (p); + b[2] += p.z * intensity_ (p); + } + // Fill in the lower triangle of A + A (1, 0) = A (0, 1); + A (2, 0) = A (0, 2); + A (2, 1) = A (1, 2); + +//* + Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b); +/*/ + + Eigen::Vector3f eigen_values; + Eigen::Matrix3f eigen_vectors; + eigen33 (A, eigen_vectors, eigen_values); + + b = eigen_vectors.transpose () * b; + + if ( eigen_values (0) != 0) + b (0) /= eigen_values (0); + else + b (0) = 0; + + if ( eigen_values (1) != 0) + b (1) /= eigen_values (1); + else + b (1) = 0; + + if ( eigen_values (2) != 0) + b (2) /= eigen_values (2); + else + b (2) = 0; + + + Eigen::Vector3f x = eigen_vectors * b; + +// if (A.col (0).squaredNorm () != 0) +// x [0] /= A.col (0).squaredNorm (); +// b -= x [0] * A.col (0); +// +// +// if (A.col (1).squaredNorm () != 0) +// x [1] /= A.col (1).squaredNorm (); +// b -= x[1] * A.col (1); +// +// x [2] = b.dot (A.col (2)); +// if (A.col (2).squaredNorm () != 0) +// x[2] /= A.col (2).squaredNorm (); + // Fit a hyperplane to the data + +//*/ +// std::cout << A << "\n*\n" << bb << "\n=\n" << x << "\nvs.\n" << x2 << "\n\n"; +// std::cout << A * x << "\nvs.\n" << A * x2 << "\n\n------\n"; + // Project the gradient vector, x, onto the tangent plane + gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntensityGradientEstimation::computeFeature (PointCloudOut &output) +{ + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + output.is_dense = true; + + // If the data is dense, we don't need to check for NaN + if (surface_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) +#endif + // Iterating over the entire index vector + for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) + { + PointOutT &p_out = output.points[idx]; + + if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists)) + { + p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + Eigen::Vector3f centroid; + float mean_intensity = 0; + // Initialize to 0 + centroid.setZero (); + for (const int &nn_index : nn_indices) + { + centroid += surface_->points[nn_index].getVector3fMap (); + mean_intensity += intensity_ (surface_->points[nn_index]); + } + centroid /= static_cast (nn_indices.size ()); + mean_intensity /= static_cast (nn_indices.size ()); + + Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal); + Eigen::Vector3f gradient; + computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient); + + p_out.gradient[0] = gradient[0]; + p_out.gradient[1] = gradient[1]; + p_out.gradient[2] = gradient[2]; + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) +#endif + // Iterating over the entire index vector + for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) + { + PointOutT &p_out = output.points[idx]; + if (!isFinite ((*surface_) [(*indices_)[idx]]) || + !this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists)) + { + p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + Eigen::Vector3f centroid; + float mean_intensity = 0; + // Initialize to 0 + centroid.setZero (); + unsigned cp = 0; + for (const int &nn_index : nn_indices) + { + // Check if the point is invalid + if (!isFinite ((*surface_) [nn_index])) + continue; + + centroid += surface_->points [nn_index].getVector3fMap (); + mean_intensity += intensity_ (surface_->points [nn_index]); + ++cp; + } + centroid /= static_cast (cp); + mean_intensity /= static_cast (cp); + Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal); + Eigen::Vector3f gradient; + computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient); + + p_out.gradient[0] = gradient[0]; + p_out.gradient[1] = gradient[1]; + p_out.gradient[2] = gradient[2]; + } + } +} + +#define PCL_INSTANTIATE_IntensityGradientEstimation(InT,NT,OutT) template class PCL_EXPORTS pcl::IntensityGradientEstimation; + +#endif // PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/intensity_spin.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/intensity_spin.hpp new file mode 100755 index 0000000..36f4fb7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/intensity_spin.hpp @@ -0,0 +1,174 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_INTENSITY_SPIN_H_ +#define PCL_FEATURES_IMPL_INTENSITY_SPIN_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntensitySpinEstimation::computeIntensitySpinImage ( + const PointCloudIn &cloud, float radius, float sigma, + int k, + const std::vector &indices, + const std::vector &squared_distances, + Eigen::MatrixXf &intensity_spin_image) +{ + // Determine the number of bins to use based on the size of intensity_spin_image + int nr_distance_bins = static_cast (intensity_spin_image.cols ()); + int nr_intensity_bins = static_cast (intensity_spin_image.rows ()); + + // Find the min and max intensity values in the given neighborhood + float min_intensity = std::numeric_limits::max (); + float max_intensity = -std::numeric_limits::max (); + for (int idx = 0; idx < k; ++idx) + { + min_intensity = (std::min) (min_intensity, cloud.points[indices[idx]].intensity); + max_intensity = (std::max) (max_intensity, cloud.points[indices[idx]].intensity); + } + + float constant = 1.0f / (2.0f * sigma_ * sigma_); + // Compute the intensity spin image + intensity_spin_image.setZero (); + for (int idx = 0; idx < k; ++idx) + { + // Normalize distance and intensity values to: 0.0 <= d,i < nr_distance_bins,nr_intensity_bins + const float eps = std::numeric_limits::epsilon (); + float d = static_cast (nr_distance_bins) * std::sqrt (squared_distances[idx]) / (radius + eps); + float i = static_cast (nr_intensity_bins) * + (cloud.points[indices[idx]].intensity - min_intensity) / (max_intensity - min_intensity + eps); + + if (sigma == 0) + { + // If sigma is zero, update the histogram with no smoothing kernel + int d_idx = static_cast (d); + int i_idx = static_cast (i); + intensity_spin_image (i_idx, d_idx) += 1; + } + else + { + // Compute the bin indices that need to be updated (+/- 3 standard deviations) + int d_idx_min = (std::max)(static_cast (std::floor (d - 3*sigma)), 0); + int d_idx_max = (std::min)(static_cast (std::ceil (d + 3*sigma)), nr_distance_bins - 1); + int i_idx_min = (std::max)(static_cast (std::floor (i - 3*sigma)), 0); + int i_idx_max = (std::min)(static_cast (std::ceil (i + 3*sigma)), nr_intensity_bins - 1); + + // Update the appropriate bins of the histogram + for (int i_idx = i_idx_min; i_idx <= i_idx_max; ++i_idx) + { + for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx) + { + // Compute a "soft" update weight based on the distance between the point and the bin + float w = std::exp (-powf (d - static_cast (d_idx), 2.0f) * constant - powf (i - static_cast (i_idx), 2.0f) * constant); + intensity_spin_image (i_idx, d_idx) += w; + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IntensitySpinEstimation::computeFeature (PointCloudOut &output) +{ + // Make sure a search radius is set + if (search_radius_ == 0.0) + { + PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n", + getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Make sure the spin image has valid dimensions + if (nr_intensity_bins_ <= 0) + { + PCL_ERROR ("[pcl::%s::computeFeature] The number of intensity bins must be greater than zero!\n", + getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + if (nr_distance_bins_ <= 0) + { + PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n", + getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + Eigen::MatrixXf intensity_spin_image (nr_intensity_bins_, nr_distance_bins_); + // Allocate enough space to hold the radiusSearch results + std::vector nn_indices (surface_->points.size ()); + std::vector nn_dist_sqr (surface_->points.size ()); + + output.is_dense = true; + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + // Find neighbors within the search radius + // TODO: do we want to use searchForNeigbors instead? + int k = tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr); + if (k == 0) + { + for (int bin = 0; bin < nr_intensity_bins_ * nr_distance_bins_; ++bin) + output.points[idx].histogram[bin] = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + // Compute the intensity spin image + computeIntensitySpinImage (*surface_, static_cast (search_radius_), sigma_, k, nn_indices, nn_dist_sqr, intensity_spin_image); + + // Copy into the resultant cloud + std::size_t bin = 0; + for (Eigen::Index bin_j = 0; bin_j < intensity_spin_image.cols (); ++bin_j) + for (Eigen::Index bin_i = 0; bin_i < intensity_spin_image.rows (); ++bin_i) + output.points[idx].histogram[bin++] = intensity_spin_image (bin_i, bin_j); + } +} + +#define PCL_INSTANTIATE_IntensitySpinEstimation(T,NT) template class PCL_EXPORTS pcl::IntensitySpinEstimation; + +#endif // PCL_FEATURES_IMPL_INTENSITY_SPIN_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/linear_least_squares_normal.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/linear_least_squares_normal.hpp new file mode 100755 index 0000000..73bd0e3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/linear_least_squares_normal.hpp @@ -0,0 +1,270 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_ +#define PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_ +#define EIGEN_II_METHOD 1 + +#include +#include +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::LinearLeastSquaresNormalEstimation::~LinearLeastSquaresNormalEstimation () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LinearLeastSquaresNormalEstimation::computePointNormal ( + const int pos_x, const int pos_y, PointOutT &normal) +{ + const float bad_point = std::numeric_limits::quiet_NaN (); + + const int width = input_->width; + const int height = input_->height; + + const int x = pos_x; + const int y = pos_y; + + const int index = y * width + x; + + const float px = input_->points[index].x; + const float py = input_->points[index].y; + const float pz = input_->points[index].z; + + if (std::isnan (px)) + { + normal.normal_x = bad_point; + normal.normal_y = bad_point; + normal.normal_z = bad_point; + normal.curvature = bad_point; + + return; + } + + float smoothingSize = normal_smoothing_size_; + if (use_depth_dependent_smoothing_) smoothingSize = smoothingSize*(pz+0.5f); + + const int smoothingSizeInt = static_cast (smoothingSize); + + float matA0 = 0.0f; + float matA1 = 0.0f; + float matA3 = 0.0f; + + float vecb0 = 0.0f; + float vecb1 = 0.0f; + + for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt) + { + for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt) + { + if (u < 0 || u >= width || v < 0 || v >= height) continue; + + const int index2 = v * width + u; + + const float qx = input_->points[index2].x; + const float qy = input_->points[index2].y; + const float qz = input_->points[index2].z; + + if (std::isnan (qx)) continue; + + const float delta = qz - pz; + const float i = qx - px; + const float j = qy - py; + + float depthChangeThreshold = pz*pz * 0.05f * max_depth_change_factor_; + if (use_depth_dependent_smoothing_) depthChangeThreshold *= pz; + + const float f = std::fabs (delta) > depthChangeThreshold ? 0 : 1; + + matA0 += f * i * i; + matA1 += f * i * j; + matA3 += f * j * j; + vecb0 += f * i * delta; + vecb1 += f * j * delta; + } + } + + const float det = matA0 * matA3 - matA1 * matA1; + const float ddx = matA3 * vecb0 - matA1 * vecb1; + const float ddy = -matA1 * vecb0 + matA0 * vecb1; + + const float nx = ddx; + const float ny = ddy; + const float nz = -det * pz; + + const float length = nx * nx + ny * ny + nz * nz; + + if (length <= 0.01f) + { + normal.normal_x = bad_point; + normal.normal_y = bad_point; + normal.normal_z = bad_point; + normal.curvature = bad_point; + } + else + { + const float normInv = 1.0f / std::sqrt (length); + + normal.normal_x = -nx * normInv; + normal.normal_y = -ny * normInv; + normal.normal_z = -nz * normInv; + normal.curvature = bad_point; + } + + return; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LinearLeastSquaresNormalEstimation::computeFeature (PointCloudOut &output) +{ + const float bad_point = std::numeric_limits::quiet_NaN (); + + const int width = input_->width; + const int height = input_->height; + + // we compute the normals as follows: + // ---------------------------------- + // + // for the depth-gradient you can make the following first-order Taylor approximation: + // D(x + dx) - D(x) = dx^T \Delta D + h.o.t. + // + // build linear system by stacking up equation for 8 neighbor points: + // Y = X \Delta D + // + // => \Delta D = (X^T X)^{-1} X^T Y + // => \Delta D = (A)^{-1} b + + //const float smoothingSize = 30.0f; + for (int y = 0; y < height; ++y) + { + for (int x = 0; x < width; ++x) + { + const int index = y * width + x; + + const float px = input_->points[index].x; + const float py = input_->points[index].y; + const float pz = input_->points[index].z; + + if (std::isnan(px)) continue; + + //float depthDependentSmoothingSize = smoothingSize + pz / 10.0f; + + float smoothingSize = normal_smoothing_size_; + //if (use_depth_dependent_smoothing_) smoothingSize *= pz; + //if (use_depth_dependent_smoothing_) smoothingSize += pz*5; + //if (smoothingSize < 1.0f) smoothingSize += 1.0f; + + const int smoothingSizeInt = static_cast(smoothingSize); + + float matA0 = 0.0f; + float matA1 = 0.0f; + float matA3 = 0.0f; + + float vecb0 = 0.0f; + float vecb1 = 0.0f; + + for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt) + { + for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt) + { + if (u < 0 || u >= width || v < 0 || v >= height) continue; + + const int index2 = v * width + u; + + const float qx = input_->points[index2].x; + const float qy = input_->points[index2].y; + const float qz = input_->points[index2].z; + + if (std::isnan(qx)) continue; + + const float delta = qz - pz; + const float i = qx - px; + const float j = qy - py; + + const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (pz) + 1.0f) * 2.0f); + const float f = std::fabs(delta) > depthDependendDepthChange ? 0 : 1; + + //float f = std::abs(delta) > (pz * 0.05f - 0.3f) ? 0 : 1; + //const float f = std::abs(delta) > (pz*pz * 0.05f * max_depth_change_factor_) ? 0 : 1; + //float f = Math.Abs(delta) > (depth * Math.Log(depth + 1.0) * 0.02f - 0.2f) ? 0 : 1; + + matA0 += f * i * i; + matA1 += f * i * j; + matA3 += f * j * j; + vecb0 += f * i * delta; + vecb1 += f * j * delta; + } + } + + const float det = matA0 * matA3 - matA1 * matA1; + const float ddx = matA3 * vecb0 - matA1 * vecb1; + const float ddy = -matA1 * vecb0 + matA0 * vecb1; + + const float nx = ddx; + const float ny = ddy; + const float nz = -det * pz; + + const float length = nx * nx + ny * ny + nz * nz; + + if (length <= 0.0f) + { + output.points[index].normal_x = bad_point; + output.points[index].normal_y = bad_point; + output.points[index].normal_z = bad_point; + output.points[index].curvature = bad_point; + } + else + { + const float normInv = 1.0f / std::sqrt (length); + + output.points[index].normal_x = nx * normInv; + output.points[index].normal_y = ny * normInv; + output.points[index].normal_z = nz * normInv; + output.points[index].curvature = bad_point; + } + } + } +} + +#define PCL_INSTANTIATE_LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation; +//#define LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation; + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/moment_invariants.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/moment_invariants.hpp new file mode 100755 index 0000000..14baabc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/moment_invariants.hpp @@ -0,0 +1,163 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_ +#define PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentInvariantsEstimation::computePointMomentInvariants ( + const pcl::PointCloud &cloud, const std::vector &indices, + float &j1, float &j2, float &j3) +{ + // Estimate the XYZ centroid + compute3DCentroid (cloud, indices, xyz_centroid_); + + // Initialize the centralized moments + float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011 = 0; + + // Iterate over the nearest neighbors set + for (const int &index : indices) + { + // Demean the points + temp_pt_[0] = cloud.points[index].x - xyz_centroid_[0]; + temp_pt_[1] = cloud.points[index].y - xyz_centroid_[1]; + temp_pt_[2] = cloud.points[index].z - xyz_centroid_[2]; + + mu200 += temp_pt_[0] * temp_pt_[0]; + mu020 += temp_pt_[1] * temp_pt_[1]; + mu002 += temp_pt_[2] * temp_pt_[2]; + mu110 += temp_pt_[0] * temp_pt_[1]; + mu101 += temp_pt_[0] * temp_pt_[2]; + mu011 += temp_pt_[1] * temp_pt_[2]; + } + + // Save the moment invariants + j1 = mu200 + mu020 + mu002; + j2 = mu200*mu020 + mu200*mu002 + mu020*mu002 - mu110*mu110 - mu101*mu101 - mu011*mu011; + j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentInvariantsEstimation::computePointMomentInvariants ( + const pcl::PointCloud &cloud, float &j1, float &j2, float &j3) +{ + // Estimate the XYZ centroid + compute3DCentroid (cloud, xyz_centroid_); + + // Initialize the centralized moments + float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011 = 0; + + // Iterate over the nearest neighbors set + for (const auto& point: cloud.points) + { + // Demean the points + temp_pt_[0] = point.x - xyz_centroid_[0]; + temp_pt_[1] = point.y - xyz_centroid_[1]; + temp_pt_[2] = point.z - xyz_centroid_[2]; + + mu200 += temp_pt_[0] * temp_pt_[0]; + mu020 += temp_pt_[1] * temp_pt_[1]; + mu002 += temp_pt_[2] * temp_pt_[2]; + mu110 += temp_pt_[0] * temp_pt_[1]; + mu101 += temp_pt_[0] * temp_pt_[2]; + mu011 += temp_pt_[1] * temp_pt_[2]; + } + + // Save the moment invariants + j1 = mu200 + mu020 + mu002; + j2 = mu200*mu020 + mu200*mu002 + mu020*mu002 - mu110*mu110 - mu101*mu101 - mu011*mu011; + j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentInvariantsEstimation::computeFeature (PointCloudOut &output) +{ + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + output.is_dense = true; + // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense + if (input_->is_dense) + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + output.points[idx].j1 = output.points[idx].j2 = output.points[idx].j3 = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + computePointMomentInvariants (*surface_, nn_indices, + output.points[idx].j1, output.points[idx].j2, output.points[idx].j3); + } + } + else + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + output.points[idx].j1 = output.points[idx].j2 = output.points[idx].j3 = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + computePointMomentInvariants (*surface_, nn_indices, + output.points[idx].j1, output.points[idx].j2, output.points[idx].j3); + } + } +} + +#define PCL_INSTANTIATE_MomentInvariantsEstimation(T,NT) template class PCL_EXPORTS pcl::MomentInvariantsEstimation; + +#endif // PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/moment_of_inertia_estimation.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/moment_of_inertia_estimation.hpp new file mode 100755 index 0000000..998e343 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/moment_of_inertia_estimation.hpp @@ -0,0 +1,646 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#ifndef PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ +#define PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MomentOfInertiaEstimation::MomentOfInertiaEstimation () : + is_valid_ (false), + step_ (10.0f), + point_mass_ (0.0001f), + normalize_ (true), + mean_value_ (0.0f, 0.0f, 0.0f), + major_axis_ (0.0f, 0.0f, 0.0f), + middle_axis_ (0.0f, 0.0f, 0.0f), + minor_axis_ (0.0f, 0.0f, 0.0f), + major_value_ (0.0f), + middle_value_ (0.0f), + minor_value_ (0.0f), + aabb_min_point_ (), + aabb_max_point_ (), + obb_min_point_ (), + obb_max_point_ (), + obb_position_ (0.0f, 0.0f, 0.0f) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MomentOfInertiaEstimation::~MomentOfInertiaEstimation () +{ + moment_of_inertia_.clear (); + eccentricity_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setAngleStep (const float step) +{ + if (step <= 0.0f) + return; + + step_ = step; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::getAngleStep () const +{ + return (step_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setNormalizePointMassFlag (bool need_to_normalize) +{ + normalize_ = need_to_normalize; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getNormalizePointMassFlag () const +{ + return (normalize_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setPointMass (const float point_mass) +{ + if (point_mass <= 0.0f) + return; + + point_mass_ = point_mass; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::getPointMass () const +{ + return (point_mass_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::compute () +{ + moment_of_inertia_.clear (); + eccentricity_.clear (); + + if (!initCompute ()) + { + deinitCompute (); + return; + } + + if (normalize_) + { + if (!indices_->empty ()) + point_mass_ = 1.0f / static_cast (indices_->size () * indices_->size ()); + else + point_mass_ = 1.0f; + } + + computeMeanValue (); + + Eigen::Matrix covariance_matrix; + covariance_matrix.setZero (); + computeCovarianceMatrix (covariance_matrix); + + computeEigenVectors (covariance_matrix, major_axis_, middle_axis_, minor_axis_, major_value_, middle_value_, minor_value_); + + float theta = 0.0f; + while (theta <= 90.0f) + { + float phi = 0.0f; + Eigen::Vector3f rotated_vector; + rotateVector (major_axis_, middle_axis_, theta, rotated_vector); + while (phi <= 360.0f) + { + Eigen::Vector3f current_axis; + rotateVector (rotated_vector, minor_axis_, phi, current_axis); + current_axis.normalize (); + + //compute moment of inertia for the current axis + float current_moment_of_inertia = calculateMomentOfInertia (current_axis, mean_value_); + moment_of_inertia_.push_back (current_moment_of_inertia); + + //compute eccentricity for the current plane + typename pcl::PointCloud::Ptr projected_cloud (new pcl::PointCloud ()); + getProjectedCloud (current_axis, mean_value_, projected_cloud); + Eigen::Matrix covariance_matrix; + covariance_matrix.setZero (); + computeCovarianceMatrix (projected_cloud, covariance_matrix); + projected_cloud.reset (); + float current_eccentricity = computeEccentricity (covariance_matrix, current_axis); + eccentricity_.push_back (current_eccentricity); + + phi += step_; + } + theta += step_; + } + + computeOBB (); + + is_valid_ = true; + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getAABB (PointT& min_point, PointT& max_point) const +{ + min_point = aabb_min_point_; + max_point = aabb_max_point_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const +{ + min_point = obb_min_point_; + max_point = obb_max_point_; + position.x = obb_position_ (0); + position.y = obb_position_ (1); + position.z = obb_position_ (2); + rotational_matrix = obb_rotational_matrix_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeOBB () +{ + obb_min_point_.x = std::numeric_limits ::max (); + obb_min_point_.y = std::numeric_limits ::max (); + obb_min_point_.z = std::numeric_limits ::max (); + + obb_max_point_.x = std::numeric_limits ::min (); + obb_max_point_.y = std::numeric_limits ::min (); + obb_max_point_.z = std::numeric_limits ::min (); + + unsigned int number_of_points = static_cast (indices_->size ()); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + float x = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * major_axis_ (0) + + (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * major_axis_ (1) + + (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * major_axis_ (2); + float y = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * middle_axis_ (0) + + (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * middle_axis_ (1) + + (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * middle_axis_ (2); + float z = (input_->points[(*indices_)[i_point]].x - mean_value_ (0)) * minor_axis_ (0) + + (input_->points[(*indices_)[i_point]].y - mean_value_ (1)) * minor_axis_ (1) + + (input_->points[(*indices_)[i_point]].z - mean_value_ (2)) * minor_axis_ (2); + + if (x <= obb_min_point_.x) obb_min_point_.x = x; + if (y <= obb_min_point_.y) obb_min_point_.y = y; + if (z <= obb_min_point_.z) obb_min_point_.z = z; + + if (x >= obb_max_point_.x) obb_max_point_.x = x; + if (y >= obb_max_point_.y) obb_max_point_.y = y; + if (z >= obb_max_point_.z) obb_max_point_.z = z; + } + + obb_rotational_matrix_ << major_axis_ (0), middle_axis_ (0), minor_axis_ (0), + major_axis_ (1), middle_axis_ (1), minor_axis_ (1), + major_axis_ (2), middle_axis_ (2), minor_axis_ (2); + + Eigen::Vector3f shift ( + (obb_max_point_.x + obb_min_point_.x) / 2.0f, + (obb_max_point_.y + obb_min_point_.y) / 2.0f, + (obb_max_point_.z + obb_min_point_.z) / 2.0f); + + obb_min_point_.x -= shift (0); + obb_min_point_.y -= shift (1); + obb_min_point_.z -= shift (2); + + obb_max_point_.x -= shift (0); + obb_max_point_.y -= shift (1); + obb_max_point_.z -= shift (2); + + obb_position_ = mean_value_ + obb_rotational_matrix_ * shift; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getEigenValues (float& major, float& middle, float& minor) const +{ + major = major_value_; + middle = middle_value_; + minor = minor_value_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const +{ + major = major_axis_; + middle = middle_axis_; + minor = minor_axis_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getMomentOfInertia (std::vector & moment_of_inertia) const +{ + moment_of_inertia.resize (moment_of_inertia_.size (), 0.0f); + std::copy (moment_of_inertia_.begin (), moment_of_inertia_.end (), moment_of_inertia.begin ()); + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getEccentricity (std::vector & eccentricity) const +{ + eccentricity.resize (eccentricity_.size (), 0.0f); + std::copy (eccentricity_.begin (), eccentricity_.end (), eccentricity.begin ()); + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeMeanValue () +{ + mean_value_ (0) = 0.0f; + mean_value_ (1) = 0.0f; + mean_value_ (2) = 0.0f; + + aabb_min_point_.x = std::numeric_limits ::max (); + aabb_min_point_.y = std::numeric_limits ::max (); + aabb_min_point_.z = std::numeric_limits ::max (); + + aabb_max_point_.x = -std::numeric_limits ::max (); + aabb_max_point_.y = -std::numeric_limits ::max (); + aabb_max_point_.z = -std::numeric_limits ::max (); + + unsigned int number_of_points = static_cast (indices_->size ()); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + mean_value_ (0) += input_->points[(*indices_)[i_point]].x; + mean_value_ (1) += input_->points[(*indices_)[i_point]].y; + mean_value_ (2) += input_->points[(*indices_)[i_point]].z; + + if (input_->points[(*indices_)[i_point]].x <= aabb_min_point_.x) aabb_min_point_.x = input_->points[(*indices_)[i_point]].x; + if (input_->points[(*indices_)[i_point]].y <= aabb_min_point_.y) aabb_min_point_.y = input_->points[(*indices_)[i_point]].y; + if (input_->points[(*indices_)[i_point]].z <= aabb_min_point_.z) aabb_min_point_.z = input_->points[(*indices_)[i_point]].z; + + if (input_->points[(*indices_)[i_point]].x >= aabb_max_point_.x) aabb_max_point_.x = input_->points[(*indices_)[i_point]].x; + if (input_->points[(*indices_)[i_point]].y >= aabb_max_point_.y) aabb_max_point_.y = input_->points[(*indices_)[i_point]].y; + if (input_->points[(*indices_)[i_point]].z >= aabb_max_point_.z) aabb_max_point_.z = input_->points[(*indices_)[i_point]].z; + } + + if (number_of_points == 0) + number_of_points = 1; + + mean_value_ (0) /= number_of_points; + mean_value_ (1) /= number_of_points; + mean_value_ (2) /= number_of_points; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeCovarianceMatrix (Eigen::Matrix & covariance_matrix) const +{ + covariance_matrix.setZero (); + + unsigned int number_of_points = static_cast (indices_->size ()); + float factor = 1.0f / static_cast ((number_of_points - 1 > 0)?(number_of_points - 1):1); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::Vector3f current_point (0.0f, 0.0f, 0.0f); + current_point (0) = input_->points[(*indices_)[i_point]].x - mean_value_ (0); + current_point (1) = input_->points[(*indices_)[i_point]].y - mean_value_ (1); + current_point (2) = input_->points[(*indices_)[i_point]].z - mean_value_ (2); + + covariance_matrix += current_point * current_point.transpose (); + } + + covariance_matrix *= factor; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix & covariance_matrix) const +{ + covariance_matrix.setZero (); + + unsigned int number_of_points = static_cast (cloud->points.size ()); + float factor = 1.0f / static_cast ((number_of_points - 1 > 0)?(number_of_points - 1):1); + Eigen::Vector3f current_point; + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + current_point (0) = cloud->points[i_point].x - mean_value_ (0); + current_point (1) = cloud->points[i_point].y - mean_value_ (1); + current_point (2) = cloud->points[i_point].z - mean_value_ (2); + + covariance_matrix += current_point * current_point.transpose (); + } + + covariance_matrix *= factor; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::computeEigenVectors (const Eigen::Matrix & covariance_matrix, + Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value, + float& middle_value, float& minor_value) +{ + Eigen::EigenSolver > eigen_solver; + eigen_solver.compute (covariance_matrix); + + Eigen::EigenSolver >::EigenvectorsType eigen_vectors; + Eigen::EigenSolver >::EigenvalueType eigen_values; + eigen_vectors = eigen_solver.eigenvectors (); + eigen_values = eigen_solver.eigenvalues (); + + unsigned int temp = 0; + unsigned int major_index = 0; + unsigned int middle_index = 1; + unsigned int minor_index = 2; + + if (eigen_values.real () (major_index) < eigen_values.real () (middle_index)) + { + temp = major_index; + major_index = middle_index; + middle_index = temp; + } + + if (eigen_values.real () (major_index) < eigen_values.real () (minor_index)) + { + temp = major_index; + major_index = minor_index; + minor_index = temp; + } + + if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index)) + { + temp = minor_index; + minor_index = middle_index; + middle_index = temp; + } + + major_value = eigen_values.real () (major_index); + middle_value = eigen_values.real () (middle_index); + minor_value = eigen_values.real () (minor_index); + + major_axis = eigen_vectors.col (major_index).real (); + middle_axis = eigen_vectors.col (middle_index).real (); + minor_axis = eigen_vectors.col (minor_index).real (); + + major_axis.normalize (); + middle_axis.normalize (); + minor_axis.normalize (); + + float det = major_axis.dot (middle_axis.cross (minor_axis)); + if (det <= 0.0f) + { + major_axis (0) = -major_axis (0); + major_axis (1) = -major_axis (1); + major_axis (2) = -major_axis (2); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const +{ + Eigen::Matrix rotation_matrix; + const float x = axis (0); + const float y = axis (1); + const float z = axis (2); + const float rad = M_PI / 180.0f; + const float cosine = std::cos (angle * rad); + const float sine = std::sin (angle * rad); + rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y, + (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x, + (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z; + + rotated_vector = rotation_matrix * vector; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const +{ + float moment_of_inertia = 0.0f; + unsigned int number_of_points = static_cast (indices_->size ()); + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::Vector3f vector; + vector (0) = mean_value (0) - input_->points[(*indices_)[i_point]].x; + vector (1) = mean_value (1) - input_->points[(*indices_)[i_point]].y; + vector (2) = mean_value (2) - input_->points[(*indices_)[i_point]].z; + + Eigen::Vector3f product = vector.cross (current_axis); + + float distance = product (0) * product (0) + product (1) * product (1) + product (2) * product (2); + + moment_of_inertia += distance; + } + + return (point_mass_ * moment_of_inertia); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud ::Ptr projected_cloud) const +{ + const float D = - normal_vector.dot (point); + + unsigned int number_of_points = static_cast (indices_->size ()); + projected_cloud->points.resize (number_of_points, PointT ()); + + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + const unsigned int index = (*indices_)[i_point]; + float K = - (D + normal_vector (0) * input_->points[index].x + normal_vector (1) * input_->points[index].y + normal_vector (2) * input_->points[index].z); + PointT projected_point; + projected_point.x = input_->points[index].x + K * normal_vector (0); + projected_point.y = input_->points[index].y + K * normal_vector (1); + projected_point.z = input_->points[index].z + K * normal_vector (2); + projected_cloud->points[i_point] = projected_point; + } + projected_cloud->width = number_of_points; + projected_cloud->height = 1; + projected_cloud->header = input_->header; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MomentOfInertiaEstimation::computeEccentricity (const Eigen::Matrix & covariance_matrix, const Eigen::Vector3f& normal_vector) +{ + Eigen::Vector3f major_axis (0.0f, 0.0f, 0.0f); + Eigen::Vector3f middle_axis (0.0f, 0.0f, 0.0f); + Eigen::Vector3f minor_axis (0.0f, 0.0f, 0.0f); + float major_value = 0.0f; + float middle_value = 0.0f; + float minor_value = 0.0f; + computeEigenVectors (covariance_matrix, major_axis, middle_axis, minor_axis, major_value, middle_value, minor_value); + + float major = std::abs (major_axis.dot (normal_vector)); + float middle = std::abs (middle_axis.dot (normal_vector)); + float minor = std::abs (minor_axis.dot (normal_vector)); + + float eccentricity = 0.0f; + + if (major >= middle && major >= minor && middle_value != 0.0f) + eccentricity = std::pow (1.0f - (minor_value * minor_value) / (middle_value * middle_value), 0.5f); + + if (middle >= major && middle >= minor && major_value != 0.0f) + eccentricity = std::pow (1.0f - (minor_value * minor_value) / (major_value * major_value), 0.5f); + + if (minor >= major && minor >= middle && major_value != 0.0f) + eccentricity = std::pow (1.0f - (middle_value * middle_value) / (major_value * major_value), 0.5f); + + return (eccentricity); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MomentOfInertiaEstimation::getMassCenter (Eigen::Vector3f& mass_center) const +{ + mass_center = mean_value_; + + return (is_valid_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setInputCloud (const PointCloudConstPtr& cloud) +{ + input_ = cloud; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (const IndicesPtr& indices) +{ + indices_ = indices; + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (const IndicesConstPtr& indices) +{ + indices_.reset (new std::vector (*indices)); + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (const PointIndicesConstPtr& indices) +{ + indices_.reset (new std::vector (indices->indices)); + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MomentOfInertiaEstimation::setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols) +{ + if ((nb_rows > input_->height) || (row_start > input_->height)) + { + PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height", input_->height); + return; + } + + if ((nb_cols > input_->width) || (col_start > input_->width)) + { + PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width", input_->width); + return; + } + + const std::size_t row_end = row_start + nb_rows; + if (row_end > input_->height) + { + PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height); + return; + } + + const std::size_t col_end = col_start + nb_cols; + if (col_end > input_->width) + { + PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width); + return; + } + + indices_.reset (new std::vector); + indices_->reserve (nb_cols * nb_rows); + for(std::size_t i = row_start; i < row_end; i++) + for(std::size_t j = col_start; j < col_end; j++) + indices_->push_back (static_cast ((i * input_->width) + j)); + fake_indices_ = false; + use_indices_ = true; + + is_valid_ = false; +} + +#endif // PCL_MOMENT_OF_INERTIA_ESTIMATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/multiscale_feature_persistence.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/multiscale_feature_persistence.hpp new file mode 100755 index 0000000..5e87871 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/multiscale_feature_persistence.hpp @@ -0,0 +1,262 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ +#define PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MultiscaleFeaturePersistence::MultiscaleFeaturePersistence () : + alpha_ (0), + distance_metric_ (L1), + feature_estimator_ (), + features_at_scale_ (), + feature_representation_ () +{ + feature_representation_.reset (new DefaultPointRepresentation); + // No input is needed, hack around the initCompute () check from PCLBase + input_.reset (new pcl::PointCloud ()); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MultiscaleFeaturePersistence::initCompute () +{ + if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n"); + return false; + } + if (!feature_estimator_) + { + PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No feature estimator was set\n"); + return false; + } + if (scale_values_.empty ()) + { + PCL_ERROR ("[pcl::MultiscaleFeaturePersistence::initCompute] No scale values were given\n"); + return false; + } + + mean_feature_.resize (feature_representation_->getNumberOfDimensions ()); + + return true; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MultiscaleFeaturePersistence::computeFeaturesAtAllScales () +{ + features_at_scale_.clear (); + features_at_scale_.reserve (scale_values_.size ()); + features_at_scale_vectorized_.clear (); + features_at_scale_vectorized_.reserve (scale_values_.size ()); + for (std::size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i) + { + FeatureCloudPtr feature_cloud (new FeatureCloud ()); + computeFeatureAtScale (scale_values_[scale_i], feature_cloud); + features_at_scale_[scale_i] = feature_cloud; + + // Vectorize each feature and insert it into the vectorized feature storage + std::vector > feature_cloud_vectorized; + feature_cloud_vectorized.reserve (feature_cloud->points.size ()); + + for (const auto& feature: feature_cloud->points) + { + std::vector feature_vectorized (feature_representation_->getNumberOfDimensions ()); + feature_representation_->vectorize (feature, feature_vectorized); + feature_cloud_vectorized.emplace_back (std::move(feature_vectorized)); + } + features_at_scale_vectorized_.emplace_back (std::move(feature_cloud_vectorized)); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MultiscaleFeaturePersistence::computeFeatureAtScale (float &scale, + FeatureCloudPtr &features) +{ + feature_estimator_->setRadiusSearch (scale); + feature_estimator_->compute (*features); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MultiscaleFeaturePersistence::distanceBetweenFeatures (const std::vector &a, + const std::vector &b) +{ + return (pcl::selectNorm > (a, b, a.size (), distance_metric_)); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MultiscaleFeaturePersistence::calculateMeanFeature () +{ + // Reset mean feature + std::fill_n(mean_feature_.begin (), mean_feature_.size (), 0.f); + + std::size_t normalization_factor = 0; + for (const auto& scale: features_at_scale_vectorized_) + { + normalization_factor += scale.size (); // not using accumulate for cache efficiency + for (const auto &feature : scale) + std::transform(mean_feature_.cbegin (), mean_feature_.cend (), + feature.cbegin (), mean_feature_.begin (), std::plus<>{}); + } + + const float factor = std::min(1, normalization_factor); + std::transform(mean_feature_.cbegin(), + mean_feature_.cend(), + mean_feature_.begin(), + [factor](const auto& mean) { + return mean / factor; + }); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MultiscaleFeaturePersistence::extractUniqueFeatures () +{ + unique_features_indices_.clear (); + unique_features_table_.clear (); + unique_features_indices_.reserve (scale_values_.size ()); + unique_features_table_.reserve (scale_values_.size ()); + + for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size (); ++scale_i) + { + // Calculate standard deviation within the scale + float standard_dev = 0.0; + std::vector diff_vector (features_at_scale_vectorized_[scale_i].size ()); + diff_vector.clear(); + + for (const auto& feature: features_at_scale_vectorized_[scale_i]) + { + float diff = distanceBetweenFeatures (feature, mean_feature_); + standard_dev += diff * diff; + diff_vector.emplace_back (diff); + } + standard_dev = std::sqrt (standard_dev / static_cast (features_at_scale_vectorized_[scale_i].size ())); + PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::extractUniqueFeatures] Standard deviation for scale %f is %f\n", scale_values_[scale_i], standard_dev); + + // Select only points outside (mean +/- alpha * standard_dev) + std::list indices_per_scale; + std::vector indices_table_per_scale (features_at_scale_[scale_i]->points.size (), false); + for (std::size_t point_i = 0; point_i < features_at_scale_[scale_i]->points.size (); ++point_i) + { + if (diff_vector[point_i] > alpha_ * standard_dev) + { + indices_per_scale.emplace_back (point_i); + indices_table_per_scale[point_i] = true; + } + } + unique_features_indices_.emplace_back (std::move(indices_per_scale)); + unique_features_table_.emplace_back (std::move(indices_table_per_scale)); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MultiscaleFeaturePersistence::determinePersistentFeatures (FeatureCloud &output_features, + shared_ptr > &output_indices) +{ + if (!initCompute ()) + return; + + // Compute the features for all scales with the given feature estimator + PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Computing features ...\n"); + computeFeaturesAtAllScales (); + + // Compute mean feature + PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Calculating mean feature ...\n"); + calculateMeanFeature (); + + // Get the 'unique' features at each scale + PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Extracting unique features ...\n"); + extractUniqueFeatures (); + + PCL_DEBUG ("[pcl::MultiscaleFeaturePersistence::determinePersistentFeatures] Determining persistent features between scales ...\n"); + // Determine persistent features between scales + +/* + // Method 1: a feature is considered persistent if it is 'unique' in at least 2 different scales + for (std::size_t scale_i = 0; scale_i < features_at_scale_vectorized_.size () - 1; ++scale_i) + for (std::list::iterator feature_it = unique_features_indices_[scale_i].begin (); feature_it != unique_features_indices_[scale_i].end (); ++feature_it) + { + if (unique_features_table_[scale_i][*feature_it] == true) + { + output_features.points.push_back (features_at_scale[scale_i]->points[*feature_it]); + output_indices->push_back (feature_estimator_->getIndices ()->at (*feature_it)); + } + } +*/ + // Method 2: a feature is considered persistent if it is 'unique' in all the scales + for (const auto& feature: unique_features_indices_.front ()) + { + bool present_in_all = true; + for (std::size_t scale_i = 0; scale_i < features_at_scale_.size (); ++scale_i) + present_in_all = present_in_all && unique_features_table_[scale_i][feature]; + + if (present_in_all) + { + output_features.points.emplace_back (features_at_scale_.front ()->points[feature]); + output_indices->emplace_back (feature_estimator_->getIndices ()->at (feature)); + } + } + + // Consider that output cloud is unorganized + output_features.header = feature_estimator_->getInputCloud ()->header; + output_features.is_dense = feature_estimator_->getInputCloud ()->is_dense; + output_features.width = static_cast (output_features.points.size ()); + output_features.height = 1; +} + + +#define PCL_INSTANTIATE_MultiscaleFeaturePersistence(InT, Feature) template class PCL_EXPORTS pcl::MultiscaleFeaturePersistence; + +#endif /* PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/narf.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/narf.hpp new file mode 100755 index 0000000..a68a744 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/narf.hpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include + +namespace pcl { + +inline float +Narf::getDescriptorDistance(const Narf& other) const +{ + float ret = L1_Norm(descriptor_, other.descriptor_, descriptor_size_); + //float ret = Sublinear_Norm(descriptor_, other.descriptor_, descriptor_size_); + ret /= static_cast (descriptor_size_); + return (ret); +} + +inline void Narf::copyToNarf36(Narf36& narf36) const +{ + if (descriptor_size_ != 36) + { + std::cerr << __PRETTY_FUNCTION__ << ": descriptor size is not 36!\n"; + return; + } + getTranslationAndEulerAngles(transformation_.inverse (), narf36.x, narf36.y, narf36.z, narf36.roll, narf36.pitch, narf36.yaw); + memcpy(narf36.descriptor, descriptor_, 36*sizeof(*descriptor_)); +} + +//inline float Narf::getDescriptorDistance(const Narf& other) const +//{ + //float middle_value = 0.1f; + //float normalization_factor1 = 1.0f/middle_value, + //normalization_factor2 = 1.0f/(1.0f-middle_value); + //const float* descriptor1_ptr = descriptor_; + //const float* descriptor2_ptr = other.getDescriptor(); + //float ret = 0; + //for (int i=0; i + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalEstimation::computeFeature (PointCloudOut &output) +{ + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + output.is_dense = true; + // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense + if (input_->is_dense) + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || + !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) + { + output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, + output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); + + } + } + else + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || + !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) + { + output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, + output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); + + } + } +} + +#define PCL_INSTANTIATE_NormalEstimation(T,NT) template class PCL_EXPORTS pcl::NormalEstimation; + +#endif // PCL_FEATURES_IMPL_NORMAL_3D_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/normal_3d_omp.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/normal_3d_omp.hpp new file mode 100755 index 0000000..a849a38 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/normal_3d_omp.hpp @@ -0,0 +1,131 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_NORMAL_3D_OMP_H_ +#define PCL_FEATURES_IMPL_NORMAL_3D_OMP_H_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalEstimationOMP::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalEstimationOMP::computeFeature (PointCloudOut &output) +{ + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + output.is_dense = true; + // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense + if (input_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_) +#endif + // Iterating over the entire index vector + for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) + { + Eigen::Vector4f n; + if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || + !pcl::computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature)) + { + output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + output.points[idx].normal_x = n[0]; + output.points[idx].normal_y = n[1]; + output.points[idx].normal_z = n[2]; + + flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, + output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); + + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_) +#endif + // Iterating over the entire index vector + for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) + { + Eigen::Vector4f n; + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || + !pcl::computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature)) + { + output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + output.points[idx].normal_x = n[0]; + output.points[idx].normal_y = n[1]; + output.points[idx].normal_z = n[2]; + + flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, + output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); + + } + } +} + +#define PCL_INSTANTIATE_NormalEstimationOMP(T,NT) template class PCL_EXPORTS pcl::NormalEstimationOMP; + +#endif // PCL_FEATURES_IMPL_NORMAL_3D_OMP_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/normal_based_signature.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/normal_based_signature.hpp new file mode 100755 index 0000000..b1d8229 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/normal_based_signature.hpp @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ +#define PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ + +#include + +template void +pcl::NormalBasedSignatureEstimation::computeFeature (FeatureCloud &output) +{ + // do a few checks before starting the computations + + PointFeature test_feature; + (void)test_feature; + if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float)) + { + PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float)); + return; + } + + std::vector k_indices; + std::vector k_sqr_distances; + + tree_->setInputCloud (input_); + output.points.resize (indices_->size ()); + + for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i) + { + std::size_t point_i = (*indices_)[index_i]; + Eigen::MatrixXf s_matrix (N_, M_); + + Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap (); + + for (std::size_t k = 0; k < N_; ++k) + { + Eigen::VectorXf s_row (M_); + + for (std::size_t l = 0; l < M_; ++l) + { + Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap (); + Eigen::Vector4f normal_u = Eigen::Vector4f::Zero (); + Eigen::Vector4f normal_v = Eigen::Vector4f::Zero (); + + if (std::abs (normal.x ()) > 0.0001f) + { + normal_u.x () = - normal.y () / normal.x (); + normal_u.y () = 1.0f; + normal_u.z () = 0.0f; + normal_u.normalize (); + + } + else if (std::abs (normal.y ()) > 0.0001f) + { + normal_u.x () = 1.0f; + normal_u.y () = - normal.x () / normal.y (); + normal_u.z () = 0.0f; + normal_u.normalize (); + } + else + { + normal_u.x () = 0.0f; + normal_u.y () = 1.0f; + normal_u.z () = - normal.y () / normal.z (); + } + normal_v = normal.cross3 (normal_u); + + Eigen::Vector4f zeta_point = 2.0f * static_cast (l + 1) * scale_h_ / static_cast (M_) * + (std::cos (2.0f * static_cast (M_PI) * static_cast ((k + 1) / N_)) * normal_u + + sinf (2.0f * static_cast (M_PI) * static_cast ((k + 1) / N_)) * normal_v); + + // Compute normal by using the neighbors + Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point; + PointT zeta_point_pcl; + zeta_point_pcl.x = zeta_point_plus_center.x (); zeta_point_pcl.y = zeta_point_plus_center.y (); zeta_point_pcl.z = zeta_point_plus_center.z (); + + tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances); + + // Do k nearest search if there are no neighbors nearby + if (k_indices.empty ()) + { + k_indices.resize (5); + k_sqr_distances.resize (5); + tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances); + } + + Eigen::Vector4f average_normal = Eigen::Vector4f::Zero (); + + float average_normalization_factor = 0.0f; + + // Normals weighted by 1/squared_distances + for (std::size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i) + { + if (k_sqr_distances[nn_i] < 1e-7f) + { + average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap (); + average_normalization_factor = 1.0f; + break; + } + average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i]; + average_normalization_factor += 1.0f / k_sqr_distances[nn_i]; + } + average_normal /= average_normalization_factor; + float s = zeta_point.dot (average_normal) / zeta_point.norm (); + s_row[l] = s; + } + + // do DCT on the s_matrix row-wise + Eigen::VectorXf dct_row (M_); + for (Eigen::Index m = 0; m < s_row.size (); ++m) + { + float Xk = 0.0f; + for (Eigen::Index n = 0; n < s_row.size (); ++n) + Xk += static_cast (s_row[n] * std::cos (M_PI / (static_cast (M_ * n) + 0.5) * static_cast (k))); + dct_row[m] = Xk; + } + s_row = dct_row; + s_matrix.row (k).matrix () = dct_row; + } + + // do DFT on the s_matrix column-wise + Eigen::MatrixXf dft_matrix (N_, M_); + for (std::size_t column_i = 0; column_i < M_; ++column_i) + { + Eigen::VectorXf dft_col (N_); + for (std::size_t k = 0; k < N_; ++k) + { + float Xk_real = 0.0f, Xk_imag = 0.0f; + for (std::size_t n = 0; n < N_; ++n) + { + Xk_real += static_cast (s_matrix (n, column_i) * std::cos (2.0f * M_PI / static_cast (N_ * k * n))); + Xk_imag += static_cast (s_matrix (n, column_i) * sin (2.0f * M_PI / static_cast (N_ * k * n))); + } + dft_col[k] = std::sqrt (Xk_real*Xk_real + Xk_imag*Xk_imag); + } + dft_matrix.col (column_i).matrix () = dft_col; + } + + Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_); + + PointFeature feature_point; + for (std::size_t i = 0; i < N_prime_; ++i) + for (std::size_t j = 0; j < M_prime_; ++j) + feature_point.values[i*M_prime_ + j] = final_matrix (i, j); + + output.points[index_i] = feature_point; + } +} + + + +#define PCL_INSTANTIATE_NormalBasedSignatureEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::NormalBasedSignatureEstimation; + + +#endif /* PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/organized_edge_detection.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/organized_edge_detection.hpp new file mode 100755 index 0000000..04d645a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/organized_edge_detection.hpp @@ -0,0 +1,353 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_ +#define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_ + +#include +#include +#include + +/** + * Directions: 1 2 3 + * 0 x 4 + * 7 6 5 + * e.g. direction y means we came from pixel with label y to the center pixel x + */ +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeBase::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + extractEdges (labels); + + assignLabelIndices (labels, label_indices); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeBase::assignLabelIndices (pcl::PointCloud& labels, std::vector& label_indices) const +{ + const unsigned invalid_label = unsigned (0); + label_indices.resize (num_of_edgetype_); + for (std::size_t idx = 0; idx < input_->points.size (); idx++) + { + if (labels[idx].label != invalid_label) + { + for (int edge_type = 0; edge_type < num_of_edgetype_; edge_type++) + { + if ((labels[idx].label >> edge_type) & 1) + label_indices[edge_type].indices.push_back (idx); + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeBase::extractEdges (pcl::PointCloud& labels) const +{ + if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED)) + { + // Fill lookup table for next points to visit + const int num_of_ngbr = 8; + Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1), + Neighbor(-1, -1, -labels.width - 1), + Neighbor( 0, -1, -labels.width ), + Neighbor( 1, -1, -labels.width + 1), + Neighbor( 1, 0, 1), + Neighbor( 1, 1, labels.width + 1), + Neighbor( 0, 1, labels.width ), + Neighbor(-1, 1, labels.width - 1)}; + + for (int row = 1; row < int(input_->height) - 1; row++) + { + for (int col = 1; col < int(input_->width) - 1; col++) + { + int curr_idx = row*int(input_->width) + col; + if (!std::isfinite (input_->points[curr_idx].z)) + continue; + + float curr_depth = std::abs (input_->points[curr_idx].z); + + // Calculate depth distances between current point and neighboring points + std::vector nghr_dist; + nghr_dist.resize (8); + bool found_invalid_neighbor = false; + for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++) + { + int nghr_idx = curr_idx + directions[d_idx].d_index; + assert (nghr_idx >= 0 && nghr_idx < input_->points.size ()); + if (!std::isfinite (input_->points[nghr_idx].z)) + { + found_invalid_neighbor = true; + break; + } + nghr_dist[d_idx] = curr_depth - std::abs (input_->points[nghr_idx].z); + } + + if (!found_invalid_neighbor) + { + // Every neighboring points are valid + std::vector::const_iterator min_itr, max_itr; + std::tie (min_itr, max_itr) = std::minmax_element (nghr_dist.cbegin (), nghr_dist.cend ()); + float nghr_dist_min = *min_itr; + float nghr_dist_max = *max_itr; + float dist_dominant = std::max(std::abs (nghr_dist_min),std::abs (nghr_dist_max)); + if (std::abs (dist_dominant) > th_depth_discon_*std::abs (curr_depth)) + { + // Found a depth discontinuity + if (dist_dominant > 0.f) + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDED) + labels[curr_idx].label |= EDGELABEL_OCCLUDED; + } + else + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDING) + labels[curr_idx].label |= EDGELABEL_OCCLUDING; + } + } + } + else + { + // Some neighboring points are not valid (nan points) + // Search for corresponding point across invalid points + // Search direction is determined by nan point locations with respect to current point + int dx = 0; + int dy = 0; + int num_of_invalid_pt = 0; + for (const auto &direction : directions) + { + int nghr_idx = curr_idx + direction.d_index; + assert (nghr_idx >= 0 && nghr_idx < input_->points.size ()); + if (!std::isfinite (input_->points[nghr_idx].z)) + { + dx += direction.d_x; + dy += direction.d_y; + num_of_invalid_pt++; + } + } + + // Search directions + assert (num_of_invalid_pt > 0); + float f_dx = static_cast (dx) / static_cast (num_of_invalid_pt); + float f_dy = static_cast (dy) / static_cast (num_of_invalid_pt); + + // Search for corresponding point across invalid points + float corr_depth = std::numeric_limits::quiet_NaN (); + for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++) + { + int s_row = row + static_cast (std::floor (f_dy*static_cast (s_idx))); + int s_col = col + static_cast (std::floor (f_dx*static_cast (s_idx))); + + if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width)) + break; + + if (std::isfinite (input_->points[s_row*int(input_->width)+s_col].z)) + { + corr_depth = std::abs (input_->points[s_row*int(input_->width)+s_col].z); + break; + } + } + + if (!std::isnan (corr_depth)) + { + // Found a corresponding point + float dist = curr_depth - corr_depth; + if (std::abs (dist) > th_depth_discon_*std::abs (curr_depth)) + { + // Found a depth discontinuity + if (dist > 0.f) + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDED) + labels[curr_idx].label |= EDGELABEL_OCCLUDED; + } + else + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDING) + labels[curr_idx].label |= EDGELABEL_OCCLUDING; + } + } + } + else + { + // Not found a corresponding point, just nan boundary edge + if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) + labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY; + } + } + } + } + } +} + + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeFromRGB::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + OrganizedEdgeBase::extractEdges (labels); + extractEdges (labels); + + this->assignLabelIndices (labels, label_indices); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeFromRGB::extractEdges (pcl::PointCloud& labels) const +{ + if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY)) + { + pcl::PointCloud::Ptr gray (new pcl::PointCloud); + gray->width = input_->width; + gray->height = input_->height; + gray->resize (input_->height*input_->width); + + for (std::size_t i = 0; i < input_->size (); ++i) + (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3); + + pcl::PointCloud img_edge_rgb; + pcl::Edge edge; + edge.setInputCloud (gray); + edge.setHysteresisThresholdLow (th_rgb_canny_low_); + edge.setHysteresisThresholdHigh (th_rgb_canny_high_); + edge.detectEdgeCanny (img_edge_rgb); + + for (std::uint32_t row=0; row void +pcl::OrganizedEdgeFromNormals::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + OrganizedEdgeBase::extractEdges (labels); + extractEdges (labels); + + this->assignLabelIndices (labels, label_indices); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeFromNormals::extractEdges (pcl::PointCloud& labels) const +{ + if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE)) + { + + pcl::PointCloud nx, ny; + nx.width = normals_->width; + nx.height = normals_->height; + nx.resize (normals_->height*normals_->width); + + ny.width = normals_->width; + ny.height = normals_->height; + ny.resize (normals_->height*normals_->width); + + for (std::uint32_t row=0; rowheight; row++) + { + for (std::uint32_t col=0; colwidth; col++) + { + nx (col, row).intensity = normals_->points[row*normals_->width + col].normal_x; + ny (col, row).intensity = normals_->points[row*normals_->width + col].normal_y; + } + } + + pcl::PointCloud img_edge; + pcl::Edge edge; + edge.setHysteresisThresholdLow (th_hc_canny_low_); + edge.setHysteresisThresholdHigh (th_hc_canny_high_); + edge.canny (nx, ny, img_edge); + + for (std::uint32_t row=0; row void +pcl::OrganizedEdgeFromRGBNormals::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + OrganizedEdgeBase::extractEdges (labels); + OrganizedEdgeFromNormals::extractEdges (labels); + OrganizedEdgeFromRGB::extractEdges (labels); + + this->assignLabelIndices (labels, label_indices); +} + +#define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase; +#define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB; +#define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals; +#define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals; + +#endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/our_cvfh.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/our_cvfh.hpp new file mode 100755 index 0000000..e987883 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/our_cvfh.hpp @@ -0,0 +1,744 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: cvfh.hpp 5311 2012-03-26 22:02:04Z aaldoma $ + * + */ + +#ifndef PCL_FEATURES_IMPL_OURCVFH_H_ +#define PCL_FEATURES_IMPL_OURCVFH_H_ + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OURCVFHEstimation::compute (PointCloudOut &output) +{ + if (!Feature::initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + // Resize the output dataset + // Important! We should only allocate precisely how many elements we will need, otherwise + // we risk at pre-allocating too much memory which could lead to bad_alloc + // (see http://dev.pointclouds.org/issues/657) + output.width = output.height = 1; + output.points.resize (1); + + // Perform the actual feature computation + computeFeature (output); + + Feature::deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OURCVFHEstimation::extractEuclideanClustersSmooth (const pcl::PointCloud &cloud, + const pcl::PointCloud &normals, + float tolerance, + const pcl::search::Search::Ptr &tree, + std::vector &clusters, double eps_angle, + unsigned int min_pts_per_cluster, + unsigned int max_pts_per_cluster) +{ + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + if (cloud.points.size () != normals.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); + return; + } + + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + // Process all points in the indices vector + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + if (processed[i]) + continue; + + std::vector seed_queue; + std::size_t sq_idx = 0; + seed_queue.push_back (i); + + processed[i] = true; + + while (sq_idx < seed_queue.size ()) + { + // Search for sq_idx + if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) + { + sq_idx++; + continue; + } + + for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + //processed[nn_indices[j]] = true; + // [-1;1] + + double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0] + + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] + normals.points[seed_queue[sq_idx]].normal[2] + * normals.points[nn_indices[j]].normal[2]; + + if (std::abs (std::acos (dot_p)) < eps_angle) + { + processed[nn_indices[j]] = true; + seed_queue.push_back (nn_indices[j]); + } + } + + sq_idx++; + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + pcl::PointIndices r; + r.indices.resize (seed_queue.size ()); + for (std::size_t j = 0; j < seed_queue.size (); ++j) + r.indices[j] = seed_queue[j]; + + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + r.header = cloud.header; + clusters.push_back (r); // We could avoid a copy by working directly in the vector + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OURCVFHEstimation::filterNormalsWithHighCurvature (const pcl::PointCloud & cloud, + std::vector &indices_to_use, + std::vector &indices_out, std::vector &indices_in, + float threshold) +{ + indices_out.resize (cloud.points.size ()); + indices_in.resize (cloud.points.size ()); + + std::size_t in, out; + in = out = 0; + + for (const int &index : indices_to_use) + { + if (cloud.points[index].curvature > threshold) + { + indices_out[out] = index; + out++; + } + else + { + indices_in[in] = index; + in++; + } + } + + indices_out.resize (out); + indices_in.resize (in); +} + +template bool +pcl::OURCVFHEstimation::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid, + PointInTPtr & processed, std::vector > & transformations, + PointInTPtr & grid, pcl::PointIndices & indices) +{ + + Eigen::Vector3f plane_normal; + plane_normal[0] = -centroid[0]; + plane_normal[1] = -centroid[1]; + plane_normal[2] = -centroid[2]; + Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ (); + plane_normal.normalize (); + Eigen::Vector3f axis = plane_normal.cross (z_vector); + double rotation = -asin (axis.norm ()); + axis.normalize (); + + Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast (rotation), axis)); + + grid->points.resize (processed->points.size ()); + for (std::size_t k = 0; k < processed->points.size (); k++) + grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap (); + + pcl::transformPointCloud (*grid, *grid, transformPC); + + Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0); + Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0); + + centroid4f = transformPC * centroid4f; + normal_centroid4f = transformPC * normal_centroid4f; + + Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]); + + Eigen::Vector4f farthest_away; + pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away); + farthest_away[3] = 0; + + float max_dist = (farthest_away - centroid4f).norm (); + + pcl::demeanPointCloud (*grid, centroid4f, *grid); + + Eigen::Matrix4f center_mat; + center_mat.setIdentity (4, 4); + center_mat (0, 3) = -centroid4f[0]; + center_mat (1, 3) = -centroid4f[1]; + center_mat (2, 3) = -centroid4f[2]; + + Eigen::Matrix3f scatter; + scatter.setZero (); + float sum_w = 0.f; + + for (const int &index : indices.indices) + { + Eigen::Vector3f pvector = grid->points[index].getVector3fMap (); + float d_k = (pvector).norm (); + float w = (max_dist - d_k); + Eigen::Vector3f diff = (pvector); + Eigen::Matrix3f mat = diff * diff.transpose (); + scatter += mat * w; + sum_w += w; + } + + scatter /= sum_w; + + Eigen::JacobiSVD svd (scatter, Eigen::ComputeFullV); + Eigen::Vector3f evx = svd.matrixV ().col (0); + Eigen::Vector3f evy = svd.matrixV ().col (1); + Eigen::Vector3f evz = svd.matrixV ().col (2); + Eigen::Vector3f evxminus = evx * -1; + Eigen::Vector3f evyminus = evy * -1; + Eigen::Vector3f evzminus = evz * -1; + + float s_xplus, s_xminus, s_yplus, s_yminus; + s_xplus = s_xminus = s_yplus = s_yminus = 0.f; + + //disambiguate rf using all points + for (const auto& point: grid->points) + { + Eigen::Vector3f pvector = point.getVector3fMap (); + float dist_x, dist_y; + dist_x = std::abs (evx.dot (pvector)); + dist_y = std::abs (evy.dot (pvector)); + + if ((pvector).dot (evx) >= 0) + s_xplus += dist_x; + else + s_xminus += dist_x; + + if ((pvector).dot (evy) >= 0) + s_yplus += dist_y; + else + s_yminus += dist_y; + + } + + if (s_xplus < s_xminus) + evx = evxminus; + + if (s_yplus < s_yminus) + evy = evyminus; + + //select the axis that could be disambiguated more easily + float fx, fy; + float max_x = static_cast (std::max (s_xplus, s_xminus)); + float min_x = static_cast (std::min (s_xplus, s_xminus)); + float max_y = static_cast (std::max (s_yplus, s_yminus)); + float min_y = static_cast (std::min (s_yplus, s_yminus)); + + fx = (min_x / max_x); + fy = (min_y / max_y); + + Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]); + if (normal3f.dot (evz) < 0) + evz = evzminus; + + //if fx/y close to 1, it was hard to disambiguate + //what if both are equally easy or difficult to disambiguate, namely fy == fx or very close + + float max_axis = std::max (fx, fy); + float min_axis = std::min (fx, fy); + + if ((min_axis / max_axis) > axis_ratio_) + { + PCL_WARN ("Both axes are equally easy/difficult to disambiguate\n"); + + Eigen::Vector3f evy_copy = evy; + Eigen::Vector3f evxminus = evx * -1; + Eigen::Vector3f evyminus = evy * -1; + + if (min_axis > min_axis_value_) + { + //combination of all possibilities + evy = evx.cross (evz); + Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); + transformations.push_back (trans); + + evx = evxminus; + evy = evx.cross (evz); + trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); + transformations.push_back (trans); + + evx = evy_copy; + evy = evx.cross (evz); + trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); + transformations.push_back (trans); + + evx = evyminus; + evy = evx.cross (evz); + trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); + transformations.push_back (trans); + + } + else + { + //1-st case (evx selected) + evy = evx.cross (evz); + Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); + transformations.push_back (trans); + + //2-nd case (evy selected) + evx = evy_copy; + evy = evx.cross (evz); + trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); + transformations.push_back (trans); + } + } + else + { + if (fy < fx) + { + evx = evy; + fx = fy; + } + + evy = evx.cross (evz); + Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat); + transformations.push_back (trans); + + } + + return true; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OURCVFHEstimation::computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut & output, + std::vector & cluster_indices) +{ + PointCloudOut ourcvfh_output; + + cluster_axes_.clear (); + cluster_axes_.resize (centroids_dominant_orientations_.size ()); + + for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++) + { + + std::vector < Eigen::Matrix4f, Eigen::aligned_allocator > transformations; + PointInTPtr grid (new pcl::PointCloud); + sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]); + + // Make a note of how many transformations correspond to each cluster + cluster_axes_[i] = transformations.size (); + + for (const auto &transformation : transformations) + { + + pcl::transformPointCloud (*processed, *grid, transformation); + transforms_.push_back (transformation); + valid_transforms_.push_back (true); + + std::vector < Eigen::VectorXf > quadrants (8); + int size_hists = 13; + int num_hists = 8; + for (int k = 0; k < num_hists; k++) + quadrants[k].setZero (size_hists); + + Eigen::Vector4f centroid_p; + centroid_p.setZero (); + Eigen::Vector4f max_pt; + pcl::getMaxDistance (*grid, centroid_p, max_pt); + max_pt[3] = 0; + double distance_normalization_factor = (centroid_p - max_pt).norm (); + + float hist_incr; + if (normalize_bins_) + hist_incr = 100.0f / static_cast (grid->points.size () - 1); + else + hist_incr = 1.0f; + + float * weights = new float[num_hists]; + float sigma = 0.01f; //1cm + float sigma_sq = sigma * sigma; + + for (const auto& point: grid->points) + { + Eigen::Vector4f p = point.getVector4fMap (); + p[3] = 0.f; + float d = p.norm (); + + //compute weight for all octants + float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq))); //how is the weight distributed among two semi-cubes + float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq))); + float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq))); + + //distribute the weights using the x-coordinate + if (p[0] >= 0) + { + for (std::size_t ii = 0; ii <= 3; ii++) + weights[ii] = 0.5f - wx * 0.5f; + + for (std::size_t ii = 4; ii <= 7; ii++) + weights[ii] = 0.5f + wx * 0.5f; + } + else + { + for (std::size_t ii = 0; ii <= 3; ii++) + weights[ii] = 0.5f + wx * 0.5f; + + for (std::size_t ii = 4; ii <= 7; ii++) + weights[ii] = 0.5f - wx * 0.5f; + } + + //distribute the weights using the y-coordinate + if (p[1] >= 0) + { + for (std::size_t ii = 0; ii <= 1; ii++) + weights[ii] *= 0.5f - wy * 0.5f; + for (std::size_t ii = 4; ii <= 5; ii++) + weights[ii] *= 0.5f - wy * 0.5f; + + for (std::size_t ii = 2; ii <= 3; ii++) + weights[ii] *= 0.5f + wy * 0.5f; + + for (std::size_t ii = 6; ii <= 7; ii++) + weights[ii] *= 0.5f + wy * 0.5f; + } + else + { + for (std::size_t ii = 0; ii <= 1; ii++) + weights[ii] *= 0.5f + wy * 0.5f; + for (std::size_t ii = 4; ii <= 5; ii++) + weights[ii] *= 0.5f + wy * 0.5f; + + for (std::size_t ii = 2; ii <= 3; ii++) + weights[ii] *= 0.5f - wy * 0.5f; + + for (std::size_t ii = 6; ii <= 7; ii++) + weights[ii] *= 0.5f - wy * 0.5f; + } + + //distribute the weights using the z-coordinate + if (p[2] >= 0) + { + for (std::size_t ii = 0; ii <= 7; ii += 2) + weights[ii] *= 0.5f - wz * 0.5f; + + for (std::size_t ii = 1; ii <= 7; ii += 2) + weights[ii] *= 0.5f + wz * 0.5f; + + } + else + { + for (std::size_t ii = 0; ii <= 7; ii += 2) + weights[ii] *= 0.5f + wz * 0.5f; + + for (std::size_t ii = 1; ii <= 7; ii += 2) + weights[ii] *= 0.5f - wz * 0.5f; + } + + int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1; + /* from http://www.pcl-users.org/OUR-CVFH-problem-td4028436.html + h_index will be 13 when d is computed on the farthest away point. + + adding the following after computing h_index fixes the problem: + */ + if(h_index > 12) + h_index = 12; + for (int j = 0; j < num_hists; j++) + quadrants[j][h_index] += hist_incr * weights[j]; + + } + + //copy to the cvfh signature + PointCloudOut vfh_signature; + vfh_signature.points.resize (1); + vfh_signature.width = vfh_signature.height = 1; + for (int d = 0; d < 308; ++d) + vfh_signature.points[0].histogram[d] = output.points[i].histogram[d]; + + int pos = 45 * 3; + for (int k = 0; k < num_hists; k++) + { + for (int ii = 0; ii < size_hists; ii++, pos++) + { + vfh_signature.points[0].histogram[pos] = quadrants[k][ii]; + } + } + + ourcvfh_output.points.push_back (vfh_signature.points[0]); + ourcvfh_output.width = ourcvfh_output.points.size (); + delete[] weights; + } + } + + if (!ourcvfh_output.points.empty ()) + { + ourcvfh_output.height = 1; + } + output = ourcvfh_output; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OURCVFHEstimation::computeFeature (PointCloudOut &output) +{ + if (refine_clusters_ <= 0.f) + refine_clusters_ = 1.f; + + // Check if input was set + if (!normals_) + { + PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + if (normals_->points.size () != surface_->points.size ()) + { + PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + centroids_dominant_orientations_.clear (); + clusters_.clear (); + transforms_.clear (); + dominant_normals_.clear (); + + // ---[ Step 0: remove normals with high curvature + std::vector indices_out; + std::vector indices_in; + filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_); + + pcl::PointCloud::Ptr normals_filtered_cloud (new pcl::PointCloud ()); + normals_filtered_cloud->width = static_cast (indices_in.size ()); + normals_filtered_cloud->height = 1; + normals_filtered_cloud->points.resize (normals_filtered_cloud->width); + + std::vector indices_from_nfc_to_indices; + indices_from_nfc_to_indices.resize (indices_in.size ()); + + for (std::size_t i = 0; i < indices_in.size (); ++i) + { + normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x; + normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y; + normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z; + //normals_filtered_cloud->points[i].getNormalVector4fMap() = normals_->points[indices_in[i]].getNormalVector4fMap(); + indices_from_nfc_to_indices[i] = indices_in[i]; + } + + std::vector clusters; + + if (normals_filtered_cloud->points.size () >= min_points_) + { + //recompute normals and use them for clustering + { + KdTreePtr normals_tree_filtered (new pcl::search::KdTree (false)); + normals_tree_filtered->setInputCloud (normals_filtered_cloud); + pcl::NormalEstimation n3d; + n3d.setRadiusSearch (radius_normals_); + n3d.setSearchMethod (normals_tree_filtered); + n3d.setInputCloud (normals_filtered_cloud); + n3d.compute (*normals_filtered_cloud); + } + + KdTreePtr normals_tree (new pcl::search::KdTree (false)); + normals_tree->setInputCloud (normals_filtered_cloud); + + extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters, + eps_angle_threshold_, static_cast (min_points_)); + + std::vector clusters_filtered; + int cluster_filtered_idx = 0; + for (const auto &cluster : clusters) + { + + pcl::PointIndices pi; + pcl::PointIndices pi_cvfh; + pcl::PointIndices pi_filtered; + + clusters_.push_back (pi); + clusters_filtered.push_back (pi_filtered); + + Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero (); + Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero (); + + for (const auto &index : cluster.indices) + { + avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap (); + avg_centroid += normals_filtered_cloud->points[index].getVector4fMap (); + } + + avg_normal /= static_cast (cluster.indices.size ()); + avg_centroid /= static_cast (cluster.indices.size ()); + avg_normal.normalize (); + + Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]); + Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); + + for (const auto &index : cluster.indices) + { + //decide if normal should be added + double dot_p = avg_normal.dot (normals_filtered_cloud->points[index].getNormalVector4fMap ()); + if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_)) + { + clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]); + clusters_filtered[cluster_filtered_idx].indices.push_back (index); + } + } + + //remove last cluster if no points found... + if (clusters_[cluster_filtered_idx].indices.empty ()) + { + clusters_.pop_back (); + clusters_filtered.pop_back (); + } + else + cluster_filtered_idx++; + } + + clusters = clusters_filtered; + + } + + pcl::VFHEstimation vfh; + vfh.setInputCloud (surface_); + vfh.setInputNormals (normals_); + vfh.setIndices (indices_); + vfh.setSearchMethod (this->tree_); + vfh.setUseGivenNormal (true); + vfh.setUseGivenCentroid (true); + vfh.setNormalizeBins (normalize_bins_); + output.height = 1; + + // ---[ Step 1b : check if any dominant cluster was found + if (!clusters.empty ()) + { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information + for (const auto &cluster : clusters) //for each cluster + { + Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero (); + Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero (); + + for (const auto &index : cluster.indices) + { + avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap (); + avg_centroid += normals_filtered_cloud->points[index].getVector4fMap (); + } + + avg_normal /= static_cast (cluster.indices.size ()); + avg_centroid /= static_cast (cluster.indices.size ()); + avg_normal.normalize (); + + //append normal and centroid for the clusters + dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]); + centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]); + } + + //compute modified VFH for all dominant clusters and add them to the list! + output.points.resize (dominant_normals_.size ()); + output.width = static_cast (dominant_normals_.size ()); + + for (std::size_t i = 0; i < dominant_normals_.size (); ++i) + { + //configure VFH computation for CVFH + vfh.setNormalToUse (dominant_normals_[i]); + vfh.setCentroidToUse (centroids_dominant_orientations_[i]); + pcl::PointCloud vfh_signature; + vfh.compute (vfh_signature); + output.points[i] = vfh_signature.points[0]; + } + + //finish filling the descriptor with the shape distribution + PointInTPtr cloud_input (new pcl::PointCloud); + pcl::copyPointCloud (*surface_, *indices_, *cloud_input); + computeRFAndShapeDistribution (cloud_input, output, clusters_); //this will set transforms_ + } + else + { // ---[ Step 1b.1 : If no, compute a VFH using all the object points + + PCL_WARN("No clusters were found in the surface... using VFH...\n"); + Eigen::Vector4f avg_centroid; + pcl::compute3DCentroid (*surface_, avg_centroid); + Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]); + centroids_dominant_orientations_.push_back (cloud_centroid); + + //configure VFH computation using all object points + vfh.setCentroidToUse (cloud_centroid); + vfh.setUseGivenNormal (false); + + pcl::PointCloud vfh_signature; + vfh.compute (vfh_signature); + + output.points.resize (1); + output.width = 1; + + output.points[0] = vfh_signature.points[0]; + Eigen::Matrix4f id = Eigen::Matrix4f::Identity (); + transforms_.push_back (id); + valid_transforms_.push_back (false); + } +} + +#define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation; + +#endif // PCL_FEATURES_IMPL_OURCVFH_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/pfh.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/pfh.hpp new file mode 100755 index 0000000..d0b57e8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/pfh.hpp @@ -0,0 +1,230 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_PFH_H_ +#define PCL_FEATURES_IMPL_PFH_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PFHEstimation::computePairFeatures ( + const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4) +{ + pcl::computePairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (), + cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (), + f1, f2, f3, f4); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PFHEstimation::computePointPFHSignature ( + const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + const std::vector &indices, int nr_split, Eigen::VectorXf &pfh_histogram) +{ + int h_index, h_p; + + // Clear the resultant point histogram + pfh_histogram.setZero (); + + // Factorization constant + float hist_incr = 100.0f / static_cast (indices.size () * (indices.size () - 1) / 2); + + std::pair key; + bool key_found = false; + + // Iterate over all the points in the neighborhood + for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx) + { + for (std::size_t j_idx = 0; j_idx < i_idx; ++j_idx) + { + // If the 3D points are invalid, don't bother estimating, just continue + if (!isFinite (cloud.points[indices[i_idx]]) || !isFinite (cloud.points[indices[j_idx]])) + continue; + + if (use_cache_) + { + // In order to create the key, always use the smaller index as the first key pair member + int p1, p2; + // if (indices[i_idx] >= indices[j_idx]) + // { + p1 = indices[i_idx]; + p2 = indices[j_idx]; + // } + // else + // { + // p1 = indices[j_idx]; + // p2 = indices[i_idx]; + // } + key = std::pair (p1, p2); + + // Check to see if we already estimated this pair in the global hashmap + std::map, Eigen::Vector4f, std::less<>, Eigen::aligned_allocator, Eigen::Vector4f> > >::iterator fm_it = feature_map_.find (key); + if (fm_it != feature_map_.end ()) + { + pfh_tuple_ = fm_it->second; + key_found = true; + } + else + { + // Compute the pair NNi to NNj + if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx], + pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3])) + continue; + + key_found = false; + } + } + else + if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx], + pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3])) + continue; + + // Normalize the f1, f2, f3 features and push them in the histogram + f_index_[0] = static_cast (std::floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_))); + if (f_index_[0] < 0) f_index_[0] = 0; + if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1; + + f_index_[1] = static_cast (std::floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5))); + if (f_index_[1] < 0) f_index_[1] = 0; + if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1; + + f_index_[2] = static_cast (std::floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5))); + if (f_index_[2] < 0) f_index_[2] = 0; + if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1; + + // Copy into the histogram + h_index = 0; + h_p = 1; + for (const int &d : f_index_) + { + h_index += h_p * d; + h_p *= nr_split; + } + pfh_histogram[h_index] += hist_incr; + + if (use_cache_ && !key_found) + { + // Save the value in the hashmap + feature_map_[key] = pfh_tuple_; + + // Use a maximum cache so that we don't go overboard on RAM usage + key_list_.push (key); + // Check to see if we need to remove an element due to exceeding max_size + if (key_list_.size () > max_cache_size_) + { + // Remove the oldest element. + feature_map_.erase (key_list_.front ()); + key_list_.pop (); + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PFHEstimation::computeFeature (PointCloudOut &output) +{ + // Clear the feature map + feature_map_.clear (); + std::queue > empty; + std::swap (key_list_, empty); + + pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_); + + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + output.is_dense = true; + // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense + if (input_->is_dense) + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d) + output.points[idx].histogram[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // Estimate the PFH signature at each patch + computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_); + + // Copy into the resultant cloud + for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d) + output.points[idx].histogram[d] = pfh_histogram_[d]; + } + } + else + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d) + output.points[idx].histogram[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // Estimate the PFH signature at each patch + computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_); + + // Copy into the resultant cloud + for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d) + output.points[idx].histogram[d] = pfh_histogram_[d]; + } + } +} + +#define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation; + +#endif // PCL_FEATURES_IMPL_PFH_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/pfhrgb.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/pfhrgb.hpp new file mode 100755 index 0000000..fd80181 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/pfhrgb.hpp @@ -0,0 +1,161 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_PFHRGB_H_ +#define PCL_FEATURES_IMPL_PFHRGB_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PFHRGBEstimation::computeRGBPairFeatures ( + const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + int p_idx, int q_idx, + float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7) +{ + Eigen::Vector4i colors1 (cloud.points[p_idx].r, cloud.points[p_idx].g, cloud.points[p_idx].b, 0), + colors2 (cloud.points[q_idx].r, cloud.points[q_idx].g, cloud.points[q_idx].b, 0); + pcl::computeRGBPairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (), + colors1, + cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (), + colors2, + f1, f2, f3, f4, f5, f6, f7); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PFHRGBEstimation::computePointPFHRGBSignature ( + const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + const std::vector &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram) +{ + int h_index, h_p; + + // Clear the resultant point histogram + pfhrgb_histogram.setZero (); + + // Factorization constant + float hist_incr = 100.0f / static_cast (indices.size () * (indices.size () - 1) / 2); + + // Iterate over all the points in the neighborhood + for (const auto& index_i: indices) + { + for (const auto& index_j: indices) + { + // Avoid unnecessary returns + if (index_i == index_j) + continue; + + // Compute the pair NNi to NNj + if (!computeRGBPairFeatures (cloud, normals, index_i, index_j, + pfhrgb_tuple_[0], pfhrgb_tuple_[1], pfhrgb_tuple_[2], pfhrgb_tuple_[3], + pfhrgb_tuple_[4], pfhrgb_tuple_[5], pfhrgb_tuple_[6])) + continue; + + // Normalize the f1, f2, f3, f5, f6, f7 features and push them in the histogram + f_index_[0] = static_cast (std::floor (nr_split * ((pfhrgb_tuple_[0] + M_PI) * d_pi_))); + // @TODO: confirm "not to do for i == 3" + for (int i = 1; i < 3; ++i) + { + const float feature_value = nr_split * ((pfhrgb_tuple_[i] + 1.0) * 0.5); + f_index_[i] = static_cast (std::floor (feature_value)); + } + // color ratios are in [-1, 1] + for (int i = 4; i < 7; ++i) + { + const float feature_value = nr_split * ((pfhrgb_tuple_[i] + 1.0) * 0.5); + f_index_[i] = static_cast (std::floor (feature_value)); + } + for (auto& feature: f_index_) + { + feature = std::min(nr_split - 1, std::max(0, feature)); + } + + // Copy into the histogram + h_index = 0; + h_p = 1; + for (int d = 0; d < 3; ++d) + { + h_index += h_p * f_index_[d]; + h_p *= nr_split; + } + pfhrgb_histogram[h_index] += hist_incr; + + // and the colors + h_index = 125; + h_p = 1; + for (int d = 4; d < 7; ++d) + { + h_index += h_p * f_index_[d]; + h_p *= nr_split; + } + pfhrgb_histogram[h_index] += hist_incr; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PFHRGBEstimation::computeFeature (PointCloudOut &output) +{ + /// nr_subdiv^3 for RGB and nr_subdiv^3 for the angular features + pfhrgb_histogram_.setZero (2 * nr_subdiv_ * nr_subdiv_ * nr_subdiv_); + pfhrgb_tuple_.setZero (7); + + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists); + + // Estimate the PFH signature at each patch + computePointPFHRGBSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfhrgb_histogram_); + + std::copy_n (pfhrgb_histogram_.data (), pfhrgb_histogram_.size (), + output.points[idx].histogram); + } +} + +#define PCL_INSTANTIATE_PFHRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHRGBEstimation; + +#endif /* PCL_FEATURES_IMPL_PFHRGB_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/ppf.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/ppf.hpp new file mode 100755 index 0000000..5a3ea88 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/ppf.hpp @@ -0,0 +1,123 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_PPF_H_ +#define PCL_FEATURES_IMPL_PPF_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::PPFEstimation::PPFEstimation () + : FeatureFromNormals () +{ + feature_name_ = "PPFEstimation"; + // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () + Feature::tree_.reset (new pcl::search::KdTree ()); + Feature::search_radius_ = 1.0f; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PPFEstimation::computeFeature (PointCloudOut &output) +{ + // Initialize output container - overwrite the sizes done by Feature::initCompute () + output.points.resize (indices_->size () * input_->points.size ()); + output.height = 1; + output.width = static_cast (output.points.size ()); + output.is_dense = true; + + // Compute point pair features for every pair of points in the cloud + for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i) + { + std::size_t i = (*indices_)[index_i]; + for (std::size_t j = 0 ; j < input_->points.size (); ++j) + { + PointOutT p; + if (i != j) + { + if (//pcl::computePPFPairFeature + pcl::computePairFeatures (input_->points[i].getVector4fMap (), + normals_->points[i].getNormalVector4fMap (), + input_->points[j].getVector4fMap (), + normals_->points[j].getNormalVector4fMap (), + p.f1, p.f2, p.f3, p.f4)) + { + // Calculate alpha_m angle + Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), + model_reference_normal = normals_->points[i].getNormalVector3fMap (), + model_point = input_->points[j].getVector3fMap (); + float rotation_angle = std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis = (parallel_to_x)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_mg (rotation_angle, rotation_axis); + Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg); + + Eigen::Vector3f model_point_transformed = transform_mg * model_point; + float angle = std::atan2 ( -model_point_transformed(2), model_point_transformed(1)); + if (std::sin (angle) * model_point_transformed(2) < 0.0f) + angle *= (-1); + p.alpha_m = -angle; + } + else + { + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %u and %u went wrong.\n", getClassName ().c_str (), i, j); + p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + } + } + // Do not calculate the feature for identity pairs (i, i) as they are not used + // in the following computations + else + { + p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + } + + output.points[index_i*input_->points.size () + j] = p; + } + } +} + +#define PCL_INSTANTIATE_PPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFEstimation; + + +#endif // PCL_FEATURES_IMPL_PPF_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/ppfrgb.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/ppfrgb.hpp new file mode 100755 index 0000000..8841634 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/ppfrgb.hpp @@ -0,0 +1,180 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_PPFRGB_H_ +#define PCL_FEATURES_IMPL_PPFRGB_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::PPFRGBEstimation::PPFRGBEstimation () +: FeatureFromNormals () +{ + feature_name_ = "PPFRGBEstimation"; + // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () + Feature::tree_.reset (new pcl::search::KdTree ()); + Feature::search_radius_ = 1.0f; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PPFRGBEstimation::computeFeature (PointCloudOut &output) +{ + // Initialize output container - overwrite the sizes done by Feature::initCompute () + output.points.resize (indices_->size () * input_->points.size ()); + output.height = 1; + output.width = static_cast (output.points.size ()); + + // Compute point pair features for every pair of points in the cloud + for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i) + { + std::size_t i = (*indices_)[index_i]; + for (std::size_t j = 0 ; j < input_->points.size (); ++j) + { + PointOutT p; + if (i != j) + { + if (pcl::computeRGBPairFeatures + (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (), + input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (), + p.f1, p.f2, p.f3, p.f4, p.r_ratio, p.g_ratio, p.b_ratio)) + { + // Calculate alpha_m angle + Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), + model_reference_normal = normals_->points[i].getNormalVector3fMap (), + model_point = input_->points[j].getVector3fMap (); + Eigen::AngleAxisf rotation_mg (std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), + model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); + Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; + + Eigen::Vector3f model_point_transformed = transform_mg * model_point; + float angle = std::atan2 ( -model_point_transformed(2), model_point_transformed(1)); + if (std::sin (angle) * model_point_transformed(2) < 0.0f) + angle *= (-1); + p.alpha_m = -angle; + } + else + { + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j); + p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f; + } + } + // Do not calculate the feature for identity pairs (i, i) as they are not used + // in the following computations + else + p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f; + + output.points[index_i*input_->points.size () + j] = p; + } + } +} + + + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::PPFRGBRegionEstimation::PPFRGBRegionEstimation () +: FeatureFromNormals () +{ + feature_name_ = "PPFRGBEstimation"; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PPFRGBRegionEstimation::computeFeature (PointCloudOut &output) +{ + PCL_INFO ("before computing output size: %u\n", output.size ()); + output.resize (indices_->size ()); + for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i) + { + int i = (*indices_)[index_i]; + std::vector nn_indices; + std::vector nn_distances; + tree_->radiusSearch (i, static_cast (search_radius_), nn_indices, nn_distances); + + PointOutT average_feature_nn; + average_feature_nn.alpha_m = 0; + average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 = + average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f; + + for (const int &j : nn_indices) + { + if (i != j) + { + float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio; + if (pcl::computeRGBPairFeatures + (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (), + input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (), + f1, f2, f3, f4, r_ratio, g_ratio, b_ratio)) + { + average_feature_nn.f1 += f1; + average_feature_nn.f2 += f2; + average_feature_nn.f3 += f3; + average_feature_nn.f4 += f4; + average_feature_nn.r_ratio += r_ratio; + average_feature_nn.g_ratio += g_ratio; + average_feature_nn.b_ratio += b_ratio; + } + else + { + PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j); + } + } + } + + float normalization_factor = static_cast (nn_indices.size ()); + average_feature_nn.f1 /= normalization_factor; + average_feature_nn.f2 /= normalization_factor; + average_feature_nn.f3 /= normalization_factor; + average_feature_nn.f4 /= normalization_factor; + average_feature_nn.r_ratio /= normalization_factor; + average_feature_nn.g_ratio /= normalization_factor; + average_feature_nn.b_ratio /= normalization_factor; + output.points[index_i] = average_feature_nn; + } + PCL_INFO ("Output size: %u\n", output.points.size ()); +} + + +#define PCL_INSTANTIATE_PPFRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBEstimation; +#define PCL_INSTANTIATE_PPFRGBRegionEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBRegionEstimation; + +#endif // PCL_FEATURES_IMPL_PPFRGB_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/principal_curvatures.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/principal_curvatures.hpp new file mode 100755 index 0000000..bfa2499 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/principal_curvatures.hpp @@ -0,0 +1,165 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ +#define PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PrincipalCurvaturesEstimation::computePointPrincipalCurvatures ( + const pcl::PointCloud &normals, int p_idx, const std::vector &indices, + float &pcx, float &pcy, float &pcz, float &pc1, float &pc2) +{ + EIGEN_ALIGN16 Eigen::Matrix3f I = Eigen::Matrix3f::Identity (); + Eigen::Vector3f n_idx (normals.points[p_idx].normal[0], normals.points[p_idx].normal[1], normals.points[p_idx].normal[2]); + EIGEN_ALIGN16 Eigen::Matrix3f M = I - n_idx * n_idx.transpose (); // projection matrix (into tangent plane) + + // Project normals into the tangent plane + Eigen::Vector3f normal; + projected_normals_.resize (indices.size ()); + xyz_centroid_.setZero (); + for (std::size_t idx = 0; idx < indices.size(); ++idx) + { + normal[0] = normals.points[indices[idx]].normal[0]; + normal[1] = normals.points[indices[idx]].normal[1]; + normal[2] = normals.points[indices[idx]].normal[2]; + + projected_normals_[idx] = M * normal; + xyz_centroid_ += projected_normals_[idx]; + } + + // Estimate the XYZ centroid + xyz_centroid_ /= static_cast (indices.size ()); + + // Initialize to 0 + covariance_matrix_.setZero (); + + // For each point in the cloud + for (std::size_t idx = 0; idx < indices.size (); ++idx) + { + demean_ = projected_normals_[idx] - xyz_centroid_; + + double demean_xy = demean_[0] * demean_[1]; + double demean_xz = demean_[0] * demean_[2]; + double demean_yz = demean_[1] * demean_[2]; + + covariance_matrix_(0, 0) += demean_[0] * demean_[0]; + covariance_matrix_(0, 1) += static_cast (demean_xy); + covariance_matrix_(0, 2) += static_cast (demean_xz); + + covariance_matrix_(1, 0) += static_cast (demean_xy); + covariance_matrix_(1, 1) += demean_[1] * demean_[1]; + covariance_matrix_(1, 2) += static_cast (demean_yz); + + covariance_matrix_(2, 0) += static_cast (demean_xz); + covariance_matrix_(2, 1) += static_cast (demean_yz); + covariance_matrix_(2, 2) += demean_[2] * demean_[2]; + } + + // Extract the eigenvalues and eigenvectors + pcl::eigen33 (covariance_matrix_, eigenvalues_); + pcl::computeCorrespondingEigenVector (covariance_matrix_, eigenvalues_ [2], eigenvector_); + + pcx = eigenvector_ [0]; + pcy = eigenvector_ [1]; + pcz = eigenvector_ [2]; + float indices_size = 1.0f / static_cast (indices.size ()); + pc1 = eigenvalues_ [2] * indices_size; + pc2 = eigenvalues_ [1] * indices_size; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PrincipalCurvaturesEstimation::computeFeature (PointCloudOut &output) +{ + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + output.is_dense = true; + // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense + if (input_->is_dense) + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + output.points[idx].principal_curvature[0] = output.points[idx].principal_curvature[1] = output.points[idx].principal_curvature[2] = + output.points[idx].pc1 = output.points[idx].pc2 = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + // Estimate the principal curvatures at each patch + computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices, + output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2], + output.points[idx].pc1, output.points[idx].pc2); + } + } + else + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + if (!isFinite ((*input_)[(*indices_)[idx]]) || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + output.points[idx].principal_curvature[0] = output.points[idx].principal_curvature[1] = output.points[idx].principal_curvature[2] = + output.points[idx].pc1 = output.points[idx].pc2 = std::numeric_limits::quiet_NaN (); + output.is_dense = false; + continue; + } + + // Estimate the principal curvatures at each patch + computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices, + output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2], + output.points[idx].pc1, output.points[idx].pc2); + } + } +} + +#define PCL_INSTANTIATE_PrincipalCurvaturesEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PrincipalCurvaturesEstimation; + +#endif // PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/range_image_border_extractor.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/range_image_border_extractor.hpp new file mode 100755 index 0000000..2a8e899 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/range_image_border_extractor.hpp @@ -0,0 +1,398 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Bastian Steder + */ + +#include + +namespace pcl { + +////////// STATIC ////////// +float RangeImageBorderExtractor::getObstacleBorderAngle(const BorderTraits& border_traits) +{ + float x=0.0f, y=0.0f; + if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_RIGHT]) + ++x; + if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_LEFT]) + --x; + if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_TOP]) + --y; + if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM]) + ++y; + + return std::atan2(y, x); +} + +inline std::ostream& operator << (std::ostream& os, const RangeImageBorderExtractor::Parameters& p) +{ + os << PVARC(p.pixel_radius_borders)<getPoint(x, y); + PointWithRange neighbor; + range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, pixel_radius, neighbor); + if (std::isinf(neighbor.range)) + { + if (neighbor.range < 0.0f) + return 0.0f; + //std::cout << "INF edge -> Setting to 1.0\n"; + return 1.0f; // TODO: Something more intelligent + } + + float neighbor_distance_squared = squaredEuclideanDistance(neighbor, point); + if (neighbor_distance_squared <= local_surface.max_neighbor_distance_squared) + return 0.0f; + float ret = 1.0f - std::sqrt (local_surface.max_neighbor_distance_squared / neighbor_distance_squared); + if (neighbor.range < point.range) + ret = -ret; + return ret; +} + +//float RangeImageBorderExtractor::getNormalBasedBorderScore(const RangeImageBorderExtractor::LocalSurface& local_surface, + //int x, int y, int offset_x, int offset_y) const +//{ + //PointWithRange neighbor; + //range_image_->get1dPointAverage(x+offset_x, y+offset_y, offset_x, offset_y, parameters_.pixel_radius_borders, neighbor); + //if (std::isinf(neighbor.range)) + //{ + //if (neighbor.range < 0.0f) + //return 0.0f; + //else + //return 1.0f; // TODO: Something more intelligent (Compare normal with viewing direction) + //} + + //float normal_distance_to_plane_squared = local_surface.smallest_eigenvalue_no_jumps; + //float distance_to_plane = local_surface.normal_no_jumps.dot(local_surface.neighborhood_mean_no_jumps-neighbor.getVector3fMap()); + //bool shadow_side = distance_to_plane < 0.0f; + //float distance_to_plane_squared = pow(distance_to_plane, 2); + //if (distance_to_plane_squared <= normal_distance_to_plane_squared) + //return 0.0f; + //float ret = 1.0f - (normal_distance_to_plane_squared/distance_to_plane_squared); + //if (shadow_side) + //ret = -ret; + ////std::cout << PVARC(normal_distance_to_plane_squared)< "<getPoint(x, y); + Eigen::Vector3f neighbor_point; + range_image_->calculate3DPoint(static_cast (x+delta_x), static_cast (y+delta_y), point.range, neighbor_point); + //std::cout << "Neighborhood point is "<getSensorPos(), + viewing_direction = neighbor_point-sensor_pos; + + float lambda = (local_surface->normal_no_jumps.dot(local_surface->neighborhood_mean_no_jumps-sensor_pos)/ + local_surface->normal_no_jumps.dot(viewing_direction)); + neighbor_point = lambda*viewing_direction + sensor_pos; + //std::cout << "Neighborhood point projected onto plane is "< "<< direction[0]<<","<< direction[1]<<","<< direction[2]<<"\n"; + direction = neighbor_point-point.getVector3fMap(); + direction.normalize(); + + return true; +} + +void RangeImageBorderExtractor::calculateBorderDirection(int x, int y) +{ + int index = y*range_image_->width + x; + Eigen::Vector3f*& border_direction = border_directions_[index]; + border_direction = nullptr; + const BorderDescription& border_description = border_descriptions_->points[index]; + const BorderTraits& border_traits = border_description.traits; + if (!border_traits[BORDER_TRAIT__OBSTACLE_BORDER]) + return; + border_direction = new Eigen::Vector3f(0.0f, 0.0f, 0.0f); + if (!get3dDirection(border_description, *border_direction, surface_structure_[index])) + { + delete border_direction; + border_direction = nullptr; + return; + } +} + +bool RangeImageBorderExtractor::changeScoreAccordingToShadowBorderValue(int x, int y, int offset_x, int offset_y, float* border_scores, + float* border_scores_other_direction, int& shadow_border_idx) const +{ + float& border_score = border_scores[y*range_image_->width+x]; + + shadow_border_idx = -1; + if (border_scoreisMaxRange(x+offset_x, y+offset_y)) + { + shadow_border_idx = (y+offset_y)*range_image_->width + x+offset_x; + return true; + } + } + + float best_shadow_border_score = 0.0f; + + for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance) + { + int neighbor_x=x+neighbor_distance*offset_x, neighbor_y=y+neighbor_distance*offset_y; + if (!range_image_->isInImage(neighbor_x, neighbor_y)) + continue; + float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x]; + + if (neighbor_shadow_border_score < best_shadow_border_score) + { + shadow_border_idx = neighbor_y*range_image_->width + neighbor_x; + best_shadow_border_score = neighbor_shadow_border_score; + } + } + if (shadow_border_idx >= 0) + { + //std::cout << PVARC(border_score)<=parameters_.minimum_border_probability) + return true; + } + shadow_border_idx = -1; + border_score = 0.0f; // Since there was no shadow border found we set this value to zero, so that it does not influence the maximum search + return false; +} + +float RangeImageBorderExtractor::updatedScoreAccordingToNeighborValues(int x, int y, const float* border_scores) const +{ + float max_score_bonus = 0.5f; + + float border_score = border_scores[y*range_image_->width+x]; + + // Check if an update can bring the score to a value higher than the minimum + if (border_score + max_score_bonus*(1.0f-border_score) < parameters_.minimum_border_probability) + return border_score; + + float average_neighbor_score=0.0f, weight_sum=0.0f; + for (int y2=y-1; y2<=y+1; ++y2) + { + for (int x2=x-1; x2<=x+1; ++x2) + { + if (!range_image_->isInImage(x2, y2) || (x2==x&&y2==y)) + continue; + average_neighbor_score += border_scores[y2*range_image_->width+x2]; + weight_sum += 1.0f; + } + } + average_neighbor_score /=weight_sum; + + if (average_neighbor_score*border_score < 0.0f) + return border_score; + + float new_border_score = border_score + max_score_bonus * average_neighbor_score * (1.0f-std::abs(border_score)); + + //std::cout << PVARC(border_score)<width+x]; + if (border_scoreisInImage(neighbor_x, neighbor_y)) + continue; + float neighbor_shadow_border_score = border_scores_other_direction[neighbor_y*range_image_->width+neighbor_x]; + + if (neighbor_shadow_border_score < best_shadow_border_score) + { + shadow_border_idx = neighbor_y*range_image_->width + neighbor_x; + best_shadow_border_score = neighbor_shadow_border_score; + } + } + if (shadow_border_idx >= 0) + { + return true; + } + border_score = 0.0f; // Since there was no shadow border found we set this value to zero, so that it does not influence the maximum search + return false; +} + +bool RangeImageBorderExtractor::checkIfMaximum(int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const +{ + float border_score = border_scores[y*range_image_->width+x]; + int neighbor_x=x-offset_x, neighbor_y=y-offset_y; + if (range_image_->isInImage(neighbor_x, neighbor_y) && border_scores[neighbor_y*range_image_->width+neighbor_x] > border_score) + return false; + + for (int neighbor_distance=1; neighbor_distance<=parameters_.pixel_radius_borders; ++neighbor_distance) + { + neighbor_x=x+neighbor_distance*offset_x; neighbor_y=y+neighbor_distance*offset_y; + if (!range_image_->isInImage(neighbor_x, neighbor_y)) + continue; + int neighbor_index = neighbor_y*range_image_->width + neighbor_x; + if (neighbor_index==shadow_border_idx) + return true; + + float neighbor_border_score = border_scores[neighbor_index]; + if (neighbor_border_score > border_score) + return false; + } + return true; +} + +bool RangeImageBorderExtractor::calculateMainPrincipalCurvature(int x, int y, int radius, float& magnitude, + Eigen::Vector3f& main_direction) const +{ + magnitude = 0.0f; + int index = y*range_image_->width+x; + LocalSurface* local_surface = surface_structure_[index]; + if (local_surface==nullptr) + return false; + //const PointWithRange& point = range_image_->getPointNoCheck(x,y); + + //Eigen::Vector3f& normal = local_surface->normal_no_jumps; + //Eigen::Matrix3f to_tangent_plane = Eigen::Matrix3f::Identity() - normal*normal.transpose(); + + VectorAverage3f vector_average; + bool beams_valid[9]; + for (int step=1; step<=radius; ++step) + { + int beam_idx = 0; + for (int y2=y-step; y2<=y+step; y2+=step) + { + for (int x2=x-step; x2<=x+step; x2+=step) + { + bool& beam_valid = beams_valid[beam_idx++]; + if (step==1) + { + if (x2==x && y2==y) + beam_valid = false; + else + beam_valid = true; + } + else + if (!beam_valid) + continue; + //std::cout << x2-x<<","<isValid(x2,y2)) + continue; + + int index2 = y2*range_image_->width + x2; + + const BorderTraits& border_traits = border_descriptions_->points[index2].traits; + if (border_traits[BORDER_TRAIT__VEIL_POINT] || border_traits[BORDER_TRAIT__SHADOW_BORDER]) + { + beam_valid = false; + continue; + } + + //const PointWithRange& point2 = range_image_->getPoint(index2); + LocalSurface* local_surface2 = surface_structure_[index2]; + if (local_surface2==nullptr) + continue; + Eigen::Vector3f& normal2 = local_surface2->normal_no_jumps; + //float distance_squared = squaredEuclideanDistance(point, point2); + //vector_average.add(to_tangent_plane*normal2); + vector_average.add(normal2); + } + } + } + //std::cout << "\n"; + if (vector_average.getNoOfSamples() < 3) + return false; + + Eigen::Vector3f eigen_values, eigen_vector1, eigen_vector2; + vector_average.doPCA(eigen_values, eigen_vector1, eigen_vector2, main_direction); + magnitude = std::sqrt (eigen_values[2]); + //magnitude = eigen_values[2]; + //magnitude = 1.0f - powf(1.0f-magnitude, 5); + //magnitude = 1.0f - powf(1.0f-magnitude, 10); + //magnitude += magnitude - powf(magnitude,2); + //magnitude += magnitude - powf(magnitude,2); + + //magnitude = std::sqrt (local_surface->eigen_values[0]/local_surface->eigen_values.sum()); + //magnitude = std::sqrt (local_surface->eigen_values_no_jumps[0]/local_surface->eigen_values_no_jumps.sum()); + + //if (surface_structure_[y*range_image_->width+x+1]==NULL||surface_structure_[y*range_image_->width+x-1]==NULL) + //{ + //magnitude = -std::numeric_limits::infinity (); + //return false; + //} + //float angle2 = std::acos(surface_structure_[y*range_image_->width+x+1]->normal.dot(local_surface->normal)), + //angle1 = std::acos(surface_structure_[y*range_image_->width+x-1]->normal.dot(local_surface->normal)); + //magnitude = angle2-angle1; + + return std::isfinite(magnitude); +} + +} // namespace end diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/rift.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/rift.hpp new file mode 100755 index 0000000..d5ed46b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/rift.hpp @@ -0,0 +1,181 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_RIFT_H_ +#define PCL_FEATURES_IMPL_RIFT_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RIFTEstimation::computeRIFT ( + const PointCloudIn &cloud, const PointCloudGradient &gradient, + int p_idx, float radius, const std::vector &indices, + const std::vector &sqr_distances, Eigen::MatrixXf &rift_descriptor) +{ + if (indices.empty ()) + { + PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n"); + return; + } + + // Determine the number of bins to use based on the size of rift_descriptor + int nr_distance_bins = static_cast (rift_descriptor.rows ()); + int nr_gradient_bins = static_cast (rift_descriptor.cols ()); + + // Get the center point + pcl::Vector3fMapConst p0 = cloud.points[p_idx].getVector3fMap (); + + // Compute the RIFT descriptor + rift_descriptor.setZero (); + for (std::size_t idx = 0; idx < indices.size (); ++idx) + { + // Compute the gradient magnitude and orientation (relative to the center point) + pcl::Vector3fMapConst point = cloud.points[indices[idx]].getVector3fMap (); + Eigen::Map gradient_vector (& (gradient.points[indices[idx]].gradient[0])); + + float gradient_magnitude = gradient_vector.norm (); + float gradient_angle_from_center = std::acos (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude); + if (!std::isfinite (gradient_angle_from_center)) + gradient_angle_from_center = 0.0; + + // Normalize distance and angle values to: 0.0 <= d,g < nr_distances_bins,nr_gradient_bins + const float eps = std::numeric_limits::epsilon (); + float d = static_cast (nr_distance_bins) * std::sqrt (sqr_distances[idx]) / (radius + eps); + float g = static_cast (nr_gradient_bins) * gradient_angle_from_center / (static_cast (M_PI) + eps); + + // Compute the bin indices that need to be updated + int d_idx_min = (std::max)(static_cast (std::ceil (d - 1)), 0); + int d_idx_max = (std::min)(static_cast (std::floor (d + 1)), nr_distance_bins - 1); + int g_idx_min = static_cast (std::ceil (g - 1)); + int g_idx_max = static_cast (std::floor (g + 1)); + + // Update the appropriate bins of the histogram + for (int g_idx = g_idx_min; g_idx <= g_idx_max; ++g_idx) + { + // Because gradient orientation is cyclical, out-of-bounds values must wrap around + int g_idx_wrapped = ((g_idx + nr_gradient_bins) % nr_gradient_bins); + + for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx) + { + // To avoid boundary effects, use linear interpolation when updating each bin + float w = (1.0f - std::abs (d - static_cast (d_idx))) * (1.0f - std::abs (g - static_cast (g_idx))); + + rift_descriptor (d_idx, g_idx_wrapped) += w * gradient_magnitude; + } + } + } + + // Normalize the RIFT descriptor to unit magnitude + rift_descriptor.normalize (); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RIFTEstimation::computeFeature (PointCloudOut &output) +{ + // Make sure a search radius is set + if (search_radius_ == 0.0) + { + PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n", + getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Make sure the RIFT descriptor has valid dimensions + if (nr_gradient_bins_ <= 0) + { + PCL_ERROR ("[pcl::%s::computeFeature] The number of gradient bins must be greater than zero!\n", + getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + if (nr_distance_bins_ <= 0) + { + PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n", + getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Check for valid input gradient + if (!gradient_) + { + PCL_ERROR ("[pcl::%s::computeFeature] No input gradient was given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + if (gradient_->points.size () != surface_->points.size ()) + { + PCL_ERROR ("[pcl::%s::computeFeature] ", getClassName ().c_str ()); + PCL_ERROR ("The number of points in the input dataset differs from the number of points in the gradient!\n"); + output.width = output.height = 0; + output.points.clear (); + return; + } + + Eigen::MatrixXf rift_descriptor (nr_distance_bins_, nr_gradient_bins_); + std::vector nn_indices; + std::vector nn_dist_sqr; + + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + // Find neighbors within the search radius + tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr); + + // Compute the RIFT descriptor + computeRIFT (*surface_, *gradient_, (*indices_)[idx], static_cast (search_radius_), nn_indices, nn_dist_sqr, rift_descriptor); + + // Default layout is column major, copy elementwise + std::copy_n (rift_descriptor.data (), rift_descriptor.size (), output.points[idx].histogram); + } +} + +#define PCL_INSTANTIATE_RIFTEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RIFTEstimation; + +#endif // PCL_FEATURES_IMPL_RIFT_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/rops_estimation.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/rops_estimation.hpp new file mode 100755 index 0000000..53213c3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/rops_estimation.hpp @@ -0,0 +1,536 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#ifndef PCL_ROPS_ESTIMATION_HPP_ +#define PCL_ROPS_ESTIMATION_HPP_ + +#include + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ROPSEstimation ::ROPSEstimation () : + number_of_bins_ (5), + number_of_rotations_ (3), + support_radius_ (1.0f), + sqr_support_radius_ (1.0f), + step_ (22.5f), + triangles_ (0), + triangles_of_the_point_ (0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ROPSEstimation ::~ROPSEstimation () +{ + triangles_.clear (); + triangles_of_the_point_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setNumberOfPartitionBins (unsigned int number_of_bins) +{ + if (number_of_bins != 0) + number_of_bins_ = number_of_bins; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::ROPSEstimation ::getNumberOfPartitionBins () const +{ + return (number_of_bins_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setNumberOfRotations (unsigned int number_of_rotations) +{ + if (number_of_rotations != 0) + { + number_of_rotations_ = number_of_rotations; + step_ = 90.0f / static_cast (number_of_rotations_ + 1); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::ROPSEstimation ::getNumberOfRotations () const +{ + return (number_of_rotations_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setSupportRadius (float support_radius) +{ + if (support_radius > 0.0f) + { + support_radius_ = support_radius; + sqr_support_radius_ = support_radius * support_radius; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::ROPSEstimation ::getSupportRadius () const +{ + return (support_radius_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::setTriangles (const std::vector & triangles) +{ + triangles_ = triangles; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::getTriangles (std::vector & triangles) const +{ + triangles = triangles_; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeFeature (PointCloudOut &output) +{ + if (triangles_.empty ()) + { + output.points.clear (); + return; + } + + buildListOfPointsTriangles (); + + //feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments + unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5; + const auto number_of_points = indices_->size (); + output.points.clear (); + output.points.reserve (number_of_points); + + for (const auto& idx: *indices_) + { + std::set local_triangles; + std::vector local_points; + getLocalSurface (input_->points[idx], local_triangles, local_points); + + Eigen::Matrix3f lrf_matrix; + computeLRF (input_->points[idx], local_triangles, lrf_matrix); + + PointCloudIn transformed_cloud; + transformCloud (input_->points[idx], lrf_matrix, local_points, transformed_cloud); + + std::array axes; + axes[0].x = 1.0f; axes[0].y = 0.0f; axes[0].z = 0.0f; + axes[1].x = 0.0f; axes[1].y = 1.0f; axes[1].z = 0.0f; + axes[2].x = 0.0f; axes[2].y = 0.0f; axes[2].z = 1.0f; + std::vector feature; + for (const auto &axis : axes) + { + float theta = step_; + do + { + //rotate local surface and get bounding box + PointCloudIn rotated_cloud; + Eigen::Vector3f min, max; + rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max); + + //for each projection (XY, XZ and YZ) compute distribution matrix and central moments + for (unsigned int i_proj = 0; i_proj < 3; i_proj++) + { + Eigen::MatrixXf distribution_matrix; + distribution_matrix.resize (number_of_bins_, number_of_bins_); + getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix); + + // TODO remove this needless copy due to API design + std::vector moments; + computeCentralMoments (distribution_matrix, moments); + + feature.insert (feature.end (), moments.begin (), moments.end ()); + } + + theta += step_; + } while (theta < 90.0f); + } + + const float norm = std::accumulate( + feature.cbegin(), feature.cend(), 0.f, [](const auto& sum, const auto& val) { + return sum + std::abs(val); + }); + float invert_norm; + if (norm < std::numeric_limits ::epsilon ()) + invert_norm = 1.0f; + else + invert_norm = 1.0f / norm; + + output.points.emplace_back (); + for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++) + output.points.back().histogram[i_dim] = feature[i_dim] * invert_norm; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::buildListOfPointsTriangles () +{ + triangles_of_the_point_.clear (); + + std::vector dummy; + dummy.reserve (100); + triangles_of_the_point_.resize (surface_->points. size (), dummy); + + for (std::size_t i_triangle = 0; i_triangle < triangles_.size (); i_triangle++) + for (const auto& vertex: triangles_[i_triangle].vertices) + triangles_of_the_point_[vertex].push_back (i_triangle); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::getLocalSurface (const PointInT& point, std::set & local_triangles, std::vector & local_points) const +{ + std::vector distances; + tree_->radiusSearch (point, support_radius_, local_points, distances); + + for (const auto& pt: local_points) + local_triangles.insert (triangles_of_the_point_[pt].begin (), + triangles_of_the_point_[pt].end ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeLRF (const PointInT& point, const std::set & local_triangles, Eigen::Matrix3f& lrf_matrix) const +{ + std::size_t number_of_triangles = local_triangles.size (); + + std::vector > scatter_matrices; + std::vector triangle_area (number_of_triangles), distance_weight (number_of_triangles); + + scatter_matrices.reserve (number_of_triangles); + triangle_area.clear (); + distance_weight.clear (); + + float total_area = 0.0f; + const float coeff = 1.0f / 12.0f; + const float coeff_1_div_3 = 1.0f / 3.0f; + + Eigen::Vector3f feature_point (point.x, point.y, point.z); + + for (const auto& triangle: local_triangles) + { + Eigen::Vector3f pt[3]; + for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++) + { + const unsigned int index = triangles_[triangle].vertices[i_vertex]; + pt[i_vertex] (0) = surface_->points[index].x; + pt[i_vertex] (1) = surface_->points[index].y; + pt[i_vertex] (2) = surface_->points[index].z; + } + + const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm (); + triangle_area.push_back (curr_area); + total_area += curr_area; + + distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f)); + + Eigen::Matrix3f curr_scatter_matrix; + curr_scatter_matrix.setZero (); + for (const auto &i_pt : pt) + { + Eigen::Vector3f vec = i_pt - feature_point; + curr_scatter_matrix += vec * (vec.transpose ()); + for (const auto &j_pt : pt) + curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ()); + } + scatter_matrices.emplace_back (coeff * curr_scatter_matrix); + } + + if (std::abs (total_area) < std::numeric_limits ::epsilon ()) + total_area = 1.0f / total_area; + else + total_area = 1.0f; + + Eigen::Matrix3f overall_scatter_matrix; + overall_scatter_matrix.setZero (); + std::vector total_weight (number_of_triangles); + const float denominator = 1.0f / 6.0f; + for (std::size_t i_triangle = 0; i_triangle < number_of_triangles; i_triangle++) + { + const float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area; + overall_scatter_matrix += factor * scatter_matrices[i_triangle]; + total_weight[i_triangle] = factor * denominator; + } + + Eigen::Vector3f v1, v2, v3; + computeEigenVectors (overall_scatter_matrix, v1, v2, v3); + + float h1 = 0.0f; + float h3 = 0.0f; + std::size_t i_triangle = 0; + for (const auto& triangle: local_triangles) + { + Eigen::Vector3f pt[3]; + for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++) + { + const unsigned int index = triangles_[triangle].vertices[i_vertex]; + pt[i_vertex] (0) = surface_->points[index].x; + pt[i_vertex] (1) = surface_->points[index].y; + pt[i_vertex] (2) = surface_->points[index].z; + } + + float factor1 = 0.0f; + float factor3 = 0.0f; + for (const auto &i_pt : pt) + { + Eigen::Vector3f vec = i_pt - feature_point; + factor1 += vec.dot (v1); + factor3 += vec.dot (v3); + } + h1 += total_weight[i_triangle] * factor1; + h3 += total_weight[i_triangle] * factor3; + i_triangle++; + } + + if (h1 < 0.0f) v1 = -v1; + if (h3 < 0.0f) v3 = -v3; + + v2 = v3.cross (v1); + + lrf_matrix.row (0) = v1; + lrf_matrix.row (1) = v2; + lrf_matrix.row (2) = v3; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeEigenVectors (const Eigen::Matrix3f& matrix, + Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis) const +{ + Eigen::EigenSolver eigen_solver; + eigen_solver.compute (matrix); + + Eigen::EigenSolver ::EigenvectorsType eigen_vectors; + Eigen::EigenSolver ::EigenvalueType eigen_values; + eigen_vectors = eigen_solver.eigenvectors (); + eigen_values = eigen_solver.eigenvalues (); + + unsigned int temp = 0; + unsigned int major_index = 0; + unsigned int middle_index = 1; + unsigned int minor_index = 2; + + if (eigen_values.real () (major_index) < eigen_values.real () (middle_index)) + { + temp = major_index; + major_index = middle_index; + middle_index = temp; + } + + if (eigen_values.real () (major_index) < eigen_values.real () (minor_index)) + { + temp = major_index; + major_index = minor_index; + minor_index = temp; + } + + if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index)) + { + temp = minor_index; + minor_index = middle_index; + middle_index = temp; + } + + major_axis = eigen_vectors.col (major_index).real (); + middle_axis = eigen_vectors.col (middle_index).real (); + minor_axis = eigen_vectors.col (minor_index).real (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector & local_points, PointCloudIn& transformed_cloud) const +{ + const auto number_of_points = local_points.size (); + transformed_cloud.points.clear (); + transformed_cloud.points.reserve (number_of_points); + + for (const auto& idx: local_points) + { + Eigen::Vector3f transformed_point (surface_->points[idx].x - point.x, + surface_->points[idx].y - point.y, + surface_->points[idx].z - point.z); + + transformed_point = matrix * transformed_point; + + PointInT new_point; + new_point.x = transformed_point (0); + new_point.y = transformed_point (1); + new_point.z = transformed_point (2); + transformed_cloud.points.emplace_back (new_point); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max) const +{ + Eigen::Matrix3f rotation_matrix; + const float x = axis.x; + const float y = axis.y; + const float z = axis.z; + const float rad = M_PI / 180.0f; + const float cosine = std::cos (angle * rad); + const float sine = std::sin (angle * rad); + rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y, + (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x, + (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z; + + const auto number_of_points = cloud.points.size (); + + rotated_cloud.header = cloud.header; + rotated_cloud.width = number_of_points; + rotated_cloud.height = 1; + rotated_cloud.points.clear (); + rotated_cloud.points.reserve (number_of_points); + + min (0) = std::numeric_limits ::max (); + min (1) = std::numeric_limits ::max (); + min (2) = std::numeric_limits ::max (); + max (0) = -std::numeric_limits ::max (); + max (1) = -std::numeric_limits ::max (); + max (2) = -std::numeric_limits ::max (); + + for (const auto& pt: cloud.points) + { + Eigen::Vector3f point (pt.x, pt.y, pt.z); + point = rotation_matrix * point; + + PointInT rotated_point; + rotated_point.x = point (0); + rotated_point.y = point (1); + rotated_point.z = point (2); + rotated_cloud.points.emplace_back (rotated_point); + + for (int i = 0; i < 3; ++i) + { + min(i) = std::min(min(i), point(i)); + max(i) = std::max(max(i), point(i)); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const +{ + matrix.setZero (); + + const unsigned int coord[3][2] = { + {0, 1}, + {0, 2}, + {1, 2}}; + + const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_; + const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_; + + for (const auto& pt: cloud.points) + { + Eigen::Vector3f point (pt.x, pt.y, pt.z); + + const float u_length = point (coord[projection][0]) - min[coord[projection][0]]; + const float v_length = point (coord[projection][1]) - min[coord[projection][1]]; + + const float u_ratio = u_length / u_bin_length; + unsigned int row = static_cast (u_ratio); + if (row == number_of_bins_) row--; + + const float v_ratio = v_length / v_bin_length; + unsigned int col = static_cast (v_ratio); + if (col == number_of_bins_) col--; + + matrix (row, col) += 1.0f; + } + + matrix /= std::max (1, cloud.points.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ROPSEstimation ::computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector & moments) const +{ + float mean_i = 0.0f; + float mean_j = 0.0f; + + for (unsigned int i = 0; i < number_of_bins_; i++) + for (unsigned int j = 0; j < number_of_bins_; j++) + { + const float m = matrix (i, j); + mean_i += static_cast (i + 1) * m; + mean_j += static_cast (j + 1) * m; + } + + const unsigned int number_of_moments_to_compute = 4; + const float power[number_of_moments_to_compute][2] = { + {1.0f, 1.0f}, + {2.0f, 1.0f}, + {1.0f, 2.0f}, + {2.0f, 2.0f}}; + + float entropy = 0.0f; + moments.resize (number_of_moments_to_compute + 1, 0.0f); + for (unsigned int i = 0; i < number_of_bins_; i++) + { + const float i_factor = static_cast (i + 1) - mean_i; + for (unsigned int j = 0; j < number_of_bins_; j++) + { + const float j_factor = static_cast (j + 1) - mean_j; + const float m = matrix (i, j); + if (m > 0.0f) + entropy -= m * std::log (m); + for (unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++) + moments[i_moment] += std::pow (i_factor, power[i_moment][0]) * std::pow (j_factor, power[i_moment][1]) * m; + } + } + + moments[number_of_moments_to_compute] = entropy; +} + +#endif // PCL_ROPS_ESTIMATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/rsd.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/rsd.hpp new file mode 100755 index 0000000..f71470a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/rsd.hpp @@ -0,0 +1,295 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_RSD_H_ +#define PCL_FEATURES_IMPL_RSD_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::MatrixXf +pcl::computeRSD (const pcl::PointCloud &surface, const pcl::PointCloud &normals, + const std::vector &indices, double max_dist, + int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram) +{ + // Check if the full histogram has to be saved or not + Eigen::MatrixXf histogram; + if (compute_histogram) + histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv); + + // Check if enough points are provided or not + if (indices.size () < 2) + { + radii.r_max = 0; + radii.r_min = 0; + return histogram; + } + + // Initialize minimum and maximum angle values in each distance bin + std::vector > min_max_angle_by_dist (nr_subdiv); + for (auto& minmax: min_max_angle_by_dist) + { + minmax.resize (2); + minmax[0] = std::numeric_limits::max(); + minmax[1] = -std::numeric_limits::max(); + } + min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0; + + // Compute distance by normal angle distribution for points + std::vector::const_iterator i, begin (indices.begin()), end (indices.end()); + for (i = begin+1; i != end; ++i) + { + // compute angle between the two lines going through normals (disregard orientation!) + double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] + + normals.points[*i].normal[1] * normals.points[*begin].normal[1] + + normals.points[*i].normal[2] * normals.points[*begin].normal[2]; + if (cosine > 1) cosine = 1; + if (cosine < -1) cosine = -1; + double angle = std::acos (cosine); + if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected! + + // Compute point to point distance + double dist = sqrt ((surface.points[*i].x - surface.points[*begin].x) * (surface.points[*i].x - surface.points[*begin].x) + + (surface.points[*i].y - surface.points[*begin].y) * (surface.points[*i].y - surface.points[*begin].y) + + (surface.points[*i].z - surface.points[*begin].z) * (surface.points[*i].z - surface.points[*begin].z)); + + if (dist > max_dist) + continue; /// \note: we neglect points that are outside the specified interval! + + // compute bins and increase + int bin_d = static_cast (std::floor (nr_subdiv * dist / max_dist)); + if (compute_histogram) + { + int bin_a = std::min (nr_subdiv-1, static_cast (std::floor (nr_subdiv * angle / (M_PI/2)))); + histogram(bin_a, bin_d)++; + } + + // update min-max values for distance bins + min_max_angle_by_dist[bin_d][0] = std::min(angle, min_max_angle_by_dist[bin_d][0]); + min_max_angle_by_dist[bin_d][1] = std::max(angle, min_max_angle_by_dist[bin_d][1]); + } + + // Estimate radius from min and max lines + double Amint_Amin = 0, Amint_d = 0; + double Amaxt_Amax = 0, Amaxt_d = 0; + for (int di=0; di= 0) + { + double p_min = min_max_angle_by_dist[di][0]; + double p_max = min_max_angle_by_dist[di][1]; + double f = (di+0.5)*max_dist/nr_subdiv; + Amint_Amin += p_min * p_min; + Amint_d += p_min * f; + Amaxt_Amax += p_max * p_max; + Amaxt_d += p_max * f; + } + } + float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius)); + float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius)); + + // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5) + min_radius *= 1.1f; + max_radius *= 0.9f; + if (min_radius < max_radius) + { + radii.r_min = min_radius; + radii.r_max = max_radius; + } + else + { + radii.r_max = min_radius; + radii.r_min = max_radius; + } + + return histogram; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::MatrixXf +pcl::computeRSD (const pcl::PointCloud &normals, + const std::vector &indices, const std::vector &sqr_dists, double max_dist, + int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram) +{ + // Check if the full histogram has to be saved or not + Eigen::MatrixXf histogram; + if (compute_histogram) + histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv); + + // Check if enough points are provided or not + if (indices.size () < 2) + { + radii.r_max = 0; + radii.r_min = 0; + return histogram; + } + + // Initialize minimum and maximum angle values in each distance bin + std::vector > min_max_angle_by_dist (nr_subdiv); + min_max_angle_by_dist[0].resize (2); + min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0; + for (int di=1; di::const_iterator i, begin (indices.begin()), end (indices.end()); + for (i = begin+1; i != end; ++i) + { + // compute angle between the two lines going through normals (disregard orientation!) + double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] + + normals.points[*i].normal[1] * normals.points[*begin].normal[1] + + normals.points[*i].normal[2] * normals.points[*begin].normal[2]; + if (cosine > 1) cosine = 1; + if (cosine < -1) cosine = -1; + double angle = std::acos (cosine); + if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected! + + // Compute point to point distance + double dist = sqrt (sqr_dists[i-begin]); + + if (dist > max_dist) + continue; /// \note: we neglect points that are outside the specified interval! + + // compute bins and increase + int bin_d = static_cast (std::floor (nr_subdiv * dist / max_dist)); + if (compute_histogram) + { + int bin_a = std::min (nr_subdiv-1, static_cast (std::floor (nr_subdiv * angle / (M_PI/2)))); + histogram(bin_a, bin_d)++; + } + + // update min-max values for distance bins + if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle; + if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle; + } + + // Estimate radius from min and max lines + double Amint_Amin = 0, Amint_d = 0; + double Amaxt_Amax = 0, Amaxt_d = 0; + for (int di=0; di= 0) + { + double p_min = min_max_angle_by_dist[di][0]; + double p_max = min_max_angle_by_dist[di][1]; + double f = (di+0.5)*max_dist/nr_subdiv; + Amint_Amin += p_min * p_min; + Amint_d += p_min * f; + Amaxt_Amax += p_max * p_max; + Amaxt_d += p_max * f; + } + } + float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius)); + float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius)); + + // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5) + min_radius *= 1.1f; + max_radius *= 0.9f; + if (min_radius < max_radius) + { + radii.r_min = min_radius; + radii.r_max = max_radius; + } + else + { + radii.r_max = min_radius; + radii.r_min = max_radius; + } + + return histogram; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RSDEstimation::computeFeature (PointCloudOut &output) +{ + // Check if search_radius_ was set + if (search_radius_ < 0) + { + PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + // List of indices and corresponding squared distances for a neighborhood + // \note resize is irrelevant for a radiusSearch (). + std::vector nn_indices; + std::vector nn_sqr_dists; + + // Check if the full histogram has to be saved or not + if (save_histograms_) + { + // Reserve space for the output histogram dataset + histograms_.reset (new std::vector >); + histograms_->reserve (output.points.size ()); + + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + // Compute and store r_min and r_max in the output cloud + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists); + //histograms_->push_back (computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true)); + histograms_->push_back (computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true)); + } + } + else + { + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + // Compute and store r_min and r_max in the output cloud + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists); + //computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false); + computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false); + } + } +} + +#define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation; + +#endif // PCL_FEATURES_IMPL_RSD_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/shot.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/shot.hpp new file mode 100755 index 0000000..1e75714 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/shot.hpp @@ -0,0 +1,900 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_FEATURES_IMPL_SHOT_H_ +#define PCL_FEATURES_IMPL_SHOT_H_ + +#include +#include +#include + +// Useful constants. +#define PST_PI 3.1415926535897932384626433832795 +#define PST_RAD_45 0.78539816339744830961566084581988 +#define PST_RAD_90 1.5707963267948966192313216916398 +#define PST_RAD_135 2.3561944901923449288469825374596 +#define PST_RAD_180 PST_PI +#define PST_RAD_360 6.283185307179586476925286766558 +#define PST_RAD_PI_7_8 2.7488935718910690836548129603691 + +const double zeroDoubleEps15 = 1E-15; +const float zeroFloatEps8 = 1E-8f; + +////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Check if val1 and val2 are equals. + * + * \param[in] val1 first number to check. + * \param[in] val2 second number to check. + * \param[in] zeroDoubleEps epsilon + * \return true if val1 is equal to val2, false otherwise. + */ +inline bool +areEquals (double val1, double val2, double zeroDoubleEps = zeroDoubleEps15) +{ + return (std::abs (val1 - val2) float +pcl::SHOTColorEstimation::sRGB_LUT[256] = {- 1}; + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::SHOTColorEstimation::sXYZ_LUT[4000] = {- 1}; + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTColorEstimation::RGB2CIELAB (unsigned char R, unsigned char G, + unsigned char B, float &L, float &A, + float &B2) +{ + // @TODO: C++17 supports constexpr lambda for compile time init + if (sRGB_LUT[0] < 0) + { + for (int i = 0; i < 256; i++) + { + float f = static_cast (i) / 255.0f; + if (f > 0.04045) + sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f); + else + sRGB_LUT[i] = f / 12.92f; + } + + for (int i = 0; i < 4000; i++) + { + float f = static_cast (i) / 4000.0f; + if (f > 0.008856) + sXYZ_LUT[i] = static_cast (powf (f, 0.3333f)); + else + sXYZ_LUT[i] = static_cast((7.787 * f) + (16.0 / 116.0)); + } + } + + float fr = sRGB_LUT[R]; + float fg = sRGB_LUT[G]; + float fb = sRGB_LUT[B]; + + // Use white = D65 + const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f; + const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f; + const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f; + + float vx = x / 0.95047f; + float vy = y; + float vz = z / 1.08883f; + + vx = sXYZ_LUT[int(vx*4000)]; + vy = sXYZ_LUT[int(vy*4000)]; + vz = sXYZ_LUT[int(vz*4000)]; + + L = 116.0f * vy - 16.0f; + if (L > 100) + L = 100.0f; + + A = 500.0f * (vx - vy); + if (A > 120) + A = 120.0f; + else if (A <- 120) + A = -120.0f; + + B2 = 200.0f * (vy - vz); + if (B2 > 120) + B2 = 120.0f; + else if (B2<- 120) + B2 = -120.0f; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SHOTEstimationBase::initCompute () +{ + if (!FeatureFromNormals::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // SHOT cannot work with k-search + if (this->getKSearch () != 0) + { + PCL_ERROR( + "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", + getClassName().c_str ()); + return (false); + } + + // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation + typename SHOTLocalReferenceFrameEstimation::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation()); + lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_)); + lrf_estimator->setInputCloud (input_); + lrf_estimator->setIndices (indices_); + if (!fake_surface_) + lrf_estimator->setSearchSurface(surface_); + + if (!FeatureWithLocalReferenceFrames::initLocalReferenceFrames (indices_->size (), lrf_estimator)) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTEstimationBase::createBinDistanceShape ( + int index, + const std::vector &indices, + std::vector &bin_distance_shape) +{ + bin_distance_shape.resize (indices.size ()); + + const PointRFT& current_frame = frames_->points[index]; + //if (!std::isfinite (current_frame.rf[0]) || !std::isfinite (current_frame.rf[4]) || !std::isfinite (current_frame.rf[11])) + //return; + + Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0); + + unsigned nan_counter = 0; + for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx) + { + // check NaN normal + const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap (); + if (!std::isfinite (normal_vec[0]) || + !std::isfinite (normal_vec[1]) || + !std::isfinite (normal_vec[2])) + { + bin_distance_shape[i_idx] = std::numeric_limits::quiet_NaN (); + ++nan_counter; + } else + { + //double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2]; + double cosineDesc = normal_vec.dot (current_frame_z); + + if (cosineDesc > 1.0) + cosineDesc = 1.0; + if (cosineDesc < - 1.0) + cosineDesc = - 1.0; + + bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2; + } + } + if (nan_counter > 0) + PCL_WARN ("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n", + getClassName ().c_str (), index, nan_counter, (static_cast(nan_counter)*100.f/static_cast(indices.size ()))); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTEstimationBase::normalizeHistogram ( + Eigen::VectorXf &shot, int desc_length) +{ + // Normalization is performed by considering the L2 norm + // and not the sum of bins, as reported in the ECCV paper. + // This is due to additional experiments performed by the authors after its pubblication, + // where L2 normalization turned out better at handling point density variations. + double acc_norm = 0; + for (int j = 0; j < desc_length; j++) + acc_norm += shot[j] * shot[j]; + acc_norm = sqrt (acc_norm); + for (int j = 0; j < desc_length; j++) + shot[j] /= static_cast (acc_norm); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTEstimationBase::interpolateSingleChannel ( + const std::vector &indices, + const std::vector &sqr_dists, + const int index, + std::vector &binDistance, + const int nr_bins, + Eigen::VectorXf &shot) +{ + const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap (); + const PointRFT& current_frame = (*frames_)[index]; + + Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0); + Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0); + Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0); + + for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx) + { + if (!std::isfinite(binDistance[i_idx])) + continue; + + Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point; + delta[3] = 0; + + // Compute the Euclidean norm + double distance = sqrt (sqr_dists[i_idx]); + + if (areEquals (distance, 0.0)) + continue; + + double xInFeatRef = delta.dot (current_frame_x); + double yInFeatRef = delta.dot (current_frame_y); + double zInFeatRef = delta.dot (current_frame_z); + + // To avoid numerical problems afterwards + if (std::abs (yInFeatRef) < 1E-30) + yInFeatRef = 0; + if (std::abs (xInFeatRef) < 1E-30) + xInFeatRef = 0; + if (std::abs (zInFeatRef) < 1E-30) + zInFeatRef = 0; + + + unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0; + unsigned char bit3 = static_cast (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4); + + assert (bit3 == 0 || bit3 == 1); + + int desc_index = (bit4<<3) + (bit3<<2); + + desc_index = desc_index << 1; + + if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0)) + desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4; + else + desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0; + + desc_index += zInFeatRef > 0 ? 1 : 0; + + // 2 RADII + desc_index += (distance > radius1_2_) ? 2 : 0; + + int step_index = static_cast(std::floor (binDistance[i_idx] +0.5)); + int volume_index = desc_index * (nr_bins+1); + + //Interpolation on the cosine (adjacent bins in the histogram) + binDistance[i_idx] -= step_index; + double intWeight = (1- std::abs (binDistance[i_idx])); + + if (binDistance[i_idx] > 0) + shot[volume_index + ((step_index+1) % nr_bins)] += static_cast (binDistance[i_idx]); + else + shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - static_cast (binDistance[i_idx]); + + //Interpolation on the distance (adjacent husks) + + if (distance > radius1_2_) //external sphere + { + double radiusDistance = (distance - radius3_4_) / radius1_2_; + + if (distance > radius3_4_) //most external sector, votes only for itself + intWeight += 1 - radiusDistance; //peso=1-d + else //3/4 of radius, votes also for the internal sphere + { + intWeight += 1 + radiusDistance; + shot[(desc_index - 2) * (nr_bins+1) + step_index] -= static_cast (radiusDistance); + } + } + else //internal sphere + { + double radiusDistance = (distance - radius1_4_) / radius1_2_; + + if (distance < radius1_4_) //most internal sector, votes only for itself + intWeight += 1 + radiusDistance; //weight=1-d + else //3/4 of radius, votes also for the external sphere + { + intWeight += 1 - radiusDistance; + shot[(desc_index + 2) * (nr_bins+1) + step_index] += static_cast (radiusDistance); + } + } + + //Interpolation on the inclination (adjacent vertical volumes) + double inclinationCos = zInFeatRef / distance; + if (inclinationCos < - 1.0) + inclinationCos = - 1.0; + if (inclinationCos > 1.0) + inclinationCos = 1.0; + + double inclination = std::acos (inclinationCos); + + assert (inclination >= 0.0 && inclination <= PST_RAD_180); + + if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0)) + { + double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90; + if (inclination > PST_RAD_135) + intWeight += 1 - inclinationDistance; + else + { + intWeight += 1 + inclinationDistance; + assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_); + shot[(desc_index + 1) * (nr_bins+1) + step_index] -= static_cast (inclinationDistance); + } + } + else + { + double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90; + if (inclination < PST_RAD_45) + intWeight += 1 + inclinationDistance; + else + { + intWeight += 1 - inclinationDistance; + assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_); + shot[(desc_index - 1) * (nr_bins+1) + step_index] += static_cast (inclinationDistance); + } + } + + if (yInFeatRef != 0.0 || xInFeatRef != 0.0) + { + //Interpolation on the azimuth (adjacent horizontal volumes) + double azimuth = std::atan2 (yInFeatRef, xInFeatRef); + + int sel = desc_index >> 2; + double angularSectorSpan = PST_RAD_45; + double angularSectorStart = - PST_RAD_PI_7_8; + + double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan; + + assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5))); + + azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5)); + + if (azimuthDistance > 0) + { + intWeight += 1 - azimuthDistance; + int interp_index = (desc_index + 4) % maxAngularSectors_; + assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_); + shot[interp_index * (nr_bins+1) + step_index] += static_cast (azimuthDistance); + } + else + { + int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_; + assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_); + intWeight += 1 + azimuthDistance; + shot[interp_index * (nr_bins+1) + step_index] -= static_cast (azimuthDistance); + } + + } + + assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_); + shot[volume_index + step_index] += static_cast (intWeight); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTColorEstimation::interpolateDoubleChannel ( + const std::vector &indices, + const std::vector &sqr_dists, + const int index, + std::vector &binDistanceShape, + std::vector &binDistanceColor, + const int nr_bins_shape, + const int nr_bins_color, + Eigen::VectorXf &shot) +{ + const Eigen::Vector4f ¢ral_point = (*input_)[(*indices_)[index]].getVector4fMap (); + const PointRFT& current_frame = (*frames_)[index]; + + int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1); + + Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0); + Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0); + Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0); + + for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx) + { + if (!std::isfinite(binDistanceShape[i_idx])) + continue; + + Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point; + delta[3] = 0; + + // Compute the Euclidean norm + double distance = sqrt (sqr_dists[i_idx]); + + if (areEquals (distance, 0.0)) + continue; + + double xInFeatRef = delta.dot (current_frame_x); + double yInFeatRef = delta.dot (current_frame_y); + double zInFeatRef = delta.dot (current_frame_z); + + // To avoid numerical problems afterwards + if (std::abs (yInFeatRef) < 1E-30) + yInFeatRef = 0; + if (std::abs (xInFeatRef) < 1E-30) + xInFeatRef = 0; + if (std::abs (zInFeatRef) < 1E-30) + zInFeatRef = 0; + + unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0; + unsigned char bit3 = static_cast (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4); + + assert (bit3 == 0 || bit3 == 1); + + int desc_index = (bit4<<3) + (bit3<<2); + + desc_index = desc_index << 1; + + if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0)) + desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4; + else + desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0; + + desc_index += zInFeatRef > 0 ? 1 : 0; + + // 2 RADII + desc_index += (distance > radius1_2_) ? 2 : 0; + + int step_index_shape = static_cast(std::floor (binDistanceShape[i_idx] +0.5)); + int step_index_color = static_cast(std::floor (binDistanceColor[i_idx] +0.5)); + + int volume_index_shape = desc_index * (nr_bins_shape+1); + int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1); + + //Interpolation on the cosine (adjacent bins in the histrogram) + binDistanceShape[i_idx] -= step_index_shape; + binDistanceColor[i_idx] -= step_index_color; + + double intWeightShape = (1- std::abs (binDistanceShape[i_idx])); + double intWeightColor = (1- std::abs (binDistanceColor[i_idx])); + + if (binDistanceShape[i_idx] > 0) + shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += static_cast (binDistanceShape[i_idx]); + else + shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= static_cast (binDistanceShape[i_idx]); + + if (binDistanceColor[i_idx] > 0) + shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += static_cast (binDistanceColor[i_idx]); + else + shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= static_cast (binDistanceColor[i_idx]); + + //Interpolation on the distance (adjacent husks) + + if (distance > radius1_2_) //external sphere + { + double radiusDistance = (distance - radius3_4_) / radius1_2_; + + if (distance > radius3_4_) //most external sector, votes only for itself + { + intWeightShape += 1 - radiusDistance; //weight=1-d + intWeightColor += 1 - radiusDistance; //weight=1-d + } + else //3/4 of radius, votes also for the internal sphere + { + intWeightShape += 1 + radiusDistance; + intWeightColor += 1 + radiusDistance; + shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= static_cast (radiusDistance); + shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= static_cast (radiusDistance); + } + } + else //internal sphere + { + double radiusDistance = (distance - radius1_4_) / radius1_2_; + + if (distance < radius1_4_) //most internal sector, votes only for itself + { + intWeightShape += 1 + radiusDistance; + intWeightColor += 1 + radiusDistance; //weight=1-d + } + else //3/4 of radius, votes also for the external sphere + { + intWeightShape += 1 - radiusDistance; //weight=1-d + intWeightColor += 1 - radiusDistance; //weight=1-d + shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += static_cast (radiusDistance); + shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += static_cast (radiusDistance); + } + } + + //Interpolation on the inclination (adjacent vertical volumes) + double inclinationCos = zInFeatRef / distance; + if (inclinationCos < - 1.0) + inclinationCos = - 1.0; + if (inclinationCos > 1.0) + inclinationCos = 1.0; + + double inclination = std::acos (inclinationCos); + + assert (inclination >= 0.0 && inclination <= PST_RAD_180); + + if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0)) + { + double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90; + if (inclination > PST_RAD_135) + { + intWeightShape += 1 - inclinationDistance; + intWeightColor += 1 - inclinationDistance; + } + else + { + intWeightShape += 1 + inclinationDistance; + intWeightColor += 1 + inclinationDistance; + assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_); + assert (shapeToColorStride + (desc_index + 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color < descLength_); + shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= static_cast (inclinationDistance); + shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= static_cast (inclinationDistance); + } + } + else + { + double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90; + if (inclination < PST_RAD_45) + { + intWeightShape += 1 + inclinationDistance; + intWeightColor += 1 + inclinationDistance; + } + else + { + intWeightShape += 1 - inclinationDistance; + intWeightColor += 1 - inclinationDistance; + assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_); + assert (shapeToColorStride + (desc_index - 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color < descLength_); + shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += static_cast (inclinationDistance); + shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += static_cast (inclinationDistance); + } + } + + if (yInFeatRef != 0.0 || xInFeatRef != 0.0) + { + //Interpolation on the azimuth (adjacent horizontal volumes) + double azimuth = std::atan2 (yInFeatRef, xInFeatRef); + + int sel = desc_index >> 2; + double angularSectorSpan = PST_RAD_45; + double angularSectorStart = - PST_RAD_PI_7_8; + + double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan; + assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5))); + azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5)); + + if (azimuthDistance > 0) + { + intWeightShape += 1 - azimuthDistance; + intWeightColor += 1 - azimuthDistance; + int interp_index = (desc_index + 4) % maxAngularSectors_; + assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_); + assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_); + shot[interp_index * (nr_bins_shape+1) + step_index_shape] += static_cast (azimuthDistance); + shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast (azimuthDistance); + } + else + { + int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_; + intWeightShape += 1 + azimuthDistance; + intWeightColor += 1 + azimuthDistance; + assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_); + assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_); + shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= static_cast (azimuthDistance); + shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast (azimuthDistance); + } + } + + assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_); + assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_); + shot[volume_index_shape + step_index_shape] += static_cast (intWeightShape); + shot[volume_index_color + step_index_color] += static_cast (intWeightColor); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTColorEstimation::computePointSHOT ( + const int index, const std::vector &indices, const std::vector &sqr_dists, Eigen::VectorXf &shot) +{ + // Clear the resultant shot + shot.setZero (); + const auto nNeighbors = indices.size (); + //Skip the current feature if the number of its neighbors is not sufficient for its description + if (nNeighbors < 5) + { + PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", + getClassName ().c_str (), (*indices_)[index]); + + shot.setConstant(descLength_, 1, std::numeric_limits::quiet_NaN () ); + + return; + } + + //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram + std::vector binDistanceShape; + if (b_describe_shape_) + { + this->createBinDistanceShape (index, indices, binDistanceShape); + } + + //If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram + std::vector binDistanceColor; + if (b_describe_color_) + { + binDistanceColor.reserve (nNeighbors); + + //unsigned char redRef = input_->points[(*indices_)[index]].rgba >> 16 & 0xFF; + //unsigned char greenRef = input_->points[(*indices_)[index]].rgba >> 8& 0xFF; + //unsigned char blueRef = input_->points[(*indices_)[index]].rgba & 0xFF; + unsigned char redRef = input_->points[(*indices_)[index]].r; + unsigned char greenRef = input_->points[(*indices_)[index]].g; + unsigned char blueRef = input_->points[(*indices_)[index]].b; + + float LRef, aRef, bRef; + + RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef); + LRef /= 100.0f; + aRef /= 120.0f; + bRef /= 120.0f; //normalized LAB components (0points[idx].r; + unsigned char green = surface_->points[idx].g; + unsigned char blue = surface_->points[idx].b; + + float L, a, b; + + RGB2CIELAB (red, green, blue, L, a, b); + L /= 100.0f; + a /= 120.0f; + b /= 120.0f; //normalized LAB components (0 1.0) + colorDistance = 1.0; + if (colorDistance < 0.0) + colorDistance = 0.0; + + binDistanceColor.push_back (colorDistance * nr_color_bins_); + } + } + + //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s) + + if (b_describe_shape_ && b_describe_color_) + interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor, + nr_shape_bins_, nr_color_bins_, + shot); + else if (b_describe_color_) + interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot); + else + interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot); + + // Normalize the final histogram + this->normalizeHistogram (shot, descLength_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTEstimation::computePointSHOT ( + const int index, const std::vector &indices, const std::vector &sqr_dists, Eigen::VectorXf &shot) +{ + //Skip the current feature if the number of its neighbors is not sufficient for its description + if (indices.size () < 5) + { + PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", + getClassName ().c_str (), (*indices_)[index]); + + shot.setConstant(descLength_, 1, std::numeric_limits::quiet_NaN () ); + + return; + } + + // Clear the resultant shot + std::vector binDistanceShape; + this->createBinDistanceShape (index, indices, binDistanceShape); + + // Interpolate + shot.setZero (); + interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot); + + // Normalize the final histogram + this->normalizeHistogram (shot, descLength_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTEstimation::computeFeature (pcl::PointCloud &output) +{ + descLength_ = nr_grid_sector_ * (nr_shape_bins_+1); + + sqradius_ = search_radius_ * search_radius_; + radius3_4_ = (search_radius_*3) / 4; + radius1_4_ = search_radius_ / 4; + radius1_2_ = search_radius_ / 2; + + assert(descLength_ == 352); + + shot_.setZero (descLength_); + + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + output.is_dense = true; + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + bool lrf_is_nan = false; + const PointRFT& current_frame = (*frames_)[idx]; + if (!std::isfinite (current_frame.x_axis[0]) || + !std::isfinite (current_frame.y_axis[0]) || + !std::isfinite (current_frame.z_axis[0])) + { + PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", + getClassName ().c_str (), (*indices_)[idx]); + lrf_is_nan = true; + } + + if (!isFinite ((*input_)[(*indices_)[idx]]) || + lrf_is_nan || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + // Copy into the resultant cloud + for (int d = 0; d < descLength_; ++d) + output.points[idx].descriptor[d] = std::numeric_limits::quiet_NaN (); + for (int d = 0; d < 9; ++d) + output.points[idx].rf[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // Estimate the SHOT descriptor at each patch + computePointSHOT (static_cast (idx), nn_indices, nn_dists, shot_); + + // Copy into the resultant cloud + for (int d = 0; d < descLength_; ++d) + output.points[idx].descriptor[d] = shot_[d]; + for (int d = 0; d < 3; ++d) + { + output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; + output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; + output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTColorEstimation::computeFeature (pcl::PointCloud &output) +{ + // Compute the current length of the descriptor + descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0; + descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0; + + assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) || + (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) || + (b_describe_color_ && b_describe_shape_ && descLength_ == 1344) + ); + + // Useful values + sqradius_ = search_radius_*search_radius_; + radius3_4_ = (search_radius_*3) / 4; + radius1_4_ = search_radius_ / 4; + radius1_2_ = search_radius_ / 2; + + shot_.setZero (descLength_); + + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + output.is_dense = true; + // Iterating over the entire index vector + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + bool lrf_is_nan = false; + const PointRFT& current_frame = (*frames_)[idx]; + if (!std::isfinite (current_frame.x_axis[0]) || + !std::isfinite (current_frame.y_axis[0]) || + !std::isfinite (current_frame.z_axis[0])) + { + PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", + getClassName ().c_str (), (*indices_)[idx]); + lrf_is_nan = true; + } + + if (!isFinite ((*input_)[(*indices_)[idx]]) || + lrf_is_nan || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + // Copy into the resultant cloud + for (int d = 0; d < descLength_; ++d) + output.points[idx].descriptor[d] = std::numeric_limits::quiet_NaN (); + for (int d = 0; d < 9; ++d) + output.points[idx].rf[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // Compute the SHOT descriptor for the current 3D feature + computePointSHOT (static_cast (idx), nn_indices, nn_dists, shot_); + + // Copy into the resultant cloud + for (int d = 0; d < descLength_; ++d) + output.points[idx].descriptor[d] = shot_[d]; + for (int d = 0; d < 3; ++d) + { + output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; + output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; + output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; + } + } +} + +#define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase; +#define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation; +#define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation; + +#endif // PCL_FEATURES_IMPL_SHOT_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/shot_lrf.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/shot_lrf.hpp new file mode 100755 index 0000000..c49031a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/shot_lrf.hpp @@ -0,0 +1,206 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_SHOT_LRF_H_ +#define PCL_FEATURES_IMPL_SHOT_LRF_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix +template float +pcl::SHOTLocalReferenceFrameEstimation::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf) +{ + const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap (); + std::vector n_indices; + std::vector n_sqr_distances; + + this->searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances); + + Eigen::Matrix vij (n_indices.size (), 4); + + Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero (); + + double distance = 0.0; + double sum = 0.0; + + int valid_nn_points = 0; + + for (std::size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx) + { + Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap (); + if (pt.head<3> () == central_point.head<3> ()) + continue; + + // Difference between current point and origin + vij.row (valid_nn_points).matrix () = (pt - central_point).cast (); + vij (valid_nn_points, 3) = 0; + + distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]); + + // Multiply vij * vij' + cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ()); + + sum += distance; + valid_nn_points++; + } + + if (valid_nn_points < 5) + { + //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]); + rf.setConstant (std::numeric_limits::quiet_NaN ()); + + return (std::numeric_limits::max ()); + } + + cov_m /= sum; + + Eigen::SelfAdjointEigenSolver solver (cov_m); + + const double& e1c = solver.eigenvalues ()[0]; + const double& e2c = solver.eigenvalues ()[1]; + const double& e3c = solver.eigenvalues ()[2]; + + if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c)) + { + //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]); + rf.setConstant (std::numeric_limits::quiet_NaN ()); + + return (std::numeric_limits::max ()); + } + + // Disambiguation + Eigen::Vector4d v1 = Eigen::Vector4d::Zero (); + Eigen::Vector4d v3 = Eigen::Vector4d::Zero (); + v1.head<3> ().matrix () = solver.eigenvectors ().col (2); + v3.head<3> ().matrix () = solver.eigenvectors ().col (0); + + int plusNormal = 0, plusTangentDirection1=0; + for (int ne = 0; ne < valid_nn_points; ne++) + { + double dp = vij.row (ne).dot (v1); + if (dp >= 0) + plusTangentDirection1++; + + dp = vij.row (ne).dot (v3); + if (dp >= 0) + plusNormal++; + } + + //TANGENT + plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points; + if (plusTangentDirection1 == 0) + { + int points = 5; //std::min(valid_nn_points*2/2+1, 11); + int medianIndex = valid_nn_points/2; + + for (int i = -points/2; i <= points/2; i++) + if ( vij.row (medianIndex - i).dot (v1) > 0) + plusTangentDirection1 ++; + + if (plusTangentDirection1 < points/2+1) + v1 *= - 1; + } + else if (plusTangentDirection1 < 0) + v1 *= - 1; + + //Normal + plusNormal = 2*plusNormal - valid_nn_points; + if (plusNormal == 0) + { + int points = 5; //std::min(valid_nn_points*2/2+1, 11); + int medianIndex = valid_nn_points/2; + + for (int i = -points/2; i <= points/2; i++) + if ( vij.row (medianIndex - i).dot (v3) > 0) + plusNormal ++; + + if (plusNormal < points/2+1) + v3 *= - 1; + } else if (plusNormal < 0) + v3 *= - 1; + + rf.row (0).matrix () = v1.head<3> ().cast (); + rf.row (2).matrix () = v3.head<3> ().cast (); + rf.row (1).matrix () = rf.row (2).cross (rf.row (0)); + + return (0.0f); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTLocalReferenceFrameEstimation::computeFeature (PointCloudOut &output) +{ + //check whether used with search radius or search k-neighbors + if (this->getKSearch () != 0) + { + PCL_ERROR( + "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", + getClassName().c_str ()); + return; + } + tree_->setSortedResults (true); + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // point result + Eigen::Matrix3f rf; + PointOutT& output_rf = output[i]; + + //output_rf.confidence = getLocalRF ((*indices_)[i], rf); + //if (output_rf.confidence == std::numeric_limits::max ()) + if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits::max ()) + { + output.is_dense = false; + } + + for (int d = 0; d < 3; ++d) + { + output_rf.x_axis[d] = rf.row (0)[d]; + output_rf.y_axis[d] = rf.row (1)[d]; + output_rf.z_axis[d] = rf.row (2)[d]; + } + } +} + +#define PCL_INSTANTIATE_SHOTLocalReferenceFrameEstimation(T,OutT) template class PCL_EXPORTS pcl::SHOTLocalReferenceFrameEstimation; + +#endif // PCL_FEATURES_IMPL_SHOT_LRF_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/shot_lrf_omp.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/shot_lrf_omp.hpp new file mode 100755 index 0000000..93e31c4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/shot_lrf_omp.hpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_SHOT_LRF_OMP_H_ +#define PCL_FEATURES_IMPL_SHOT_LRF_OMP_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTLocalReferenceFrameEstimationOMP::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTLocalReferenceFrameEstimationOMP::computeFeature (PointCloudOut &output) +{ + //check whether used with search radius or search k-neighbors + if (this->getKSearch () != 0) + { + PCL_ERROR( + "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", + getClassName().c_str ()); + return; + } + tree_->setSortedResults (true); + +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (std::ptrdiff_t i = 0; i < static_cast (indices_->size ()); ++i) + { + // point result + Eigen::Matrix3f rf; + PointOutT& output_rf = output[i]; + + //output_rf.confidence = getLocalRF ((*indices_)[i], rf); + //if (output_rf.confidence == std::numeric_limits::max ()) + + std::vector n_indices; + std::vector n_sqr_distances; + this->searchForNeighbors ((*indices_)[i], search_parameter_, n_indices, n_sqr_distances); + if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits::max ()) + { + output.is_dense = false; + } + + for (int d = 0; d < 3; ++d) + { + output_rf.x_axis[d] = rf.row (0)[d]; + output_rf.y_axis[d] = rf.row (1)[d]; + output_rf.z_axis[d] = rf.row (2)[d]; + } + } + +} + +#define PCL_INSTANTIATE_SHOTLocalReferenceFrameEstimationOMP(T,OutT) template class PCL_EXPORTS pcl::SHOTLocalReferenceFrameEstimationOMP; + +#endif // PCL_FEATURES_IMPL_SHOT_LRF_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/shot_omp.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/shot_omp.hpp new file mode 100755 index 0000000..5e0f39f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/shot_omp.hpp @@ -0,0 +1,293 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_ +#define PCL_FEATURES_IMPL_SHOT_OMP_H_ + +#include +#include +#include + +template bool +pcl::SHOTEstimationOMP::initCompute () +{ + if (!FeatureFromNormals::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // SHOT cannot work with k-search + if (this->getKSearch () != 0) + { + PCL_ERROR( + "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", + getClassName().c_str ()); + return (false); + } + + // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP + typename SHOTLocalReferenceFrameEstimationOMP::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP); + lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_)); + lrf_estimator->setInputCloud (input_); + lrf_estimator->setIndices (indices_); + lrf_estimator->setNumberOfThreads(threads_); + + if (!fake_surface_) + lrf_estimator->setSearchSurface(surface_); + + if (!FeatureWithLocalReferenceFrames::initLocalReferenceFrames (indices_->size (), lrf_estimator)) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SHOTColorEstimationOMP::initCompute () +{ + if (!FeatureFromNormals::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // SHOT cannot work with k-search + if (this->getKSearch () != 0) + { + PCL_ERROR( + "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", + getClassName().c_str ()); + return (false); + } + + // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP + typename SHOTLocalReferenceFrameEstimationOMP::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP); + lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_)); + lrf_estimator->setInputCloud (input_); + lrf_estimator->setIndices (indices_); + lrf_estimator->setNumberOfThreads(threads_); + + if (!fake_surface_) + lrf_estimator->setSearchSurface(surface_); + + if (!FeatureWithLocalReferenceFrames::initLocalReferenceFrames (indices_->size (), lrf_estimator)) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTEstimationOMP::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTEstimationOMP::computeFeature (PointCloudOut &output) +{ + descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1); + + sqradius_ = search_radius_ * search_radius_; + radius3_4_ = (search_radius_ * 3) / 4; + radius1_4_ = search_radius_ / 4; + radius1_2_ = search_radius_ / 2; + + assert(descLength_ == 352); + + output.is_dense = true; + // Iterating over the entire index vector +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) + { + + Eigen::VectorXf shot; + shot.setZero (descLength_); + + bool lrf_is_nan = false; + const PointRFT& current_frame = (*frames_)[idx]; + if (!std::isfinite (current_frame.x_axis[0]) || + !std::isfinite (current_frame.y_axis[0]) || + !std::isfinite (current_frame.z_axis[0])) + { + PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", + getClassName ().c_str (), (*indices_)[idx]); + lrf_is_nan = true; + } + + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, + nn_dists) == 0) + { + // Copy into the resultant cloud + for (Eigen::Index d = 0; d < shot.size (); ++d) + output.points[idx].descriptor[d] = std::numeric_limits::quiet_NaN (); + for (int d = 0; d < 9; ++d) + output.points[idx].rf[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // Estimate the SHOT at each patch + this->computePointSHOT (idx, nn_indices, nn_dists, shot); + + // Copy into the resultant cloud + for (Eigen::Index d = 0; d < shot.size (); ++d) + output.points[idx].descriptor[d] = shot[d]; + for (int d = 0; d < 3; ++d) + { + output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; + output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; + output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTColorEstimationOMP::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SHOTColorEstimationOMP::computeFeature (PointCloudOut &output) +{ + descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0; + descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0; + + assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) || + (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) || + (b_describe_color_ && b_describe_shape_ && descLength_ == 1344) + ); + + sqradius_ = search_radius_ * search_radius_; + radius3_4_ = (search_radius_ * 3) / 4; + radius1_4_ = search_radius_ / 4; + radius1_2_ = search_radius_ / 2; + + output.is_dense = true; + // Iterating over the entire index vector +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) + { + Eigen::VectorXf shot; + shot.setZero (descLength_); + + // Allocate enough space to hold the results + // \note This resize is irrelevant for a radiusSearch (). + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + bool lrf_is_nan = false; + const PointRFT& current_frame = (*frames_)[idx]; + if (!std::isfinite (current_frame.x_axis[0]) || + !std::isfinite (current_frame.y_axis[0]) || + !std::isfinite (current_frame.z_axis[0])) + { + PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", + getClassName ().c_str (), (*indices_)[idx]); + lrf_is_nan = true; + } + + if (!isFinite ((*input_)[(*indices_)[idx]]) || + lrf_is_nan || + this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) + { + // Copy into the resultant cloud + for (Eigen::Index d = 0; d < shot.size (); ++d) + output.points[idx].descriptor[d] = std::numeric_limits::quiet_NaN (); + for (int d = 0; d < 9; ++d) + output.points[idx].rf[d] = std::numeric_limits::quiet_NaN (); + + output.is_dense = false; + continue; + } + + // Estimate the SHOT at each patch + this->computePointSHOT (idx, nn_indices, nn_dists, shot); + + // Copy into the resultant cloud + for (Eigen::Index d = 0; d < shot.size (); ++d) + output.points[idx].descriptor[d] = shot[d]; + for (int d = 0; d < 3; ++d) + { + output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; + output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; + output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; + } + } +} + +#define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP; +#define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP; + +#endif // PCL_FEATURES_IMPL_SHOT_OMP_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/spin_image.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/spin_image.hpp new file mode 100755 index 0000000..cf29439 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/spin_image.hpp @@ -0,0 +1,343 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_ +#define PCL_FEATURES_IMPL_SPIN_IMAGE_H_ + +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::SpinImageEstimation::SpinImageEstimation ( + unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb) : + input_normals_ (), rotation_axes_cloud_ (), + is_angular_ (false), rotation_axis_ (), use_custom_axis_(false), use_custom_axes_cloud_ (false), + is_radial_ (false), image_width_ (image_width), support_angle_cos_ (support_angle_cos), + min_pts_neighb_ (min_pts_neighb) +{ + assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine? + + feature_name_ = "SpinImageEstimation"; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::ArrayXXd +pcl::SpinImageEstimation::computeSiForPoint (int index) const +{ + assert (image_width_ > 0); + assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine? + + const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ()); + + Eigen::Vector3f origin_normal; + origin_normal = + input_normals_ ? + input_normals_->points[index].getNormalVector3fMap () : + Eigen::Vector3f (); // just a placeholder; should never be used! + + const Eigen::Vector3f rotation_axis = use_custom_axis_ ? + rotation_axis_.getNormalVector3fMap () : + use_custom_axes_cloud_ ? + rotation_axes_cloud_->points[index].getNormalVector3fMap () : + origin_normal; + + Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1)); + Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1)); + + // OK, we are interested in the points of the cylinder of height 2*r and + // base radius r, where r = m_dBinSize * in_iImageWidth + // it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth + // suppose that points are uniformly distributed, so we lose ~40% + // according to the volumes ratio + double bin_size = 0.0; + if (is_radial_) + bin_size = search_radius_ / image_width_; + else + bin_size = search_radius_ / image_width_ / sqrt(2.0); + + std::vector nn_indices; + std::vector nn_sqr_dists; + const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists); + if (neighb_cnt < static_cast (min_pts_neighb_)) + { + throw PCLException ( + "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius", + "spin_image.hpp", "computeSiForPoint"); + } + + // for all neighbor points + for (int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++) + { + // first, skip the points with distant normals + double cos_between_normals = -2.0; // should be initialized if used + if (support_angle_cos_ > 0.0 || is_angular_) // not bogus + { + cos_between_normals = origin_normal.dot (input_normals_->points[nn_indices[i_neigh]].getNormalVector3fMap ()); + if (std::abs (cos_between_normals) > (1.0 + 10*std::numeric_limits::epsilon ())) // should be okay for numeric stability + { + PCL_ERROR ("[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n", + getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals); + throw PCLException ("Some normals are not normalized", + "spin_image.hpp", "computeSiForPoint"); + } + cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals)); + + if (std::abs (cos_between_normals) < support_angle_cos_ ) // allow counter-directed normals + { + continue; + } + + if (cos_between_normals < 0.0) + { + cos_between_normals = -cos_between_normals; // the normal is not used explicitly from now + } + } + + // now compute the coordinate in cylindric coordinate system associated with the origin point + const Eigen::Vector3f direction ( + surface_->points[nn_indices[i_neigh]].getVector3fMap () - origin_point); + const double direction_norm = direction.norm (); + if (std::abs(direction_norm) < 10*std::numeric_limits::epsilon ()) + continue; // ignore the point itself; it does not contribute really + assert (direction_norm > 0.0); + + // the angle between the normal vector and the direction to the point + double cos_dir_axis = direction.dot(rotation_axis) / direction_norm; + if (std::abs(cos_dir_axis) > (1.0 + 10*std::numeric_limits::epsilon())) // should be okay for numeric stability + { + PCL_ERROR ("[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n", + getClassName ().c_str (), index, cos_dir_axis); + throw PCLException ("Some rotation axis is not normalized", + "spin_image.hpp", "computeSiForPoint"); + } + cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis)); + + // compute coordinates w.r.t. the reference frame + double beta = std::numeric_limits::signaling_NaN (); + double alpha = std::numeric_limits::signaling_NaN (); + if (is_radial_) // radial spin image structure + { + beta = asin (cos_dir_axis); // yes, arc sine! to get the angle against tangent, not normal! + alpha = direction_norm; + } + else // rectangular spin-image structure + { + beta = direction_norm * cos_dir_axis; + alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis); + + if (std::abs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_) + { + continue; // outside the cylinder + } + } + + assert (alpha >= 0.0); + assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits::epsilon () ); + + + // bilinear interpolation + double beta_bin_size = is_radial_ ? (M_PI / 2 / image_width_) : bin_size; + int beta_bin = int(std::floor (beta / beta_bin_size)) + int(image_width_); + assert (0 <= beta_bin && beta_bin < m_matrix.cols ()); + int alpha_bin = int(std::floor (alpha / bin_size)); + assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ()); + + if (alpha_bin == static_cast (image_width_)) // border points + { + alpha_bin--; + // HACK: to prevent a > 1 + alpha = bin_size * (alpha_bin + 1) - std::numeric_limits::epsilon (); + } + if (beta_bin == int(2*image_width_) ) // border points + { + beta_bin--; + // HACK: to prevent b > 1 + beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits::epsilon (); + } + + double a = alpha/bin_size - double(alpha_bin); + double b = beta/beta_bin_size - double(beta_bin-int(image_width_)); + + assert (0 <= a && a <= 1); + assert (0 <= b && b <= 1); + + m_matrix (alpha_bin, beta_bin) += (1-a) * (1-b); + m_matrix (alpha_bin+1, beta_bin) += a * (1-b); + m_matrix (alpha_bin, beta_bin+1) += (1-a) * b; + m_matrix (alpha_bin+1, beta_bin+1) += a * b; + + if (is_angular_) + { + m_averAngles (alpha_bin, beta_bin) += (1-a) * (1-b) * std::acos (cos_between_normals); + m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * std::acos (cos_between_normals); + m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * std::acos (cos_between_normals); + m_averAngles (alpha_bin+1, beta_bin+1) += a * b * std::acos (cos_between_normals); + } + } + + if (is_angular_) + { + // transform sum to average + m_matrix = m_averAngles / (m_matrix + std::numeric_limits::epsilon ()); // +eps to avoid division by zero + } + else if (neighb_cnt > 1) // to avoid division by zero, also no need to divide by 1 + { + // normalization + m_matrix /= m_matrix.sum(); + } + + return m_matrix; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SpinImageEstimation::initCompute () +{ + if (!Feature::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // Check if input normals are set + if (!input_normals_) + { + PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ()); + Feature::deinitCompute (); + return (false); + } + + // Check if the size of normals is the same as the size of the surface + if (input_normals_->points.size () != input_->points.size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ()); + PCL_ERROR ("The number of points in the input dataset differs from "); + PCL_ERROR ("the number of points in the dataset containing the normals!\n"); + Feature::deinitCompute (); + return (false); + } + + // We need a positive definite search radius to continue + if (search_radius_ == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ()); + Feature::deinitCompute (); + return (false); + } + if (k_ != 0) + { + PCL_ERROR ("[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ()); + Feature::deinitCompute (); + return (false); + } + // If the surface won't be set, make fake surface and fake surface normals + // if we wouldn't do it here, the following method would alarm that no surface normals is given + if (!surface_) + { + surface_ = input_; + fake_surface_ = true; + } + + //if (fake_surface_ && !input_normals_) + // input_normals_ = normals_; // normals_ is set, as checked earlier + + assert(!(use_custom_axis_ && use_custom_axes_cloud_)); + + if (!use_custom_axis_ && !use_custom_axes_cloud_ // use input normals as rotation axes + && !input_normals_) + { + PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ()); + // Cleanup + Feature::deinitCompute (); + return (false); + } + + if ((is_angular_ || support_angle_cos_ > 0.0) // support angle is not bogus NOTE this is for randomly-flipped normals + && !input_normals_) + { + PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ()); + // Cleanup + Feature::deinitCompute (); + return (false); + } + + if (use_custom_axes_cloud_ + && rotation_axes_cloud_->size () == input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ()); + // Cleanup + Feature::deinitCompute (); + return (false); + } + + return (true); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SpinImageEstimation::computeFeature (PointCloudOut &output) +{ + for (std::size_t i_input = 0; i_input < indices_->size (); ++i_input) + { + Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input)); + + // Copy into the resultant cloud + for (Eigen::Index iRow = 0; iRow < res.rows () ; iRow++) + { + for (Eigen::Index iCol = 0; iCol < res.cols () ; iCol++) + { + output.points[i_input].histogram[ iRow*res.cols () + iCol ] = static_cast (res (iRow, iCol)); + } + } + } +} + +#define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation; + +#endif // PCL_FEATURES_IMPL_SPIN_IMAGE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/statistical_multiscale_interest_region_extraction.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/statistical_multiscale_interest_region_extraction.hpp new file mode 100755 index 0000000..2ce7550 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/statistical_multiscale_interest_region_extraction.hpp @@ -0,0 +1,253 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ +#define PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ + +#include +#include +#include +#include +#include +#include + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::StatisticalMultiscaleInterestRegionExtraction::generateCloudGraph () +{ + // generate a K-NNG (K-nearest neighbors graph) + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud (input_); + + using namespace boost; + using Weight = property; + using Graph = adjacency_list; + Graph cloud_graph; + + for (std::size_t point_i = 0; point_i < input_->points.size (); ++point_i) + { + std::vector k_indices (16); + std::vector k_distances (16); + kdtree.nearestKSearch (static_cast (point_i), 16, k_indices, k_distances); + + for (std::size_t k_i = 0; k_i < k_indices.size (); ++k_i) + add_edge (point_i, k_indices[k_i], Weight (std::sqrt (k_distances[k_i])), cloud_graph); + } + + const std::size_t E = num_edges (cloud_graph), + V = num_vertices (cloud_graph); + PCL_INFO ("The graph has %lu vertices and %lu edges.\n", V, E); + geodesic_distances_.clear (); + for (std::size_t i = 0; i < V; ++i) + { + std::vector aux (V); + geodesic_distances_.push_back (aux); + } + johnson_all_pairs_shortest_paths (cloud_graph, geodesic_distances_); + + PCL_INFO ("Done generating the graph\n"); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute () +{ + if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n"); + return (false); + } + if (scale_values_.empty ()) + { + PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] No scale values were given\n"); + return (false); + } + + return (true); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::StatisticalMultiscaleInterestRegionExtraction::geodesicFixedRadiusSearch (std::size_t &query_index, + float &radius, + std::vector &result_indices) +{ + for (std::size_t i = 0; i < geodesic_distances_[query_index].size (); ++i) + if (i != query_index && geodesic_distances_[query_index][i] < radius) + result_indices.push_back (static_cast (i)); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::StatisticalMultiscaleInterestRegionExtraction::computeRegionsOfInterest (std::list &rois) +{ + if (!initCompute ()) + { + PCL_ERROR ("StatisticalMultiscaleInterestRegionExtraction: not completely initialized\n"); + return; + } + + generateCloudGraph (); + + computeF (); + + extractExtrema (rois); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::StatisticalMultiscaleInterestRegionExtraction::computeF () +{ + PCL_INFO ("Calculating statistical information\n"); + + // declare and initialize data structure + F_scales_.resize (scale_values_.size ()); + std::vector point_density (input_->points.size ()), + F (input_->points.size ()); + std::vector > phi (input_->points.size ()); + std::vector phi_row (input_->points.size ()); + + for (std::size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i) + { + float scale_squared = scale_values_[scale_i] * scale_values_[scale_i]; + + // calculate point density for each point x_i + for (std::size_t point_i = 0; point_i < input_->points.size (); ++point_i) + { + float point_density_i = 0.0; + for (std::size_t point_j = 0; point_j < input_->points.size (); ++point_j) + { + float d_g = geodesic_distances_[point_i][point_j]; + float phi_i_j = 1.0f / std::sqrt (2.0f * static_cast (M_PI) * scale_squared) * std::exp ( (-1) * d_g*d_g / (2.0f * scale_squared)); + + point_density_i += phi_i_j; + phi_row[point_j] = phi_i_j; + } + point_density[point_i] = point_density_i; + phi[point_i] = phi_row; + } + + // compute weights for each pair (x_i, x_j), evaluate the operator A_hat + for (std::size_t point_i = 0; point_i < input_->points.size (); ++point_i) + { + float A_hat_normalization = 0.0; + PointT A_hat; A_hat.x = A_hat.y = A_hat.z = 0.0; + for (std::size_t point_j = 0; point_j < input_->points.size (); ++point_j) + { + float phi_hat_i_j = phi[point_i][point_j] / (point_density[point_i] * point_density[point_j]); + A_hat_normalization += phi_hat_i_j; + + PointT aux = input_->points[point_j]; + aux.x *= phi_hat_i_j; aux.y *= phi_hat_i_j; aux.z *= phi_hat_i_j; + + A_hat.x += aux.x; A_hat.y += aux.y; A_hat.z += aux.z; + } + A_hat.x /= A_hat_normalization; A_hat.y /= A_hat_normalization; A_hat.z /= A_hat_normalization; + + // compute the invariant F + float aux = 2.0f / scale_values_[scale_i] * euclideanDistance (A_hat, input_->points[point_i]); + F[point_i] = aux * std::exp (-aux); + } + + F_scales_[scale_i] = F; + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::StatisticalMultiscaleInterestRegionExtraction::extractExtrema (std::list &rois) +{ + std::vector > is_min (scale_values_.size ()), + is_max (scale_values_.size ()); + + // for each point, check if it is a local extrema on each scale + for (std::size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i) + { + std::vector is_min_scale (input_->points.size ()), + is_max_scale (input_->points.size ()); + for (std::size_t point_i = 0; point_i < input_->points.size (); ++point_i) + { + std::vector nn_indices; + geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices); + bool is_max_point = true, is_min_point = true; + for (const int &nn_index : nn_indices) + if (F_scales_[scale_i][point_i] < F_scales_[scale_i][nn_index]) + is_max_point = false; + else + is_min_point = false; + + is_min_scale[point_i] = is_min_point; + is_max_scale[point_i] = is_max_point; + } + + is_min[scale_i] = is_min_scale; + is_max[scale_i] = is_max_scale; + } + + // look for points that are min/max over three consecutive scales + for (std::size_t scale_i = 1; scale_i < scale_values_.size () - 1; ++scale_i) + { + for (std::size_t point_i = 0; point_i < input_->points.size (); ++point_i) + if ((is_min[scale_i - 1][point_i] && is_min[scale_i][point_i] && is_min[scale_i + 1][point_i]) || + (is_max[scale_i - 1][point_i] && is_max[scale_i][point_i] && is_max[scale_i + 1][point_i])) + { + // add the point to the result vector + IndicesPtr region (new std::vector); + region->push_back (static_cast (point_i)); + + // and also add its scale-sized geodesic neighborhood + std::vector nn_indices; + geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices); + region->insert (region->end (), nn_indices.begin (), nn_indices.end ()); + rois.push_back (region); + } + } +} + + +#define PCL_INSTANTIATE_StatisticalMultiscaleInterestRegionExtraction(T) template class PCL_EXPORTS pcl::StatisticalMultiscaleInterestRegionExtraction; + +#endif /* PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ */ + diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/usc.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/usc.hpp new file mode 100755 index 0000000..da6487a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/usc.hpp @@ -0,0 +1,263 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_USC_HPP_ +#define PCL_FEATURES_IMPL_USC_HPP_ + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::UniqueShapeContext::initCompute () +{ + if (!Feature::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation + typename SHOTLocalReferenceFrameEstimation::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation()); + lrf_estimator->setRadiusSearch (local_radius_); + lrf_estimator->setInputCloud (input_); + lrf_estimator->setIndices (indices_); + if (!fake_surface_) + lrf_estimator->setSearchSurface(surface_); + + if (!FeatureWithLocalReferenceFrames::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 (azimuth_bins_); + float elevation_interval = 180.0f / static_cast (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 (std::size_t j = 0; j < radius_bins_ + 1; j++) + radii_interval_[j] = static_cast (std::exp (std::log (min_radius_) + ((static_cast (j) / static_cast (radius_bins_)) * std::log (search_radius_/min_radius_)))); + + // Fill theta divisions of elevation + theta_divisions_.resize (elevation_bins_ + 1, elevation_interval); + theta_divisions_[0] = 0; + std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ()); + + // Fill phi divisions of elevation + phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval); + phi_divisions_[0] = 0; + std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ()); + + // 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 (std::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 (std::size_t k = 0; k < elevation_bins_; k++) + { + // "theta" term of the volume integral + float integr_theta = std::cos (deg2rad (theta_divisions_[k])) - std::cos (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 (std::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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UniqueShapeContext::computePointDescriptor (std::size_t index, /*float rf[9],*/ std::vector &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 nn_indices; + std::vector nn_dists; + const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists); + // For each point within radius + for (std::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 (std::acos (std::min (1.0f, std::max (-1.0f, theta)))); + + /// Compute the Bin(j, k, l) coordinates of current neighbour + const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r); + const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta); + const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi); + + /// Bin (j, k, l) + const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min)); + const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min)); + const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min)); + + /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour + std::vector neighbour_indices; + std::vector neighbour_didtances; + float point_density = static_cast (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::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 void +pcl::UniqueShapeContext::computeFeature (PointCloudOut &output) +{ + assert (descriptor_length_ == 1960); + + output.is_dense = true; + + for (std::size_t point_index = 0; point_index < indices_->size (); ++point_index) + { + //output[point_index].descriptor.resize (descriptor_length_); + + // If the point is not finite, set the descriptor to NaN and continue + const PointRFT& current_frame = (*frames_)[point_index]; + if (!isFinite ((*input_)[(*indices_)[point_index]]) || + !std::isfinite (current_frame.x_axis[0]) || + !std::isfinite (current_frame.y_axis[0]) || + !std::isfinite (current_frame.z_axis[0]) ) + { + std::fill (output.points[point_index].descriptor, output.points[point_index].descriptor + descriptor_length_, + std::numeric_limits::quiet_NaN ()); + std::fill (output.points[point_index].rf, output.points[point_index].rf + 9, 0); + output.is_dense = false; + continue; + } + + for (int d = 0; d < 3; ++d) + { + output.points[point_index].rf[0 + d] = current_frame.x_axis[d]; + output.points[point_index].rf[3 + d] = current_frame.y_axis[d]; + output.points[point_index].rf[6 + d] = current_frame.z_axis[d]; + } + + std::vector descriptor (descriptor_length_); + computePointDescriptor (point_index, descriptor); + std::copy (descriptor.begin (), descriptor.end (), output.points[point_index].descriptor); + } +} + +#define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/impl/vfh.hpp b/deps_install/include/pcl-1.10/pcl/features/impl/vfh.hpp new file mode 100755 index 0000000..f47180d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/impl/vfh.hpp @@ -0,0 +1,253 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FEATURES_IMPL_VFH_H_ +#define PCL_FEATURES_IMPL_VFH_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::VFHEstimation::initCompute () +{ + if (input_->points.size () < 2 || (surface_ && surface_->points.size () < 2)) + { + PCL_ERROR ("[pcl::VFHEstimation::initCompute] Input dataset must have at least 2 points!\n"); + return (false); + } + if (search_radius_ == 0 && k_ == 0) + k_ = 1; + return (Feature::initCompute ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::VFHEstimation::compute (PointCloudOut &output) +{ + if (!initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + // Copy the header + output.header = input_->header; + + // Resize the output dataset + // Important! We should only allocate precisely how many elements we will need, otherwise + // we risk at pre-allocating too much memory which could lead to bad_alloc + // (see http://dev.pointclouds.org/issues/657) + output.width = output.height = 1; + output.is_dense = input_->is_dense; + output.points.resize (1); + + // Perform the actual feature computation + computeFeature (output); + + Feature::deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::VFHEstimation::computePointSPFHSignature (const Eigen::Vector4f ¢roid_p, + const Eigen::Vector4f ¢roid_n, + const pcl::PointCloud &cloud, + const pcl::PointCloud &normals, + const std::vector &indices) +{ + Eigen::Vector4f pfh_tuple; + // Reset the whole thing + for (int i = 0; i < 4; ++i) + { + hist_f_[i].setZero (nr_bins_f_[i]); + } + + // Get the bounding box of the current cluster + //Eigen::Vector4f min_pt, max_pt; + //pcl::getMinMax3D (cloud, indices, min_pt, max_pt); + //double distance_normalization_factor = (std::max)((centroid_p - min_pt).norm (), (centroid_p - max_pt).norm ()); + + //Instead of using the bounding box to normalize the VFH distance component, it is better to use the max_distance + //from any point to centroid. VFH is invariant to rotation about the roll axis but the bounding box is not, + //resulting in different normalization factors for point clouds that are just rotated about that axis. + + double distance_normalization_factor = 1.0; + if (normalize_distances_) + { + Eigen::Vector4f max_pt; + pcl::getMaxDistance (cloud, indices, centroid_p, max_pt); + max_pt[3] = 0; + distance_normalization_factor = (centroid_p - max_pt).norm (); + } + + // Factorization constant + float hist_incr = 1; + if (normalize_bins_) + hist_incr = 100.0f / static_cast (indices.size () - 1); + + float hist_incr_size_component = 0;; + if (size_component_) + hist_incr_size_component = hist_incr; + + // Iterate over all the points in the neighborhood + for (const int &index : indices) + { + // Compute the pair P to NNi + if (!computePairFeatures (centroid_p, centroid_n, cloud.points[index].getVector4fMap (), + normals.points[index].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1], + pfh_tuple[2], pfh_tuple[3])) + continue; + + // Normalize the f1, f2, f3, f4 features and push them in the histogram + for (int i = 0; i < 3; ++i) + { + const int raw_index = static_cast (std::floor (nr_bins_f_[i] * ((pfh_tuple[i] + M_PI) * d_pi_))); + const int h_index = std::max(std::min(raw_index, nr_bins_f_[i] - 1), 0); + hist_f_[i] (h_index) += hist_incr; + } + + if (hist_incr_size_component) + { + int h_index; + if (normalize_distances_) + h_index = static_cast (std::floor (nr_bins_f_[3] * (pfh_tuple[3] / distance_normalization_factor))); + else + h_index = static_cast (pcl_round (pfh_tuple[3] * 100)); + + h_index = std::max (std::min (h_index, nr_bins_f_[3] - 1), 0); + hist_f_[3] (h_index) += hist_incr_size_component; + } + } +} +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::VFHEstimation::computeFeature (PointCloudOut &output) +{ + // ---[ Step 1a : compute the centroid in XYZ space + Eigen::Vector4f xyz_centroid (0, 0, 0, 0); + + if (use_given_centroid_) + xyz_centroid = centroid_to_use_; + else + compute3DCentroid (*surface_, *indices_, xyz_centroid); // Estimate the XYZ centroid + + // ---[ Step 1b : compute the centroid in normal space + Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero (); + + // If the data is dense, we don't need to check for NaN + if (use_given_normal_) + normal_centroid = normal_to_use_; + else + { + std::size_t cp = 0; + if (normals_->is_dense) + { + for (const auto& index: *indices_) + { + normal_centroid.noalias () += normals_->points[index].getNormalVector4fMap (); + } + cp = indices_->size(); + } + // NaN or Inf values could exist => check for them + else + { + for (const auto& index: *indices_) + { + if (!std::isfinite (normals_->points[index].normal[0]) || + !std::isfinite (normals_->points[index].normal[1]) || + !std::isfinite (normals_->points[index].normal[2])) + continue; + normal_centroid.noalias () += normals_->points[index].getNormalVector4fMap (); + cp++; + } + } + normal_centroid /= static_cast (cp); + } + + // Compute the direction of view from the viewpoint to the centroid + Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0); + Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid; + d_vp_p.normalize (); + + // Estimate the SPFH at nn_indices[0] using the entire cloud + computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_); + + // ---[ Step 2 : obtain the viewpoint component + hist_vp_.setZero (nr_bins_vp_); + + float hist_incr = 1.0; + if (normalize_bins_) + hist_incr = 100.0 / static_cast (indices_->size ()); + + for (const auto& index: *indices_) + { + Eigen::Vector4f normal (normals_->points[index].normal[0], + normals_->points[index].normal[1], + normals_->points[index].normal[2], 0); + // Normalize + double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5; + std::size_t fi = static_cast (std::floor (alpha * hist_vp_.size ())); + fi = std::max (0u, fi); + fi = std::min (hist_vp_.size () - 1, fi); + // Bin into the histogram + hist_vp_ [fi] += hist_incr; + } + + // We only output _1_ signature + output.points.resize (1); + output.width = 1; + output.height = 1; + + // Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature + auto outPtr = std::begin (output.points[0].histogram); + + for (int i = 0; i < 4; ++i) + { + outPtr = std::copy_n (hist_f_[i].data (), hist_f_[i].size (), outPtr); + } + outPtr = std::copy_n (hist_vp_.data (), hist_vp_.size (), outPtr); +} + +#define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation; + +#endif // PCL_FEATURES_IMPL_VFH_H_ diff --git a/deps_install/include/pcl-1.10/pcl/features/integral_image2D.h b/deps_install/include/pcl-1.10/pcl/features/integral_image2D.h new file mode 100755 index 0000000..a69537d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/integral_image2D.h @@ -0,0 +1,348 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: feature.h 2784 2011-10-15 22:05:38Z aichim $ + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + template + struct IntegralImageTypeTraits + { + using Type = DataType; + using IntegralType = DataType; + }; + + template <> + struct IntegralImageTypeTraits + { + using Type = float; + using IntegralType = double; + }; + + template <> + struct IntegralImageTypeTraits + { + using Type = char; + using IntegralType = int; + }; + + template <> + struct IntegralImageTypeTraits + { + using Type = short; + using IntegralType = long; + }; + + template <> + struct IntegralImageTypeTraits + { + using Type = unsigned short; + using IntegralType = unsigned long; + }; + + template <> + struct IntegralImageTypeTraits + { + using Type = unsigned char; + using IntegralType = unsigned int; + }; + + template <> + struct IntegralImageTypeTraits + { + using Type = int; + using IntegralType = long; + }; + + template <> + struct IntegralImageTypeTraits + { + using Type = unsigned int; + using IntegralType = unsigned long; + }; + + /** \brief Determines an integral image representation for a given organized data array + * \author Suat Gedikli + */ + template + class IntegralImage2D + { + public: + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + static const unsigned second_order_size = (Dimension * (Dimension + 1)) >> 1; + using ElementType = Eigen::Matrix::IntegralType, Dimension, 1>; + using SecondOrderType = Eigen::Matrix::IntegralType, second_order_size, 1>; + + /** \brief Constructor for an Integral Image + * \param[in] compute_second_order_integral_images set to true if we want to compute a second order image + */ + IntegralImage2D (bool compute_second_order_integral_images) : + first_order_integral_image_ (), + second_order_integral_image_ (), + width_ (1), + height_ (1), + compute_second_order_integral_images_ (compute_second_order_integral_images) + { + } + + /** \brief Destructor */ + virtual + ~IntegralImage2D () { } + + /** \brief sets the computation for second order integral images on or off. + * \param compute_second_order_integral_images + */ + void + setSecondOrderComputation (bool compute_second_order_integral_images); + + /** \brief Set the input data to compute the integral image for + * \param[in] data the input data + * \param[in] width the width of the data + * \param[in] height the height of the data + * \param[in] element_stride the element stride of the data + * \param[in] row_stride the row stride of the data + */ + void + setInput (const DataType * data, + unsigned width, unsigned height, unsigned element_stride, unsigned row_stride); + + /** \brief Compute the first order sum within a given rectangle + * \param[in] start_x x position of rectangle + * \param[in] start_y y position of rectangle + * \param[in] width width of rectangle + * \param[in] height height of rectangle + */ + inline ElementType + getFirstOrderSum (unsigned start_x, unsigned start_y, unsigned width, unsigned height) const; + + /** \brief Compute the first order sum within a given rectangle + * \param[in] start_x x position of the start of the rectangle + * \param[in] start_y x position of the start of the rectangle + * \param[in] end_x x position of the end of the rectangle + * \param[in] end_y x position of the end of the rectangle + */ + inline ElementType + getFirstOrderSumSE (unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const; + + /** \brief Compute the second order sum within a given rectangle + * \param[in] start_x x position of rectangle + * \param[in] start_y y position of rectangle + * \param[in] width width of rectangle + * \param[in] height height of rectangle + */ + inline SecondOrderType + getSecondOrderSum (unsigned start_x, unsigned start_y, unsigned width, unsigned height) const; + + /** \brief Compute the second order sum within a given rectangle + * \param[in] start_x x position of the start of the rectangle + * \param[in] start_y x position of the start of the rectangle + * \param[in] end_x x position of the end of the rectangle + * \param[in] end_y x position of the end of the rectangle + */ + inline SecondOrderType + getSecondOrderSumSE (unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const; + + /** \brief Compute the number of finite elements within a given rectangle + * \param[in] start_x x position of rectangle + * \param[in] start_y y position of rectangle + * \param[in] width width of rectangle + * \param[in] height height of rectangle + */ + inline unsigned + getFiniteElementsCount (unsigned start_x, unsigned start_y, unsigned width, unsigned height) const; + + /** \brief Compute the number of finite elements within a given rectangle + * \param[in] start_x x position of the start of the rectangle + * \param[in] start_y x position of the start of the rectangle + * \param[in] end_x x position of the end of the rectangle + * \param[in] end_y x position of the end of the rectangle + */ + inline unsigned + getFiniteElementsCountSE (unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const; + + private: + using InputType = Eigen::Matrix::Type, Dimension, 1>; + + /** \brief Compute the actual integral image data + * \param[in] data the input data + * \param[in] element_stride the element stride of the data + * \param[in] row_stride the row stride of the data + */ + void + computeIntegralImages (const DataType * data, unsigned row_stride, unsigned element_stride); + + std::vector > first_order_integral_image_; + std::vector > second_order_integral_image_; + std::vector finite_values_integral_image_; + + /** \brief The width of the 2d input data array */ + unsigned width_; + /** \brief The height of the 2d input data array */ + unsigned height_; + + /** \brief Indicates whether second order integral images are available **/ + bool compute_second_order_integral_images_; + }; + + /** + * \brief partial template specialization for integral images with just one channel. + */ + template + class IntegralImage2D + { + public: + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + static const unsigned second_order_size = 1; + using ElementType = typename IntegralImageTypeTraits::IntegralType; + using SecondOrderType = typename IntegralImageTypeTraits::IntegralType; + + /** \brief Constructor for an Integral Image + * \param[in] compute_second_order_integral_images set to true if we want to compute a second order image + */ + IntegralImage2D (bool compute_second_order_integral_images) : + first_order_integral_image_ (), + second_order_integral_image_ (), + + width_ (1), height_ (1), + compute_second_order_integral_images_ (compute_second_order_integral_images) + { + } + + /** \brief Destructor */ + virtual + ~IntegralImage2D () { } + + /** \brief Set the input data to compute the integral image for + * \param[in] data the input data + * \param[in] width the width of the data + * \param[in] height the height of the data + * \param[in] element_stride the element stride of the data + * \param[in] row_stride the row stride of the data + */ + void + setInput (const DataType * data, + unsigned width, unsigned height, unsigned element_stride, unsigned row_stride); + + /** \brief Compute the first order sum within a given rectangle + * \param[in] start_x x position of rectangle + * \param[in] start_y y position of rectangle + * \param[in] width width of rectangle + * \param[in] height height of rectangle + */ + inline ElementType + getFirstOrderSum (unsigned start_x, unsigned start_y, unsigned width, unsigned height) const; + + /** \brief Compute the first order sum within a given rectangle + * \param[in] start_x x position of the start of the rectangle + * \param[in] start_y x position of the start of the rectangle + * \param[in] end_x x position of the end of the rectangle + * \param[in] end_y x position of the end of the rectangle + */ + inline ElementType + getFirstOrderSumSE (unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const; + + /** \brief Compute the second order sum within a given rectangle + * \param[in] start_x x position of rectangle + * \param[in] start_y y position of rectangle + * \param[in] width width of rectangle + * \param[in] height height of rectangle + */ + inline SecondOrderType + getSecondOrderSum (unsigned start_x, unsigned start_y, unsigned width, unsigned height) const; + + /** \brief Compute the second order sum within a given rectangle + * \param[in] start_x x position of the start of the rectangle + * \param[in] start_y x position of the start of the rectangle + * \param[in] end_x x position of the end of the rectangle + * \param[in] end_y x position of the end of the rectangle + */ + inline SecondOrderType + getSecondOrderSumSE (unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const; + + /** \brief Compute the number of finite elements within a given rectangle + * \param[in] start_x x position of rectangle + * \param[in] start_y y position of rectangle + * \param[in] width width of rectangle + * \param[in] height height of rectangle + */ + inline unsigned + getFiniteElementsCount (unsigned start_x, unsigned start_y, unsigned width, unsigned height) const; + + /** \brief Compute the number of finite elements within a given rectangle + * \param[in] start_x x position of the start of the rectangle + * \param[in] start_y x position of the start of the rectangle + * \param[in] end_x x position of the end of the rectangle + * \param[in] end_y x position of the end of the rectangle + */ + inline unsigned + getFiniteElementsCountSE (unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const; + + private: + // using InputType = typename IntegralImageTypeTraits::Type; + + /** \brief Compute the actual integral image data + * \param[in] data the input data + * \param[in] element_stride the element stride of the data + * \param[in] row_stride the row stride of the data + */ + void + computeIntegralImages (const DataType * data, unsigned row_stride, unsigned element_stride); + + std::vector > first_order_integral_image_; + std::vector > second_order_integral_image_; + std::vector finite_values_integral_image_; + + /** \brief The width of the 2d input data array */ + unsigned width_; + /** \brief The height of the 2d input data array */ + unsigned height_; + + /** \brief Indicates whether second order integral images are available **/ + bool compute_second_order_integral_images_; + }; + } + +#include diff --git a/deps_install/include/pcl-1.10/pcl/features/integral_image_normal.h b/deps_install/include/pcl-1.10/pcl/features/integral_image_normal.h new file mode 100755 index 0000000..c84987f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/integral_image_normal.h @@ -0,0 +1,474 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Surface normal estimation on organized data using integral images. + * + * For detailed information about this method see: + * + * S. Holzer and R. B. Rusu and M. Dixon and S. Gedikli and N. Navab, + * Adaptive Neighborhood Selection for Real-Time Surface Normal Estimation + * from Organized Point Cloud Data Using Integral Images, IROS 2012. + * + * D. Holz, S. Holzer, R. B. Rusu, and S. Behnke (2011, July). + * Real-Time Plane Segmentation using RGB-D Cameras. In Proceedings of + * the 15th RoboCup International Symposium, Istanbul, Turkey. + * http://www.ais.uni-bonn.de/~holz/papers/holz_2011_robocup.pdf + * + * \author Stefan Holzer + */ + template + class IntegralImageNormalEstimation: public Feature + { + using Feature::input_; + using Feature::feature_name_; + using Feature::tree_; + using Feature::k_; + using Feature::indices_; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Different types of border handling. */ + enum BorderPolicy + { + BORDER_POLICY_IGNORE, + BORDER_POLICY_MIRROR + }; + + /** \brief Different normal estimation methods. + *
    + *
  • COVARIANCE_MATRIX - creates 9 integral images to compute the normal for a specific point + * from the covariance matrix of its local neighborhood.
    • + *
  • AVERAGE_3D_GRADIENT - creates 6 integral images to compute smoothed versions of + * horizontal and vertical 3D gradients and computes the normals using the cross-product between these + * two gradients. + *
  • AVERAGE_DEPTH_CHANGE - creates only a single integral image and computes the normals + * from the average depth changes. + *
+ */ + enum NormalEstimationMethod + { + COVARIANCE_MATRIX, + AVERAGE_3D_GRADIENT, + AVERAGE_DEPTH_CHANGE, + SIMPLE_3D_GRADIENT + }; + + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Constructor */ + IntegralImageNormalEstimation () + : normal_estimation_method_(AVERAGE_3D_GRADIENT) + , border_policy_ (BORDER_POLICY_IGNORE) + , rect_width_ (0), rect_width_2_ (0), rect_width_4_ (0) + , rect_height_ (0), rect_height_2_ (0), rect_height_4_ (0) + , distance_threshold_ (0) + , integral_image_DX_ (false) + , integral_image_DY_ (false) + , integral_image_depth_ (false) + , integral_image_XYZ_ (true) + , diff_x_ (nullptr) + , diff_y_ (nullptr) + , depth_data_ (nullptr) + , distance_map_ (nullptr) + , use_depth_dependent_smoothing_ (false) + , max_depth_change_factor_ (20.0f*0.001f) + , normal_smoothing_size_ (10.0f) + , init_covariance_matrix_ (false) + , init_average_3d_gradient_ (false) + , init_simple_3d_gradient_ (false) + , init_depth_change_ (false) + , vpx_ (0.0f) + , vpy_ (0.0f) + , vpz_ (0.0f) + , use_sensor_origin_ (true) + { + feature_name_ = "IntegralImagesNormalEstimation"; + tree_.reset (); + k_ = 1; + } + + /** \brief Destructor **/ + ~IntegralImageNormalEstimation (); + + /** \brief Set the regions size which is considered for normal estimation. + * \param[in] width the width of the search rectangle + * \param[in] height the height of the search rectangle + */ + void + setRectSize (const int width, const int height); + + /** \brief Sets the policy for handling borders. + * \param[in] border_policy the border policy. + */ + void + setBorderPolicy (const BorderPolicy border_policy) + { + border_policy_ = border_policy; + } + + /** \brief Computes the normal at the specified position. + * \param[in] pos_x x position (pixel) + * \param[in] pos_y y position (pixel) + * \param[in] point_index the position index of the point + * \param[out] normal the output estimated normal + */ + void + computePointNormal (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal); + + /** \brief Computes the normal at the specified position with mirroring for border handling. + * \param[in] pos_x x position (pixel) + * \param[in] pos_y y position (pixel) + * \param[in] point_index the position index of the point + * \param[out] normal the output estimated normal + */ + void + computePointNormalMirror (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal); + + /** \brief The depth change threshold for computing object borders + * \param[in] max_depth_change_factor the depth change threshold for computing object borders based on + * depth changes + */ + void + setMaxDepthChangeFactor (float max_depth_change_factor) + { + max_depth_change_factor_ = max_depth_change_factor; + } + + /** \brief Set the normal smoothing size + * \param[in] normal_smoothing_size factor which influences the size of the area used to smooth normals + * (depth dependent if useDepthDependentSmoothing is true) + */ + void + setNormalSmoothingSize (float normal_smoothing_size) + { + if (normal_smoothing_size <= 0) + { + PCL_ERROR ("[pcl::%s::setNormalSmoothingSize] Invalid normal smoothing size given! (%f). Allowed ranges are: 0 < N. Defaulting to %f.\n", + feature_name_.c_str (), normal_smoothing_size, normal_smoothing_size_); + return; + } + normal_smoothing_size_ = normal_smoothing_size; + } + + /** \brief Set the normal estimation method. The current implemented algorithms are: + *
    + *
  • COVARIANCE_MATRIX - creates 9 integral images to compute the normal for a specific point + * from the covariance matrix of its local neighborhood.
    • + *
  • AVERAGE_3D_GRADIENT - creates 6 integral images to compute smoothed versions of + * horizontal and vertical 3D gradients and computes the normals using the cross-product between these + * two gradients. + *
  • AVERAGE_DEPTH_CHANGE - creates only a single integral image and computes the normals + * from the average depth changes. + *
+ * \param[in] normal_estimation_method the method used for normal estimation + */ + void + setNormalEstimationMethod (NormalEstimationMethod normal_estimation_method) + { + normal_estimation_method_ = normal_estimation_method; + } + + /** \brief Set whether to use depth depending smoothing or not + * \param[in] use_depth_dependent_smoothing decides whether the smoothing is depth dependent + */ + void + setDepthDependentSmoothing (bool use_depth_dependent_smoothing) + { + use_depth_dependent_smoothing_ = use_depth_dependent_smoothing; + } + + /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + inline void + setInputCloud (const typename PointCloudIn::ConstPtr &cloud) override + { + input_ = cloud; + if (!cloud->isOrganized ()) + { + PCL_ERROR ("[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).\n"); + return; + } + + init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false; + + if (use_sensor_origin_) + { + vpx_ = input_->sensor_origin_.coeff (0); + vpy_ = input_->sensor_origin_.coeff (1); + vpz_ = input_->sensor_origin_.coeff (2); + } + + // Initialize the correct data structure based on the normal estimation method chosen + initData (); + } + + /** \brief Returns a pointer to the distance map which was computed internally + */ + inline float* + getDistanceMap () + { + return (distance_map_); + } + + /** \brief Set the viewpoint. + * \param vpx the X coordinate of the viewpoint + * \param vpy the Y coordinate of the viewpoint + * \param vpz the Z coordinate of the viewpoint + */ + inline void + setViewPoint (float vpx, float vpy, float vpz) + { + vpx_ = vpx; + vpy_ = vpy; + vpz_ = vpz; + use_sensor_origin_ = false; + } + + /** \brief Get the viewpoint. + * \param [out] vpx x-coordinate of the view point + * \param [out] vpy y-coordinate of the view point + * \param [out] vpz z-coordinate of the view point + * \note this method returns the currently used viewpoint for normal flipping. + * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates. + * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0) + */ + inline void + getViewPoint (float &vpx, float &vpy, float &vpz) + { + vpx = vpx_; + vpy = vpy_; + vpz = vpz_; + } + + /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the + * normal estimation method uses the sensor origin of the input cloud. + * to use a user defined view point, use the method setViewPoint + */ + inline void + useSensorOriginAsViewPoint () + { + use_sensor_origin_ = true; + if (input_) + { + vpx_ = input_->sensor_origin_.coeff (0); + vpy_ = input_->sensor_origin_.coeff (1); + vpz_ = input_->sensor_origin_.coeff (2); + } + else + { + vpx_ = 0; + vpy_ = 0; + vpz_ = 0; + } + } + + protected: + + /** \brief Computes the normal for the complete cloud or only \a indices_ if provided. + * \param[out] output the resultant normals + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Computes the normal for the complete cloud. + * \param[in] distance_map distance map + * \param[in] bad_point constant given to invalid normal components + * \param[out] output the resultant normals + */ + void + computeFeatureFull (const float* distance_map, const float& bad_point, PointCloudOut& output); + + /** \brief Computes the normal for part of the cloud specified by \a indices_ + * \param[in] distance_map distance map + * \param[in] bad_point constant given to invalid normal components + * \param[out] output the resultant normals + */ + void + computeFeaturePart (const float* distance_map, const float& bad_point, PointCloudOut& output); + + /** \brief Initialize the data structures, based on the normal estimation method chosen. */ + void + initData (); + + private: + + /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint + * \param point a given point + * \param vp_x the X coordinate of the viewpoint + * \param vp_y the X coordinate of the viewpoint + * \param vp_z the X coordinate of the viewpoint + * \param nx the resultant X component of the plane normal + * \param ny the resultant Y component of the plane normal + * \param nz the resultant Z component of the plane normal + * \ingroup features + */ + inline void + flipNormalTowardsViewpoint (const PointInT &point, + float vp_x, float vp_y, float vp_z, + float &nx, float &ny, float &nz) + { + // See if we need to flip any plane normals + vp_x -= point.x; + vp_y -= point.y; + vp_z -= point.z; + + // Dot product between the (viewpoint - point) and the plane normal + float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); + + // Flip the plane normal + if (cos_theta < 0) + { + nx *= -1; + ny *= -1; + nz *= -1; + } + } + + /** \brief The normal estimation method to use. Currently, 3 implementations are provided: + * + * - COVARIANCE_MATRIX + * - AVERAGE_3D_GRADIENT + * - AVERAGE_DEPTH_CHANGE + */ + NormalEstimationMethod normal_estimation_method_; + + /** \brief The policy for handling borders. */ + BorderPolicy border_policy_; + + /** The width of the neighborhood region used for computing the normal. */ + int rect_width_; + int rect_width_2_; + int rect_width_4_; + /** The height of the neighborhood region used for computing the normal. */ + int rect_height_; + int rect_height_2_; + int rect_height_4_; + + /** the threshold used to detect depth discontinuities */ + float distance_threshold_; + + /** integral image in x-direction */ + IntegralImage2D integral_image_DX_; + /** integral image in y-direction */ + IntegralImage2D integral_image_DY_; + /** integral image */ + IntegralImage2D integral_image_depth_; + /** integral image xyz */ + IntegralImage2D integral_image_XYZ_; + + /** derivatives in x-direction */ + float *diff_x_; + /** derivatives in y-direction */ + float *diff_y_; + + /** depth data */ + float *depth_data_; + + /** distance map */ + float *distance_map_; + + /** \brief Smooth data based on depth (true/false). */ + bool use_depth_dependent_smoothing_; + + /** \brief Threshold for detecting depth discontinuities */ + float max_depth_change_factor_; + + /** \brief */ + float normal_smoothing_size_; + + /** \brief True when a dataset has been received and the covariance_matrix data has been initialized. */ + bool init_covariance_matrix_; + + /** \brief True when a dataset has been received and the average 3d gradient data has been initialized. */ + bool init_average_3d_gradient_; + + /** \brief True when a dataset has been received and the simple 3d gradient data has been initialized. */ + bool init_simple_3d_gradient_; + + /** \brief True when a dataset has been received and the depth change data has been initialized. */ + bool init_depth_change_; + + /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit + * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */ + float vpx_, vpy_, vpz_; + + /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/ + bool use_sensor_origin_; + + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief Internal initialization method for COVARIANCE_MATRIX estimation. */ + void + initCovarianceMatrixMethod (); + + /** \brief Internal initialization method for AVERAGE_3D_GRADIENT estimation. */ + void + initAverage3DGradientMethod (); + + /** \brief Internal initialization method for AVERAGE_DEPTH_CHANGE estimation. */ + void + initAverageDepthChangeMethod (); + + /** \brief Internal initialization method for SIMPLE_3D_GRADIENT estimation. */ + void + initSimple3DGradientMethod (); + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/intensity_gradient.h b/deps_install/include/pcl-1.10/pcl/features/intensity_gradient.h new file mode 100755 index 0000000..07dc44f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/intensity_gradient.h @@ -0,0 +1,117 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position + * and intensity values. The intensity gradient at a given point will be a vector orthogonal to the surface + * normal and pointing in the direction of the greatest increase in local intensity; the vector's magnitude + * indicates the rate of intensity change. + * \author Michael Dixon + * \ingroup features + */ + template > + class IntensityGradientEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::surface_; + using Feature::k_; + using Feature::search_parameter_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Empty constructor. */ + IntensityGradientEstimation () : intensity_ (), threads_ (0) + { + feature_name_ = "IntensityGradientEstimation"; + }; + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + protected: + /** \brief Estimate the intensity gradients for a set of points given in using + * the surface in setSearchSurface () and the spatial locator in setSearchMethod (). + * \param output the resultant point cloud that contains the intensity gradient vectors + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Estimate the intensity gradient around a given point based on its spatial neighborhood of points + * \param cloud a point cloud dataset containing XYZI coordinates (Cartesian coordinates + intensity) + * \param indices the indices of the neighoring points in the dataset + * \param point the 3D Cartesian coordinates of the point at which to estimate the gradient + * \param mean_intensity + * \param normal the 3D surface normal of the given point + * \param gradient the resultant 3D gradient vector + */ + void + computePointIntensityGradient (const pcl::PointCloud &cloud, + const std::vector &indices, + const Eigen::Vector3f &point, + float mean_intensity, + const Eigen::Vector3f &normal, + Eigen::Vector3f &gradient); + + protected: + ///intensity field accessor structure + IntensitySelectorT intensity_; + ///number of threads to be used, default 0 (auto) + unsigned int threads_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/intensity_spin.h b/deps_install/include/pcl-1.10/pcl/features/intensity_spin.h new file mode 100755 index 0000000..14e2449 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/intensity_spin.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief IntensitySpinEstimation estimates the intensity-domain spin image descriptors for a given point cloud + * dataset containing points and intensity. For more information about the intensity-domain spin image descriptor, + * see: + * + * Svetlana Lazebnik, Cordelia Schmid, and Jean Ponce. + * A sparse texture representation using local affine regions. + * In IEEE Transactions on Pattern Analysis and Machine Intelligence, volume 27, pages 1265-1278, August 2005. + * \author Michael Dixon + * \ingroup features + */ + template + class IntensitySpinEstimation: public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + + using Feature::input_; + using Feature::indices_; + using Feature::surface_; + + using Feature::tree_; + using Feature::search_radius_; + + using PointCloudIn = pcl::PointCloud; + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Empty constructor. */ + IntensitySpinEstimation () : nr_distance_bins_ (4), nr_intensity_bins_ (5), sigma_ (1.0) + { + feature_name_ = "IntensitySpinEstimation"; + }; + + /** \brief Estimate the intensity-domain spin image descriptor for a given point based on its spatial + * neighborhood of 3D points and their intensities. + * \param[in] cloud the dataset containing the Cartesian coordinates and intensity values of the points + * \param[in] radius the radius of the feature + * \param[in] sigma the standard deviation of the Gaussian smoothing kernel to use during the soft histogram update + * \param[in] k the number of neighbors to use from \a indices and \a squared_distances + * \param[in] indices the indices of the points that comprise the query point's neighborhood + * \param[in] squared_distances the squared distances from the query point to each point in the neighborhood + * \param[out] intensity_spin_image the resultant intensity-domain spin image descriptor + */ + void + computeIntensitySpinImage (const PointCloudIn &cloud, + float radius, float sigma, int k, + const std::vector &indices, + const std::vector &squared_distances, + Eigen::MatrixXf &intensity_spin_image); + + /** \brief Set the number of bins to use in the distance dimension of the spin image + * \param[in] nr_distance_bins the number of bins to use in the distance dimension of the spin image + */ + inline void + setNrDistanceBins (std::size_t nr_distance_bins) { nr_distance_bins_ = static_cast (nr_distance_bins); }; + + /** \brief Returns the number of bins in the distance dimension of the spin image. */ + inline int + getNrDistanceBins () { return (nr_distance_bins_); }; + + /** \brief Set the number of bins to use in the intensity dimension of the spin image. + * \param[in] nr_intensity_bins the number of bins to use in the intensity dimension of the spin image + */ + inline void + setNrIntensityBins (std::size_t nr_intensity_bins) { nr_intensity_bins_ = static_cast (nr_intensity_bins); }; + + /** \brief Returns the number of bins in the intensity dimension of the spin image. */ + inline int + getNrIntensityBins () { return (nr_intensity_bins_); }; + + /** \brief Set the standard deviation of the Gaussian smoothing kernel to use when constructing the spin images. + * \param[in] sigma the standard deviation of the Gaussian smoothing kernel to use when constructing the spin images + */ + inline void + setSmoothingBandwith (float sigma) { sigma_ = sigma; }; + + /** \brief Returns the standard deviation of the Gaussian smoothing kernel used to construct the spin images. */ + inline float + getSmoothingBandwith () { return (sigma_); }; + + + /** \brief Estimate the intensity-domain descriptors at a set of points given by + * using the surface in setSearchSurface (), and the spatial locator in setSearchMethod (). + * \param[out] output the resultant point cloud model dataset that contains the intensity-domain spin image features + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief The number of distance bins in the descriptor. */ + int nr_distance_bins_; + + /** \brief The number of intensity bins in the descriptor. */ + int nr_intensity_bins_; + + /** \brief The standard deviation of the Gaussian smoothing kernel used to construct the spin images. */ + float sigma_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + + + + diff --git a/deps_install/include/pcl-1.10/pcl/features/linear_least_squares_normal.h b/deps_install/include/pcl-1.10/pcl/features/linear_least_squares_normal.h new file mode 100755 index 0000000..892a805 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/linear_least_squares_normal.h @@ -0,0 +1,148 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylor approximation. + * \author Stefan Holzer, Cedric Cagniart + */ + template + class LinearLeastSquaresNormalEstimation : public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudOut = typename Feature::PointCloudOut; + using Feature::input_; + using Feature::feature_name_; + using Feature::tree_; + using Feature::k_; + + /** \brief Constructor */ + LinearLeastSquaresNormalEstimation () : + use_depth_dependent_smoothing_(false), + max_depth_change_factor_(1.0f), + normal_smoothing_size_(9.0f) + { + feature_name_ = "LinearLeastSquaresNormalEstimation"; + tree_.reset (); + k_ = 1; + }; + + /** \brief Destructor */ + ~LinearLeastSquaresNormalEstimation (); + + /** \brief Computes the normal at the specified position. + * \param[in] pos_x x position (pixel) + * \param[in] pos_y y position (pixel) + * \param[out] normal the output estimated normal + */ + void + computePointNormal (const int pos_x, const int pos_y, PointOutT &normal); + + /** \brief Set the normal smoothing size + * \param[in] normal_smoothing_size factor which influences the size of the area used to smooth normals + * (depth dependent if useDepthDependentSmoothing is true) + */ + void + setNormalSmoothingSize (float normal_smoothing_size) + { + normal_smoothing_size_ = normal_smoothing_size; + } + + /** \brief Set whether to use depth depending smoothing or not + * \param[in] use_depth_dependent_smoothing decides whether the smoothing is depth dependent + */ + void + setDepthDependentSmoothing (bool use_depth_dependent_smoothing) + { + use_depth_dependent_smoothing_ = use_depth_dependent_smoothing; + } + + /** \brief The depth change threshold for computing object borders + * \param[in] max_depth_change_factor the depth change threshold for computing object borders based on + * depth changes + */ + void + setMaxDepthChangeFactor (float max_depth_change_factor) + { + max_depth_change_factor_ = max_depth_change_factor; + } + + /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + inline void + setInputCloud (const typename PointCloudIn::ConstPtr &cloud) override + { + input_ = cloud; + } + + protected: + /** \brief Computes the normal for the complete cloud. + * \param[out] output the resultant normals + */ + void + computeFeature (PointCloudOut &output) override; + + private: + + /** the threshold used to detect depth discontinuities */ + //float distance_threshold_; + + /** \brief Smooth data based on depth (true/false). */ + bool use_depth_dependent_smoothing_; + + /** \brief Threshold for detecting depth discontinuities */ + float max_depth_change_factor_; + + /** \brief */ + float normal_smoothing_size_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/moment_invariants.h b/deps_install/include/pcl-1.10/pcl/features/moment_invariants.h new file mode 100755 index 0000000..c744bd5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/moment_invariants.h @@ -0,0 +1,117 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief MomentInvariantsEstimation estimates the 3 moment invariants (j1, j2, j3) at each 3D point. + * + * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \ref NormalEstimationOMP for an example on how to extend this to parallel implementations. + * \author Radu B. Rusu + * \ingroup features + */ + template + class MomentInvariantsEstimation: public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::surface_; + using Feature::input_; + + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Empty constructor. */ + MomentInvariantsEstimation () + { + feature_name_ = "MomentInvariantsEstimation"; + }; + + /** \brief Compute the 3 moment invariants (j1, j2, j3) for a given set of points, using their indices. + * \param[in] cloud the input point cloud + * \param[in] indices the point cloud indices that need to be used + * \param[out] j1 the resultant first moment invariant + * \param[out] j2 the resultant second moment invariant + * \param[out] j3 the resultant third moment invariant + */ + void + computePointMomentInvariants (const pcl::PointCloud &cloud, + const std::vector &indices, + float &j1, float &j2, float &j3); + + /** \brief Compute the 3 moment invariants (j1, j2, j3) for a given set of points, using their indices. + * \param[in] cloud the input point cloud + * \param[out] j1 the resultant first moment invariant + * \param[out] j2 the resultant second moment invariant + * \param[out] j3 the resultant third moment invariant + */ + void + computePointMomentInvariants (const pcl::PointCloud &cloud, + float &j1, float &j2, float &j3); + + protected: + + /** \brief Estimate moment invariants for all points given in using the surface + * in setSearchSurface () and the spatial locator in setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains the moment invariants + */ + void + computeFeature (PointCloudOut &output) override; + private: + /** \brief 16-bytes aligned placeholder for the XYZ centroid of a surface patch. */ + Eigen::Vector4f xyz_centroid_; + + /** \brief Internal data vector. */ + Eigen::Vector4f temp_pt_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/moment_of_inertia_estimation.h b/deps_install/include/pcl-1.10/pcl/features/moment_of_inertia_estimation.h new file mode 100755 index 0000000..b662d86 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/moment_of_inertia_estimation.h @@ -0,0 +1,360 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * Implements the method for extracting features based on moment of inertia. It also + * calculates AABB, OBB and eccentricity of the projected cloud. + */ + template + class PCL_EXPORTS MomentOfInertiaEstimation : public pcl::PCLBase + { + public: + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::fake_indices_; + using PCLBase ::use_indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + using PointCloudConstPtr = typename pcl::PCLBase::PointCloudConstPtr; + using PointIndicesConstPtr = typename pcl::PCLBase::PointIndicesConstPtr; + + public: + + /** \brief Provide a pointer to the input dataset + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + void + setInputCloud (const PointCloudConstPtr& cloud) override; + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the vector of indices that represents the input data. + */ + void + setIndices (const IndicesPtr& indices) override; + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the vector of indices that represents the input data. + */ + void + setIndices (const IndicesConstPtr& indices) override; + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the vector of indices that represents the input data. + */ + void + setIndices (const PointIndicesConstPtr& indices) override; + + /** \brief Set the indices for the points laying within an interest region of + * the point cloud. + * \note you shouldn't call this method on unorganized point clouds! + * \param[in] row_start the offset on rows + * \param[in] col_start the offset on columns + * \param[in] nb_rows the number of rows to be considered row_start included + * \param[in] nb_cols the number of columns to be considered col_start included + */ + void + setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols) override; + + /** \brief Constructor that sets default values for member variables. */ + MomentOfInertiaEstimation (); + + /** \brief Virtual destructor which frees the memory. */ + + ~MomentOfInertiaEstimation (); + + /** \brief This method allows to set the angle step. It is used for the rotation + * of the axis which is used for moment of inertia/eccentricity calculation. + * \param[in] step angle step + */ + void + setAngleStep (const float step); + + /** \brief Returns the angle step. */ + float + getAngleStep () const; + + /** \brief This method allows to set the normalize_ flag. If set to false, then + * point_mass_ will be used to scale the moment of inertia values. Otherwise, + * point_mass_ will be set to 1 / number_of_points. Default value is true. + * \param[in] need_to_normalize desired value + */ + void + setNormalizePointMassFlag (bool need_to_normalize); + + /** \brief Returns the normalize_ flag. */ + bool + getNormalizePointMassFlag () const; + + /** \brief This method allows to set point mass that will be used for + * moment of inertia calculation. It is needed to scale moment of inertia values. + * default value is 0.0001. + * \param[in] point_mass point mass + */ + void + setPointMass (const float point_mass); + + /** \brief Returns the mass of point. */ + float + getPointMass () const; + + /** \brief This method launches the computation of all features. After execution + * it sets is_valid_ flag to true and each feature can be accessed with the + * corresponding get method. + */ + void + compute (); + + /** \brief This method gives access to the computed axis aligned bounding box. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] min_point min point of the AABB + * \param[out] max_point max point of the AABB + */ + bool + getAABB (PointT& min_point, PointT& max_point) const; + + /** \brief This method gives access to the computed oriented bounding box. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * Note that in order to get the OBB, each vertex of the given AABB (specified with min_point and max_point) + * must be rotated with the given rotational matrix (rotation transform) and then positioned. + * Also pay attention to the fact that this is not the minimal possible bounding box. This is the bounding box + * which is oriented in accordance with the eigen vectors. + * \param[out] min_point min point of the OBB + * \param[out] max_point max point of the OBB + * \param[out] position position of the OBB + * \param[out] rotational_matrix this matrix represents the rotation transform + */ + bool + getOBB (PointT& min_point, PointT& max_point, PointT& position, Eigen::Matrix3f& rotational_matrix) const; + + /** \brief This method gives access to the computed eigen values. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] major major eigen value + * \param[out] middle middle eigen value + * \param[out] minor minor eigen value + */ + bool + getEigenValues (float& major, float& middle, float& minor) const; + + /** \brief This method gives access to the computed eigen vectors. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] major axis which corresponds to the eigen vector with the major eigen value + * \param[out] middle axis which corresponds to the eigen vector with the middle eigen value + * \param[out] minor axis which corresponds to the eigen vector with the minor eigen value + */ + bool + getEigenVectors (Eigen::Vector3f& major, Eigen::Vector3f& middle, Eigen::Vector3f& minor) const; + + /** \brief This method gives access to the computed moments of inertia. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] moment_of_inertia computed moments of inertia + */ + bool + getMomentOfInertia (std::vector & moment_of_inertia) const; + + /** \brief This method gives access to the computed ecentricities. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * \param[out] eccentricity computed eccentricities + */ + bool + getEccentricity (std::vector & eccentricity) const; + + /** \brief This method gives access to the computed mass center. It returns true + * if the current values (eccentricity, moment of inertia etc) are valid and false otherwise. + * Note that when mass center of a cloud is computed, mass point is always considered equal 1. + * \param[out] mass_center computed mass center + */ + bool + getMassCenter (Eigen::Vector3f& mass_center) const; + + private: + + /** \brief This method rotates the given vector around the given axis. + * \param[in] vector vector that must be rotated + * \param[in] axis axis around which vector must be rotated + * \param[in] angle angle in degrees + * \param[out] rotated_vector resultant vector + */ + void + rotateVector (const Eigen::Vector3f& vector, const Eigen::Vector3f& axis, const float angle, Eigen::Vector3f& rotated_vector) const; + + /** \brief This method computes center of mass and axis aligned bounding box. */ + void + computeMeanValue (); + + /** \brief This method computes the oriented bounding box. */ + void + computeOBB (); + + /** \brief This method computes the covariance matrix for the input_ cloud. + * \param[out] covariance_matrix stores the computed covariance matrix + */ + void + computeCovarianceMatrix (Eigen::Matrix & covariance_matrix) const; + + /** \brief This method computes the covariance matrix for the given cloud. + * It uses all points in the cloud, unlike the previous method that uses indices. + * \param[in] cloud cloud for which covariance matrix will be computed + * \param[out] covariance_matrix stores the computed covariance matrix + */ + void + computeCovarianceMatrix (PointCloudConstPtr cloud, Eigen::Matrix & covariance_matrix) const; + + /** \brief This method calculates the eigen values and eigen vectors + * for the given covariance matrix. Note that it returns normalized eigen + * vectors that always form the right-handed coordinate system. + * \param[in] covariance_matrix covariance matrix + * \param[out] major_axis eigen vector which corresponds to a major eigen value + * \param[out] middle_axis eigen vector which corresponds to a middle eigen value + * \param[out] minor_axis eigen vector which corresponds to a minor eigen value + * \param[out] major_value major eigen value + * \param[out] middle_value middle eigen value + * \param[out] minor_value minor eigen value + */ + void + computeEigenVectors (const Eigen::Matrix & covariance_matrix, Eigen::Vector3f& major_axis, + Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, float& major_value, float& middle_value, + float& minor_value); + + /** \brief This method returns the moment of inertia of a given input_ cloud. + * Note that when moment of inertia is computed it is multiplied by the point mass. + * Point mass can be accessed with the corresponding get/set methods. + * \param[in] current_axis axis that will be used in moment of inertia computation + * \param[in] mean_value mean value(center of mass) of the cloud + */ + float + calculateMomentOfInertia (const Eigen::Vector3f& current_axis, const Eigen::Vector3f& mean_value) const; + + /** \brief This method simply projects the given input_ cloud on the plane specified with + * the normal vector. + * \param[in] normal_vector nrmal vector of the plane + * \param[in] point point belonging to the plane + * \param[out] projected_cloud projected cloud + */ + void + getProjectedCloud (const Eigen::Vector3f& normal_vector, const Eigen::Vector3f& point, typename pcl::PointCloud ::Ptr projected_cloud) const; + + /** \brief This method returns the eccentricity of the projected cloud. + * \param[in] covariance_matrix covariance matrix of the projected cloud + * \param[in] normal_vector normal vector of the plane, it is used to discard the + * third eigen vector and eigen value*/ + float + computeEccentricity (const Eigen::Matrix & covariance_matrix, const Eigen::Vector3f& normal_vector); + + private: + + /** \brief Indicates if the stored values (eccentricity, moment of inertia, AABB etc.) + * are valid when accessed with the get methods. */ + bool is_valid_; + + /** \brief Stores the angle step */ + float step_; + + /** \brief Stores the mass of point in the cloud */ + float point_mass_; + + /** \brief Stores the flag for mass normalization */ + bool normalize_; + + /** \brief Stores the mean value (center of mass) of the cloud */ + Eigen::Vector3f mean_value_; + + /** \brief Major eigen vector */ + Eigen::Vector3f major_axis_; + + /** \brief Middle eigen vector */ + Eigen::Vector3f middle_axis_; + + /** \brief Minor eigen vector */ + Eigen::Vector3f minor_axis_; + + /** \brief Major eigen value */ + float major_value_; + + /** \brief Middle eigen value */ + float middle_value_; + + /** \brief Minor eigen value */ + float minor_value_; + + /** \brief Stores calculated moments of inertia */ + std::vector moment_of_inertia_; + + /** \brief Stores calculated eccentricities */ + std::vector eccentricity_; + + /** \brief Min point of the axis aligned bounding box */ + PointT aabb_min_point_; + + /** \brief Max point of the axis aligned bounding box */ + PointT aabb_max_point_; + + /** \brief Min point of the oriented bounding box */ + PointT obb_min_point_; + + /** \brief Max point of the oriented bounding box */ + PointT obb_max_point_; + + /** \brief Stores position of the oriented bounding box */ + Eigen::Vector3f obb_position_; + + /** \brief Stores the rotational matrix of the oriented bounding box */ + Eigen::Matrix3f obb_rotational_matrix_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#define PCL_INSTANTIATE_MomentOfInertiaEstimation(T) template class pcl::MomentOfInertiaEstimation; + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/multiscale_feature_persistence.h b/deps_install/include/pcl-1.10/pcl/features/multiscale_feature_persistence.h new file mode 100755 index 0000000..ca55b60 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/multiscale_feature_persistence.h @@ -0,0 +1,206 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Generic class for extracting the persistent features from an input point cloud + * It can be given any Feature estimator instance and will compute the features of the input + * over a multiscale representation of the cloud and output the unique ones over those scales. + * + * Please refer to the following publication for more details: + * Radu Bogdan Rusu, Zoltan Csaba Marton, Nico Blodow, and Michael Beetz + * Persistent Point Feature Histograms for 3D Point Clouds + * Proceedings of the 10th International Conference on Intelligent Autonomous Systems (IAS-10) + * 2008, Baden-Baden, Germany + * + * \author Alexandru-Eugen Ichim + */ + template + class MultiscaleFeaturePersistence : public PCLBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using FeatureCloud = pcl::PointCloud; + using FeatureCloudPtr = typename pcl::PointCloud::Ptr; + using FeatureEstimatorPtr = typename pcl::Feature::Ptr; + using FeatureRepresentationConstPtr = typename pcl::PointRepresentation::ConstPtr; + + using pcl::PCLBase::input_; + + /** \brief Empty constructor */ + MultiscaleFeaturePersistence (); + + /** \brief Empty destructor */ + ~MultiscaleFeaturePersistence () {} + + /** \brief Method that calls computeFeatureAtScale () for each scale parameter */ + void + computeFeaturesAtAllScales (); + + /** \brief Central function that computes the persistent features + * \param output_features a cloud containing the persistent features + * \param output_indices vector containing the indices of the points in the input cloud + * that have persistent features, under a one-to-one correspondence with the output_features cloud + */ + void + determinePersistentFeatures (FeatureCloud &output_features, + shared_ptr > &output_indices); + + /** \brief Method for setting the scale parameters for the algorithm + * \param scale_values vector of scales to determine the characteristic of each scaling step + */ + inline void + setScalesVector (std::vector &scale_values) { scale_values_ = scale_values; } + + /** \brief Method for getting the scale parameters vector */ + inline std::vector + getScalesVector () { return scale_values_; } + + /** \brief Setter method for the feature estimator + * \param feature_estimator pointer to the feature estimator instance that will be used + * \note the feature estimator instance should already have the input data given beforehand + * and everything set, ready to be given the compute () command + */ + inline void + setFeatureEstimator (FeatureEstimatorPtr feature_estimator) { feature_estimator_ = feature_estimator; }; + + /** \brief Getter method for the feature estimator */ + inline FeatureEstimatorPtr + getFeatureEstimator () { return feature_estimator_; } + + /** \brief Provide a pointer to the feature representation to use to convert features to k-D vectors. + * \param feature_representation the const boost shared pointer to a PointRepresentation + */ + inline void + setPointRepresentation (const FeatureRepresentationConstPtr& feature_representation) { feature_representation_ = feature_representation; } + + /** \brief Get a pointer to the feature representation used when converting features into k-D vectors. */ + inline FeatureRepresentationConstPtr const + getPointRepresentation () { return feature_representation_; } + + /** \brief Sets the alpha parameter + * \param alpha value to replace the current alpha with + */ + inline void + setAlpha (float alpha) { alpha_ = alpha; } + + /** \brief Get the value of the alpha parameter */ + inline float + getAlpha () { return alpha_; } + + /** \brief Method for setting the distance metric that will be used for computing the difference between feature vectors + * \param distance_metric the new distance metric chosen from the NormType enum + */ + inline void + setDistanceMetric (NormType distance_metric) { distance_metric_ = distance_metric; } + + /** \brief Returns the distance metric that is currently used to calculate the difference between feature vectors */ + inline NormType + getDistanceMetric () { return distance_metric_; } + + + private: + /** \brief Checks if all the necessary input was given and the computations can successfully start */ + bool + initCompute (); + + + /** \brief Method to compute the features for the point cloud at the given scale */ + virtual void + computeFeatureAtScale (float &scale, + FeatureCloudPtr &features); + + + /** \brief Function that calculates the scalar difference between two features + * \return the difference as a floating point type + */ + float + distanceBetweenFeatures (const std::vector &a, + const std::vector &b); + + /** \brief Method that averages all the features at all scales in order to obtain the global mean feature; + * this value is stored in the mean_feature field + */ + void + calculateMeanFeature (); + + /** \brief Selects the so-called 'unique' features from the cloud of features at each level. + * These features are the ones that fall outside the standard deviation * alpha_ + */ + void + extractUniqueFeatures (); + + + /** \brief The general parameter for determining each scale level */ + std::vector scale_values_; + + /** \brief Parameter that determines if a feature is to be considered unique or not */ + float alpha_; + + /** \brief Parameter that determines which distance metric is to be usedto calculate the difference between feature vectors */ + NormType distance_metric_; + + /** \brief the feature estimator that will be used to determine the feature set at each scale level */ + FeatureEstimatorPtr feature_estimator_; + + std::vector features_at_scale_; + std::vector > > features_at_scale_vectorized_; + std::vector mean_feature_; + FeatureRepresentationConstPtr feature_representation_; + + /** \brief Two structures in which to hold the results of the unique feature extraction process. + * They are superfluous with respect to each other, but improve the time performance of the algorithm + */ + std::vector > unique_features_indices_; + std::vector > unique_features_table_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/narf.h b/deps_install/include/pcl-1.10/pcl/features/narf.h new file mode 100755 index 0000000..10afe01 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/narf.h @@ -0,0 +1,291 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + // Forward declarations + class RangeImage; + struct InterestPoint; + +#define NARF_DEFAULT_SURFACE_PATCH_PIXEL_SIZE 10 + + /** + * \brief NARF (Normal Aligned Radial Features) is a point feature descriptor type for 3D data. + * Please refer to pcl/features/narf_descriptor.h if you want the class derived from pcl Feature. + * See B. Steder, R. B. Rusu, K. Konolige, and W. Burgard + * Point Feature Extraction on 3D Range Scans Taking into Account Object Boundaries + * In Proc. of the IEEE Int. Conf. on Robotics &Automation (ICRA). 2011. + * \author Bastian Steder + * \ingroup features + */ + class PCL_EXPORTS Narf + { + public: + // =====CONSTRUCTOR & DESTRUCTOR===== + //! Constructor + Narf(); + //! Copy Constructor + Narf(const Narf& other); + //! Destructor + ~Narf(); + + // =====Operators===== + //! Assignment operator + const Narf& operator=(const Narf& other); + + // =====STATIC===== + /** The maximum number of openmp threads that can be used in this class */ + static int max_no_of_threads; + + /** Add features extracted at the given interest point and add them to the list */ + static void + extractFromRangeImageAndAddToList (const RangeImage& range_image, const Eigen::Vector3f& interest_point, int descriptor_size, + float support_size, bool rotation_invariant, std::vector& feature_list); + /** Same as above */ + static void + extractFromRangeImageAndAddToList (const RangeImage& range_image, float image_x, float image_y, int descriptor_size, + float support_size, bool rotation_invariant, std::vector& feature_list); + /** Get a list of features from the given interest points. */ + static void + extractForInterestPoints (const RangeImage& range_image, const PointCloud& interest_points, + int descriptor_size, float support_size, bool rotation_invariant, std::vector& feature_list); + /** Extract an NARF for every point in the range image. */ + static void + extractForEveryRangeImagePointAndAddToList (const RangeImage& range_image, int descriptor_size, float support_size, + bool rotation_invariant, std::vector& feature_list); + + // =====PUBLIC METHODS===== + /** Method to extract a NARF feature from a certain 3D point using a range image. + * pose determines the coordinate system of the feature, whereas it transforms a point from the world into the feature system. + * This means the interest point at which the feature is extracted will be the inverse application of pose onto (0,0,0). + * descriptor_size_ determines the size of the descriptor, + * support_size determines the support size of the feature, meaning the size in the world it covers */ + bool + extractFromRangeImage (const RangeImage& range_image, const Eigen::Affine3f& pose, int descriptor_size, float support_size, + int surface_patch_world_size=NARF_DEFAULT_SURFACE_PATCH_PIXEL_SIZE); + + //! Same as above, but determines the transformation from the surface in the range image + bool + extractFromRangeImage (const RangeImage& range_image, float x, float y, int descriptor_size, float support_size); + + //! Same as above + bool + extractFromRangeImage (const RangeImage& range_image, const InterestPoint& interest_point, int descriptor_size, float support_size); + + //! Same as above + bool + extractFromRangeImage (const RangeImage& range_image, const Eigen::Vector3f& interest_point, int descriptor_size, float support_size); + + /** Same as above, but using the rotational invariant version by choosing the best extracted rotation around the normal. + * Use extractFromRangeImageAndAddToList if you want to enable the system to return multiple features with different rotations. */ + bool + extractFromRangeImageWithBestRotation (const RangeImage& range_image, const Eigen::Vector3f& interest_point, + int descriptor_size, float support_size); + + /* Get the dominant rotations of the current descriptor + * \param rotations the returned rotations + * \param strength values describing how pronounced the corresponding rotations are + */ + void + getRotations (std::vector& rotations, std::vector& strengths) const; + + /* Get the feature with a different rotation around the normal + * You are responsible for deleting the new features! + * \param range_image the source from which the feature is extracted + * \param rotations list of angles (in radians) + * \param rvps returned features + */ + void + getRotatedVersions (const RangeImage& range_image, const std::vector& rotations, std::vector& features) const; + + //! Calculate descriptor distance, value in [0,1] with 0 meaning identical and 1 every cell above maximum distance + inline float + getDescriptorDistance (const Narf& other) const; + + //! How many points on each beam of the gradient star are used to calculate the descriptor? + inline int + getNoOfBeamPoints () const { return (static_cast (pcl_lrint (std::ceil (0.5f * float (surface_patch_pixel_size_))))); } + + //! Copy the descriptor and pose to the point struct Narf36 + inline void + copyToNarf36 (Narf36& narf36) const; + + /** Write to file */ + void + saveBinary (const std::string& filename) const; + /** Write to output stream */ + void + saveBinary (std::ostream& file) const; + + /** Read from file */ + void + loadBinary (const std::string& filename); + /** Read from input stream */ + void + loadBinary (std::istream& file); + + //! Create the descriptor from the already set other members + bool + extractDescriptor (int descriptor_size); + + // =====GETTERS===== + //! Getter (const) for the descriptor + inline const float* + getDescriptor () const { return descriptor_;} + //! Getter for the descriptor + inline float* + getDescriptor () { return descriptor_;} + //! Getter (const) for the descriptor length + inline const int& + getDescriptorSize () const { return descriptor_size_;} + //! Getter for the descriptor length + inline int& + getDescriptorSize () { return descriptor_size_;} + //! Getter (const) for the position + inline const Eigen::Vector3f& + getPosition () const { return position_;} + //! Getter for the position + inline Eigen::Vector3f& + getPosition () { return position_;} + //! Getter (const) for the 6DoF pose + inline const Eigen::Affine3f& + getTransformation () const { return transformation_;} + //! Getter for the 6DoF pose + inline Eigen::Affine3f& + getTransformation () { return transformation_;} + //! Getter (const) for the pixel size of the surface patch (only one dimension) + inline const int& + getSurfacePatchPixelSize () const { return surface_patch_pixel_size_;} + //! Getter for the pixel size of the surface patch (only one dimension) + inline int& + getSurfacePatchPixelSize () { return surface_patch_pixel_size_;} + //! Getter (const) for the world size of the surface patch + inline const float& + getSurfacePatchWorldSize () const { return surface_patch_world_size_;} + //! Getter for the world size of the surface patch + inline float& + getSurfacePatchWorldSize () { return surface_patch_world_size_;} + //! Getter (const) for the rotation of the surface patch + inline const float& + getSurfacePatchRotation () const { return surface_patch_rotation_;} + //! Getter for the rotation of the surface patch + inline float& + getSurfacePatchRotation () { return surface_patch_rotation_;} + //! Getter (const) for the surface patch + inline const float* + getSurfacePatch () const { return surface_patch_;} + //! Getter for the surface patch + inline float* + getSurfacePatch () { return surface_patch_;} + //! Method to erase the surface patch and free the memory + inline void + freeSurfacePatch () { delete[] surface_patch_; surface_patch_=nullptr; surface_patch_pixel_size_=0; } + + // =====SETTERS===== + //! Setter for the descriptor + inline void + setDescriptor (float* descriptor) { descriptor_ = descriptor;} + //! Setter for the surface patch + inline void + setSurfacePatch (float* surface_patch) { surface_patch_ = surface_patch;} + + // =====PUBLIC MEMBER VARIABLES===== + + // =====PUBLIC STRUCTS===== + struct FeaturePointRepresentation : public PointRepresentation + { + using PointT = Narf *; + FeaturePointRepresentation(int nr_dimensions) { this->nr_dimensions_ = nr_dimensions; } + /** \brief Empty destructor */ + ~FeaturePointRepresentation () {} + void copyToFloatArray (const PointT& p, float* out) const override { memcpy(out, p->getDescriptor(), sizeof(*p->getDescriptor())*this->nr_dimensions_); } + }; + + protected: + // =====PROTECTED METHODS===== + //! Reset al members to default values and free allocated memory + void + reset (); + //! Create a deep copy of other + void + deepCopy (const Narf& other); + //! Get the surface patch with a blur on it + float* + getBlurredSurfacePatch (int new_pixel_size, int blur_radius) const; + + /** Write header to output stream */ + void + saveHeader (std::ostream& file) const; + /** Read header from input stream */ + int + loadHeader (std::istream& file) const; + + // =====PROTECTED STATIC METHODS===== + static const std::string + getHeaderKeyword () { return "NARF"; } + + // =====PROTECTED STATIC VARIABLES===== + const static int VERSION = 1; + + // =====PROTECTED MEMBER VARIABLES===== + Eigen::Vector3f position_; + Eigen::Affine3f transformation_; + float* surface_patch_; + int surface_patch_pixel_size_; + float surface_patch_world_size_; + float surface_patch_rotation_; + float* descriptor_; + int descriptor_size_; + + // =====STATIC PROTECTED===== + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +#undef NARF_DEFAULT_SURFACE_PATCH_PIXEL_SIZE + +} // end namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/features/narf_descriptor.h b/deps_install/include/pcl-1.10/pcl/features/narf_descriptor.h new file mode 100755 index 0000000..b2b7c46 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/narf_descriptor.h @@ -0,0 +1,102 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +namespace pcl +{ + // Forward declarations + class RangeImage; + + /** @b Computes NARF feature descriptors for points in a range image + * See B. Steder, R. B. Rusu, K. Konolige, and W. Burgard + * Point Feature Extraction on 3D Range Scans Taking into Account Object Boundaries + * In Proc. of the IEEE Int. Conf. on Robotics &Automation (ICRA). 2011. + * \author Bastian Steder + * \ingroup features + */ + class PCL_EXPORTS NarfDescriptor : public Feature + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + // =====TYPEDEFS===== + using BaseClass = Feature; + + // =====STRUCTS/CLASSES===== + struct Parameters + { + Parameters() : support_size(-1.0f), rotation_invariant(true) {} + float support_size; + bool rotation_invariant; + }; + + // =====CONSTRUCTOR & DESTRUCTOR===== + /** Constructor */ + NarfDescriptor (const RangeImage* range_image=nullptr, const std::vector* indices=nullptr); + /** Destructor */ + ~NarfDescriptor(); + + // =====METHODS===== + //! Set input data + void + setRangeImage (const RangeImage* range_image, const std::vector* indices=nullptr); + + //! Overwrite the compute function of the base class + void + compute (PointCloudOut& output); + + // =====GETTER===== + //! Get a reference to the parameters struct + Parameters& + getParameters () { return parameters_;} + + protected: + // =====PROTECTED MEMBER VARIABLES===== + const RangeImage* range_image_; + Parameters parameters_; + + // =====PROTECTED METHODS===== + /** Implementation of abstract derived function */ + void + computeFeature (PointCloudOut& output) override; + }; + +} // namespace end diff --git a/deps_install/include/pcl-1.10/pcl/features/normal_3d.h b/deps_install/include/pcl-1.10/pcl/features/normal_3d.h new file mode 100755 index 0000000..a83fc0c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/normal_3d.h @@ -0,0 +1,423 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Compute the Least-Squares plane fit for a given set of points, and return the estimated plane + * parameters together with the surface curvature. + * \param cloud the input point cloud + * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) + * \param curvature the estimated surface curvature as a measure of + * \f[ + * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) + * \f] + * \ingroup features + */ + template inline bool + computePointNormal (const pcl::PointCloud &cloud, + Eigen::Vector4f &plane_parameters, float &curvature) + { + // Placeholder for the 3x3 covariance matrix at each surface patch + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + // 16-bytes aligned placeholder for the XYZ centroid of a surface patch + Eigen::Vector4f xyz_centroid; + + if (cloud.size () < 3 || + computeMeanAndCovarianceMatrix (cloud, covariance_matrix, xyz_centroid) == 0) + { + plane_parameters.setConstant (std::numeric_limits::quiet_NaN ()); + curvature = std::numeric_limits::quiet_NaN (); + return false; + } + + // Get the plane normal and surface curvature + solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); + return true; + } + + /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, + * and return the estimated plane parameters together with the surface curvature. + * \param cloud the input point cloud + * \param indices the point cloud indices that need to be used + * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) + * \param curvature the estimated surface curvature as a measure of + * \f[ + * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) + * \f] + * \ingroup features + */ + template inline bool + computePointNormal (const pcl::PointCloud &cloud, const std::vector &indices, + Eigen::Vector4f &plane_parameters, float &curvature) + { + // Placeholder for the 3x3 covariance matrix at each surface patch + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + // 16-bytes aligned placeholder for the XYZ centroid of a surface patch + Eigen::Vector4f xyz_centroid; + if (indices.size () < 3 || + computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0) + { + plane_parameters.setConstant (std::numeric_limits::quiet_NaN ()); + curvature = std::numeric_limits::quiet_NaN (); + return false; + } + // Get the plane normal and surface curvature + solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); + return true; + } + + /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint + * \param point a given point + * \param vp_x the X coordinate of the viewpoint + * \param vp_y the X coordinate of the viewpoint + * \param vp_z the X coordinate of the viewpoint + * \param normal the plane normal to be flipped + * \ingroup features + */ + template inline void + flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, + Eigen::Matrix& normal) + { + Eigen::Matrix vp (vp_x - point.x, vp_y - point.y, vp_z - point.z, 0); + + // Dot product between the (viewpoint - point) and the plane normal + float cos_theta = vp.dot (normal); + + // Flip the plane normal + if (cos_theta < 0) + { + normal *= -1; + normal[3] = 0.0f; + // Hessian form (D = nc . p_plane (centroid here) + p) + normal[3] = -1 * normal.dot (point.getVector4fMap ()); + } + } + + /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint + * \param point a given point + * \param vp_x the X coordinate of the viewpoint + * \param vp_y the X coordinate of the viewpoint + * \param vp_z the X coordinate of the viewpoint + * \param normal the plane normal to be flipped + * \ingroup features + */ + template inline void + flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, + Eigen::Matrix& normal) + { + Eigen::Matrix vp (vp_x - point.x, vp_y - point.y, vp_z - point.z); + + // Flip the plane normal + if (vp.dot (normal) < 0) + normal *= -1; + } + + /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint + * \param point a given point + * \param vp_x the X coordinate of the viewpoint + * \param vp_y the X coordinate of the viewpoint + * \param vp_z the X coordinate of the viewpoint + * \param nx the resultant X component of the plane normal + * \param ny the resultant Y component of the plane normal + * \param nz the resultant Z component of the plane normal + * \ingroup features + */ + template inline void + flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, + float &nx, float &ny, float &nz) + { + // See if we need to flip any plane normals + vp_x -= point.x; + vp_y -= point.y; + vp_z -= point.z; + + // Dot product between the (viewpoint - point) and the plane normal + float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); + + // Flip the plane normal + if (cos_theta < 0) + { + nx *= -1; + ny *= -1; + nz *= -1; + } + } + + /** \brief Flip (in place) normal to get the same sign of the mean of the normals specified by normal_indices. + * + * The method is described in: + * A. Petrelli, L. Di Stefano, "A repeatable and efficient canonical reference for surface matching", 3DimPVT, 2012 + * A. Petrelli, L. Di Stefano, "On the repeatability of the local reference frame for partial shape matching", 13th International Conference on Computer Vision (ICCV), 2011 + * + * Normals should be unit vectors. Otherwise the resulting mean would be weighted by the normal norms. + * \param[in] normal_cloud Cloud of normals used to compute the mean + * \param[in] normal_indices Indices of normals used to compute the mean + * \param[in] normal input Normal to flip. Normal is modified by the function. + * \return false if normal_indices does not contain any valid normal. + * \ingroup features + */ + template inline bool + flipNormalTowardsNormalsMean ( pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal) + { + Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero (); + + for (const int &normal_index : normal_indices) + { + const PointNT& cur_pt = normal_cloud[normal_index]; + + if (pcl::isFinite (cur_pt)) + { + normal_mean += cur_pt.getNormalVector3fMap (); + } + } + + if (normal_mean.isZero ()) + return false; + + normal_mean.normalize (); + + if (normal.dot (normal_mean) < 0) + { + normal = -normal; + } + + return true; + } + + /** \brief NormalEstimation estimates local surface properties (surface normals and curvatures)at each + * 3D point. If PointOutT is specified as pcl::Normal, the normal is stored in the first 3 components (0-2), + * and the curvature is stored in component 3. + * + * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \ref NormalEstimationOMP for a parallel implementation. + * \author Radu B. Rusu + * \ingroup features + */ + template + class NormalEstimation: public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::input_; + using Feature::surface_; + using Feature::k_; + using Feature::search_radius_; + using Feature::search_parameter_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudConstPtr = typename Feature::PointCloudConstPtr; + + /** \brief Empty constructor. */ + NormalEstimation () + : vpx_ (0) + , vpy_ (0) + , vpz_ (0) + , use_sensor_origin_ (true) + { + feature_name_ = "NormalEstimation"; + }; + + /** \brief Empty destructor */ + ~NormalEstimation () {} + + /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, + * and return the estimated plane parameters together with the surface curvature. + * \param cloud the input point cloud + * \param indices the point cloud indices that need to be used + * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) + * \param curvature the estimated surface curvature as a measure of + * \f[ + * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) + * \f] + */ + inline bool + computePointNormal (const pcl::PointCloud &cloud, const std::vector &indices, + Eigen::Vector4f &plane_parameters, float &curvature) + { + if (indices.size () < 3 || + computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) + { + plane_parameters.setConstant (std::numeric_limits::quiet_NaN ()); + curvature = std::numeric_limits::quiet_NaN (); + return false; + } + + // Get the plane normal and surface curvature + solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature); + return true; + } + + /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, + * and return the estimated plane parameters together with the surface curvature. + * \param cloud the input point cloud + * \param indices the point cloud indices that need to be used + * \param nx the resultant X component of the plane normal + * \param ny the resultant Y component of the plane normal + * \param nz the resultant Z component of the plane normal + * \param curvature the estimated surface curvature as a measure of + * \f[ + * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) + * \f] + */ + inline bool + computePointNormal (const pcl::PointCloud &cloud, const std::vector &indices, + float &nx, float &ny, float &nz, float &curvature) + { + if (indices.size () < 3 || + computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) + { + nx = ny = nz = curvature = std::numeric_limits::quiet_NaN (); + return false; + } + + // Get the plane normal and surface curvature + solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature); + return true; + } + + /** \brief Provide a pointer to the input dataset + * \param cloud the const boost shared pointer to a PointCloud message + */ + inline void + setInputCloud (const PointCloudConstPtr &cloud) override + { + input_ = cloud; + if (use_sensor_origin_) + { + vpx_ = input_->sensor_origin_.coeff (0); + vpy_ = input_->sensor_origin_.coeff (1); + vpz_ = input_->sensor_origin_.coeff (2); + } + } + + /** \brief Set the viewpoint. + * \param vpx the X coordinate of the viewpoint + * \param vpy the Y coordinate of the viewpoint + * \param vpz the Z coordinate of the viewpoint + */ + inline void + setViewPoint (float vpx, float vpy, float vpz) + { + vpx_ = vpx; + vpy_ = vpy; + vpz_ = vpz; + use_sensor_origin_ = false; + } + + /** \brief Get the viewpoint. + * \param [out] vpx x-coordinate of the view point + * \param [out] vpy y-coordinate of the view point + * \param [out] vpz z-coordinate of the view point + * \note this method returns the currently used viewpoint for normal flipping. + * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates. + * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0) + */ + inline void + getViewPoint (float &vpx, float &vpy, float &vpz) + { + vpx = vpx_; + vpy = vpy_; + vpz = vpz_; + } + + /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the + * normal estimation method uses the sensor origin of the input cloud. + * to use a user defined view point, use the method setViewPoint + */ + inline void + useSensorOriginAsViewPoint () + { + use_sensor_origin_ = true; + if (input_) + { + vpx_ = input_->sensor_origin_.coeff (0); + vpy_ = input_->sensor_origin_.coeff (1); + vpz_ = input_->sensor_origin_.coeff (2); + } + else + { + vpx_ = 0; + vpy_ = 0; + vpz_ = 0; + } + } + + protected: + /** \brief Estimate normals for all points given in using the surface in + * setSearchSurface () and the spatial locator in setSearchMethod () + * \note In situations where not enough neighbors are found, the normal and curvature values are set to NaN. + * \param output the resultant point cloud model dataset that contains surface normals and curvatures + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit + * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */ + float vpx_, vpy_, vpz_; + + /** \brief Placeholder for the 3x3 covariance matrix at each surface patch. */ + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_; + + /** \brief 16-bytes aligned placeholder for the XYZ centroid of a surface patch. */ + Eigen::Vector4f xyz_centroid_; + + /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/ + bool use_sensor_origin_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/normal_3d_omp.h b/deps_install/include/pcl-1.10/pcl/features/normal_3d_omp.h new file mode 100755 index 0000000..a8ae45b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/normal_3d_omp.h @@ -0,0 +1,105 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief NormalEstimationOMP estimates local surface properties at each 3D point, such as surface normals and + * curvatures, in parallel, using the OpenMP standard. + * \author Radu Bogdan Rusu + * \ingroup features + */ + template + class NormalEstimationOMP: public NormalEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using NormalEstimation::feature_name_; + using NormalEstimation::getClassName; + using NormalEstimation::indices_; + using NormalEstimation::input_; + using NormalEstimation::k_; + using NormalEstimation::vpx_; + using NormalEstimation::vpy_; + using NormalEstimation::vpz_; + using NormalEstimation::search_parameter_; + using NormalEstimation::surface_; + using NormalEstimation::getViewPoint; + + using PointCloudOut = typename NormalEstimation::PointCloudOut; + + public: + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + NormalEstimationOMP (unsigned int nr_threads = 0) + { + feature_name_ = "NormalEstimationOMP"; + + setNumberOfThreads(nr_threads); + } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + protected: + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + + private: + /** \brief Estimate normals for all points given in using the surface in + * setSearchSurface () and the spatial locator in setSearchMethod () + * \param output the resultant point cloud model dataset that contains surface normals and curvatures + */ + void + computeFeature (PointCloudOut &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/normal_based_signature.h b/deps_install/include/pcl-1.10/pcl/features/normal_based_signature.h new file mode 100755 index 0000000..a3576b4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/normal_based_signature.h @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Normal-based feature signature estimation class. Obtains the feature vector by applying Discrete Cosine and + * Fourier Transforms on an NxM array of real numbers representing the projection distances of the points in the input + * cloud to a disc around the point of interest. + * Please consult the following publication for more details: + * Xinju Li and Igor Guskov + * Multi-scale features for approximate alignment of point-based surfaces + * Proceedings of the third Eurographics symposium on Geometry processing + * July 2005, Vienna, Austria + * + * \note These features were meant to be used at keypoints detected by a detector using different smoothing radii + * (e.g., SmoothedSurfacesKeypoint) + * \author Alexandru-Eugen Ichim + */ + template + class NormalBasedSignatureEstimation : public FeatureFromNormals + { + public: + using Feature::input_; + using Feature::tree_; + using Feature::search_radius_; + using PCLBase::indices_; + using FeatureFromNormals::normals_; + + using FeatureCloud = pcl::PointCloud; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + + /** \brief Empty constructor, initializes the internal parameters to the default values + */ + NormalBasedSignatureEstimation () + : FeatureFromNormals (), + scale_h_ (), + N_ (36), + M_ (8), + N_prime_ (4), + M_prime_ (3) + { + } + + /** \brief Setter method for the N parameter - the length of the columns used for the Discrete Fourier Transform. + * \param[in] n the length of the columns used for the Discrete Fourier Transform. + */ + inline void + setN (std::size_t n) { N_ = n; } + + /** \brief Returns the N parameter - the length of the columns used for the Discrete Fourier Transform. */ + inline std::size_t + getN () { return N_; } + + /** \brief Setter method for the M parameter - the length of the rows used for the Discrete Cosine Transform. + * \param[in] m the length of the rows used for the Discrete Cosine Transform. + */ + inline void + setM (std::size_t m) { M_ = m; } + + /** \brief Returns the M parameter - the length of the rows used for the Discrete Cosine Transform */ + inline std::size_t + getM () { return M_; } + + /** \brief Setter method for the N' parameter - the number of columns to be taken from the matrix of DFT and DCT + * values that will be contained in the output feature vector + * \note This value directly influences the dimensions of the type of output points (PointFeature) + * \param[in] n_prime the number of columns from the matrix of DFT and DCT that will be contained in the output + */ + inline void + setNPrime (std::size_t n_prime) { N_prime_ = n_prime; } + + /** \brief Returns the N' parameter - the number of rows to be taken from the matrix of DFT and DCT + * values that will be contained in the output feature vector + * \note This value directly influences the dimensions of the type of output points (PointFeature) + */ + inline std::size_t + getNPrime () { return N_prime_; } + + /** \brief Setter method for the M' parameter - the number of rows to be taken from the matrix of DFT and DCT + * values that will be contained in the output feature vector + * \note This value directly influences the dimensions of the type of output points (PointFeature) + * \param[in] m_prime the number of rows from the matrix of DFT and DCT that will be contained in the output + */ + inline void + setMPrime (std::size_t m_prime) { M_prime_ = m_prime; } + + /** \brief Returns the M' parameter - the number of rows to be taken from the matrix of DFT and DCT + * values that will be contained in the output feature vector + * \note This value directly influences the dimensions of the type of output points (PointFeature) + */ + inline std::size_t + getMPrime () { return M_prime_; } + + /** \brief Setter method for the scale parameter - used to determine the radius of the sampling disc around the + * point of interest - linked to the smoothing scale of the input cloud + */ + inline void + setScale (float scale) { scale_h_ = scale; } + + /** \brief Returns the scale parameter - used to determine the radius of the sampling disc around the + * point of interest - linked to the smoothing scale of the input cloud + */ + inline float + getScale () { return scale_h_; } + + + protected: + void + computeFeature (FeatureCloud &output) override; + + private: + float scale_h_; + std::size_t N_, M_, N_prime_, M_prime_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/organized_edge_detection.h b/deps_install/include/pcl-1.10/pcl/features/organized_edge_detection.h new file mode 100755 index 0000000..5551640 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/organized_edge_detection.h @@ -0,0 +1,429 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, + * and OrganizedEdgeFromRGBNormals find 3D edges from an organized point + * cloud data. Given an organized point cloud, they will output a PointCloud + * of edge labels and a vector of PointIndices. + * OrganizedEdgeBase accepts PCL_XYZ_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, and EDGELABEL_OCCLUDED. + * OrganizedEdgeFromRGB accepts PCL_RGB_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_RGB_CANNY. + * OrganizedEdgeFromNormals accepts PCL_XYZ_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_HIGH_CURVATURE. + * OrganizedEdgeFromRGBNormals accepts PCL_RGB_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, EDGELABEL_HIGH_CURVATURE, and EDGELABEL_RGB_CANNY. + * + * \author Changhyun Choi + */ + template + class OrganizedEdgeBase : public PCLBase + { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + /** \brief Constructor for OrganizedEdgeBase */ + OrganizedEdgeBase () + : th_depth_discon_ (0.02f) + , max_search_neighbors_ (50) + , detecting_edge_types_ (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED) + { + } + + /** \brief Destructor for OrganizedEdgeBase */ + + ~OrganizedEdgeBase () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities) + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + + /** \brief Set the tolerance in meters for difference in depth values between neighboring points. */ + inline void + setDepthDisconThreshold (const float th) + { + th_depth_discon_ = th; + } + + /** \brief Get the tolerance in meters for difference in depth values between neighboring points. */ + inline float + getDepthDisconThreshold () const + { + return (th_depth_discon_); + } + + /** \brief Set the max search distance for deciding occluding and occluded edges. */ + inline void + setMaxSearchNeighbors (const int max_dist) + { + max_search_neighbors_ = max_dist; + } + + /** \brief Get the max search distance for deciding occluding and occluded edges. */ + inline int + getMaxSearchNeighbors () const + { + return (max_search_neighbors_); + } + + /** \brief Set the detecting edge types. */ + inline void + setEdgeType (int edge_types) + { + detecting_edge_types_ = edge_types; + } + + /** \brief Get the detecting edge types. */ + inline int + getEdgeType () const + { + return detecting_edge_types_; + } + + enum {EDGELABEL_NAN_BOUNDARY=1, EDGELABEL_OCCLUDING=2, EDGELABEL_OCCLUDED=4, EDGELABEL_HIGH_CURVATURE=8, EDGELABEL_RGB_CANNY=16}; + static const int num_of_edgetype_ = 5; + + protected: + /** \brief Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + */ + void + extractEdges (pcl::PointCloud& labels) const; + + /** \brief Assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + assignLabelIndices (pcl::PointCloud& labels, std::vector& label_indices) const; + + struct Neighbor + { + Neighbor (int dx, int dy, int didx) + : d_x (dx) + , d_y (dy) + , d_index (didx) + {} + + int d_x; + int d_y; + int d_index; // = dy * width + dx: pre-calculated + }; + + /** \brief The tolerance in meters for difference in depth values between neighboring points + * (The value is set for 1 meter and is adapted with respect to depth value linearly. + * (e.g. 2.0*th_depth_discon_ in 2 meter depth)) + */ + float th_depth_discon_; + + /** \brief The max search distance for deciding occluding and occluded edges */ + int max_search_neighbors_; + + /** \brief The bit encoded value that represents edge types to detect */ + int detecting_edge_types_; + }; + + template + class OrganizedEdgeFromRGB : virtual public OrganizedEdgeBase + { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + public: + using OrganizedEdgeBase::input_; + using OrganizedEdgeBase::indices_; + using OrganizedEdgeBase::initCompute; + using OrganizedEdgeBase::deinitCompute; + using OrganizedEdgeBase::detecting_edge_types_; + using OrganizedEdgeBase::EDGELABEL_NAN_BOUNDARY; + using OrganizedEdgeBase::EDGELABEL_OCCLUDING; + using OrganizedEdgeBase::EDGELABEL_OCCLUDED; + using OrganizedEdgeBase::EDGELABEL_RGB_CANNY; + + /** \brief Constructor for OrganizedEdgeFromRGB */ + OrganizedEdgeFromRGB () + : OrganizedEdgeBase () + , th_rgb_canny_low_ (40.0) + , th_rgb_canny_high_ (100.0) + { + this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY); + } + + /** \brief Destructor for OrganizedEdgeFromRGB */ + + ~OrganizedEdgeFromRGB () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + + /** \brief Set the low threshold value for RGB Canny edge detection */ + inline void + setRGBCannyLowThreshold (const float th) + { + th_rgb_canny_low_ = th; + } + + /** \brief Get the low threshold value for RGB Canny edge detection */ + inline float + getRGBCannyLowThreshold () const + { + return (th_rgb_canny_low_); + } + + /** \brief Set the high threshold value for RGB Canny edge detection */ + inline void + setRGBCannyHighThreshold (const float th) + { + th_rgb_canny_high_ = th; + } + + /** \brief Get the high threshold value for RGB Canny edge detection */ + inline float + getRGBCannyHighThreshold () const + { + return (th_rgb_canny_high_); + } + + protected: + /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) + * \param[out] labels a PointCloud of edge labels + */ + void + extractEdges (pcl::PointCloud& labels) const; + + /** \brief The low threshold value for RGB Canny edge detection (default: 40.0) */ + float th_rgb_canny_low_; + + /** \brief The high threshold value for RGB Canny edge detection (default: 100.0) */ + float th_rgb_canny_high_; + }; + + template + class OrganizedEdgeFromNormals : virtual public OrganizedEdgeBase + { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + public: + using OrganizedEdgeBase::input_; + using OrganizedEdgeBase::indices_; + using OrganizedEdgeBase::initCompute; + using OrganizedEdgeBase::deinitCompute; + using OrganizedEdgeBase::detecting_edge_types_; + using OrganizedEdgeBase::EDGELABEL_NAN_BOUNDARY; + using OrganizedEdgeBase::EDGELABEL_OCCLUDING; + using OrganizedEdgeBase::EDGELABEL_OCCLUDED; + using OrganizedEdgeBase::EDGELABEL_HIGH_CURVATURE; + + /** \brief Constructor for OrganizedEdgeFromNormals */ + OrganizedEdgeFromNormals () + : OrganizedEdgeBase () + , normals_ () + , th_hc_canny_low_ (0.4f) + , th_hc_canny_high_ (1.1f) + { + this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_HIGH_CURVATURE); + } + + /** \brief Destructor for OrganizedEdgeFromNormals */ + + ~OrganizedEdgeFromNormals () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + + /** \brief Provide a pointer to the input normals. + * \param[in] normals the input normal cloud + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) + { + normals_ = normals; + } + + /** \brief Get the input normals. */ + inline PointCloudNConstPtr + getInputNormals () const + { + return (normals_); + } + + /** \brief Set the low threshold value for high curvature Canny edge detection */ + inline void + setHCCannyLowThreshold (const float th) + { + th_hc_canny_low_ = th; + } + + /** \brief Get the low threshold value for high curvature Canny edge detection */ + inline float + getHCCannyLowThreshold () const + { + return (th_hc_canny_low_); + } + + /** \brief Set the high threshold value for high curvature Canny edge detection */ + inline void + setHCCannyHighThreshold (const float th) + { + th_hc_canny_high_ = th; + } + + /** \brief Get the high threshold value for high curvature Canny edge detection */ + inline float + getHCCannyHighThreshold () const + { + return (th_hc_canny_high_); + } + + protected: + /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) + * \param[out] labels a PointCloud of edge labels + */ + void + extractEdges (pcl::PointCloud& labels) const; + + /** \brief A pointer to the input normals */ + PointCloudNConstPtr normals_; + + /** \brief The low threshold value for high curvature Canny edge detection (default: 0.4) */ + float th_hc_canny_low_; + + /** \brief The high threshold value for high curvature Canny edge detection (default: 1.1) */ + float th_hc_canny_high_; + }; + + template + class OrganizedEdgeFromRGBNormals : public OrganizedEdgeFromRGB, public OrganizedEdgeFromNormals + { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + public: + using OrganizedEdgeFromNormals::input_; + using OrganizedEdgeFromNormals::indices_; + using OrganizedEdgeFromNormals::initCompute; + using OrganizedEdgeFromNormals::deinitCompute; + using OrganizedEdgeFromNormals::detecting_edge_types_; + using OrganizedEdgeBase::EDGELABEL_NAN_BOUNDARY; + using OrganizedEdgeBase::EDGELABEL_OCCLUDING; + using OrganizedEdgeBase::EDGELABEL_OCCLUDED; + using OrganizedEdgeBase::EDGELABEL_HIGH_CURVATURE; + using OrganizedEdgeBase::EDGELABEL_RGB_CANNY; + + /** \brief Constructor for OrganizedEdgeFromRGBNormals */ + OrganizedEdgeFromRGBNormals () + : OrganizedEdgeFromRGB () + , OrganizedEdgeFromNormals () + { + this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY | EDGELABEL_HIGH_CURVATURE); + } + + /** \brief Destructor for OrganizedEdgeFromRGBNormals */ + + ~OrganizedEdgeFromRGBNormals () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities, RGB Canny edge, and high curvature regions) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/our_cvfh.h b/deps_install/include/pcl-1.10/pcl/features/our_cvfh.h new file mode 100755 index 0000000..66ca5b5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/our_cvfh.h @@ -0,0 +1,408 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: cvfh.h 4936 2012-03-07 11:12:45Z aaldoma $ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given + * point cloud dataset given XYZ data and normals, as presented in: + * - OUR-CVFH – Oriented, Unique and Repeatable Clustered Viewpoint Feature Histogram for Object Recognition and 6DOF Pose Estimation + * A. Aldoma, F. Tombari, R.B. Rusu and M. Vincze + * DAGM-OAGM 2012 + * Graz, Austria + * The suggested PointOutT is pcl::VFHSignature308. + * + * \author Aitor Aldoma + * \ingroup features + */ + template + class OURCVFHEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_radius_; + using Feature::surface_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + using KdTreePtr = typename pcl::search::Search::Ptr; + using PointInTPtr = typename pcl::PointCloud::Ptr; + /** \brief Empty constructor. */ + OURCVFHEstimation () : + vpx_ (0), vpy_ (0), vpz_ (0), leaf_size_ (0.005f), normalize_bins_ (false), curv_threshold_ (0.03f), cluster_tolerance_ (leaf_size_ * 3), + eps_angle_threshold_ (0.125f), min_points_ (50), radius_normals_ (leaf_size_ * 3) + { + search_radius_ = 0; + k_ = 1; + feature_name_ = "OURCVFHEstimation"; + refine_clusters_ = 1.f; + min_axis_value_ = 0.925f; + axis_ratio_ = 0.8f; + } + ; + + /** \brief Creates an affine transformation from the RF axes + * \param[in] evx the x-axis + * \param[in] evy the y-axis + * \param[in] evz the z-axis + * \param[out] transformPC the resulting transformation + * \param[in] center_mat 4x4 matrix concatenated to the resulting transformation + */ + inline Eigen::Matrix4f + createTransFromAxes (Eigen::Vector3f & evx, Eigen::Vector3f & evy, Eigen::Vector3f & evz, Eigen::Affine3f & transformPC, + Eigen::Matrix4f & center_mat) + { + Eigen::Matrix4f trans; + trans.setIdentity (4, 4); + trans (0, 0) = evx (0, 0); + trans (1, 0) = evx (1, 0); + trans (2, 0) = evx (2, 0); + trans (0, 1) = evy (0, 0); + trans (1, 1) = evy (1, 0); + trans (2, 1) = evy (2, 0); + trans (0, 2) = evz (0, 0); + trans (1, 2) = evz (1, 0); + trans (2, 2) = evz (2, 0); + + Eigen::Matrix4f homMatrix = Eigen::Matrix4f (); + homMatrix.setIdentity (4, 4); + homMatrix = transformPC.matrix (); + + Eigen::Matrix4f trans_copy = trans.inverse (); + trans = trans_copy * center_mat * homMatrix; + return trans; + } + + /** \brief Computes SGURF and the shape distribution based on the selected SGURF + * \param[in] processed the input cloud + * \param[out] output the resulting signature + * \param[in] cluster_indices the indices of the stable cluster + */ + void + computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut &output, std::vector & cluster_indices); + + /** \brief Computes SGURF + * \param[in] centroid the centroid of the cluster + * \param[in] normal_centroid the average of the normals + * \param[in] processed the input cloud + * \param[out] transformations the transformations aligning the cloud to the SGURF axes + * \param[out] grid the cloud transformed internally + * \param[in] indices the indices of the stable cluster + */ + bool + sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid, PointInTPtr & processed, std::vector > & transformations, + PointInTPtr & grid, pcl::PointIndices & indices); + + /** \brief Removes normals with high curvature caused by real edges or noisy data + * \param[in] cloud pointcloud to be filtered + * \param[in] indices_to_use + * \param[out] indices_out the indices of the points with higher curvature than threshold + * \param[out] indices_in the indices of the remaining points after filtering + * \param[in] threshold threshold value for curvature + */ + void + filterNormalsWithHighCurvature (const pcl::PointCloud & cloud, std::vector & indices_to_use, std::vector &indices_out, + std::vector &indices_in, float threshold); + + /** \brief Set the viewpoint. + * \param[in] vpx the X coordinate of the viewpoint + * \param[in] vpy the Y coordinate of the viewpoint + * \param[in] vpz the Z coordinate of the viewpoint + */ + inline void + setViewPoint (float vpx, float vpy, float vpz) + { + vpx_ = vpx; + vpy_ = vpy; + vpz_ = vpz; + } + + /** \brief Set the radius used to compute normals + * \param[in] radius_normals the radius + */ + inline void + setRadiusNormals (float radius_normals) + { + radius_normals_ = radius_normals; + } + + /** \brief Get the viewpoint. + * \param[out] vpx the X coordinate of the viewpoint + * \param[out] vpy the Y coordinate of the viewpoint + * \param[out] vpz the Z coordinate of the viewpoint + */ + inline void + getViewPoint (float &vpx, float &vpy, float &vpz) + { + vpx = vpx_; + vpy = vpy_; + vpz = vpz_; + } + + /** \brief Get the centroids used to compute different CVFH descriptors + * \param[out] centroids vector to hold the centroids + */ + inline void + getCentroidClusters (std::vector > & centroids) + { + for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); ++i) + centroids.push_back (centroids_dominant_orientations_[i]); + } + + /** \brief Get the normal centroids used to compute different CVFH descriptors + * \param[out] centroids vector to hold the normal centroids + */ + inline void + getCentroidNormalClusters (std::vector > & centroids) + { + for (std::size_t i = 0; i < dominant_normals_.size (); ++i) + centroids.push_back (dominant_normals_[i]); + } + + /** \brief Sets max. Euclidean distance between points to be added to the cluster + * \param[in] d the maximum Euclidean distance + */ + + inline void + setClusterTolerance (float d) + { + cluster_tolerance_ = d; + } + + /** \brief Sets max. deviation of the normals between two points so they can be clustered together + * \param[in] d the maximum deviation + */ + inline void + setEPSAngleThreshold (float d) + { + eps_angle_threshold_ = d; + } + + /** \brief Sets curvature threshold for removing normals + * \param[in] d the curvature threshold + */ + inline void + setCurvatureThreshold (float d) + { + curv_threshold_ = d; + } + + /** \brief Set minimum amount of points for a cluster to be considered + * \param[in] min the minimum amount of points to be set + */ + inline void + setMinPoints (std::size_t min) + { + min_points_ = min; + } + + /** \brief Sets whether the signatures should be normalized or not + * \param[in] normalize true if normalization is required, false otherwise + */ + inline void + setNormalizeBins (bool normalize) + { + normalize_bins_ = normalize; + } + + /** \brief Gets the indices of the original point cloud used to compute the signatures + * \param[out] indices vector of point indices + */ + inline void + getClusterIndices (std::vector & indices) + { + indices = clusters_; + } + + /** \brief Gets the number of non-disambiguable axes that correspond to each centroid + * \param[out] cluster_axes vector mapping each centroid to the number of signatures + */ + inline void + getClusterAxes (std::vector & cluster_axes) + { + cluster_axes = cluster_axes_; + } + + /** \brief Sets the refinement factor for the clusters + * \param[in] rc the factor used to decide if a point is used to estimate a stable cluster + */ + void + setRefineClusters (float rc) + { + refine_clusters_ = rc; + } + + /** \brief Returns the transformations aligning the point cloud to the corresponding SGURF + * \param[out] trans vector of transformations + */ + void + getTransforms (std::vector > & trans) + { + trans = transforms_; + } + + /** \brief Returns a boolean vector indicating of the transformation obtained by getTransforms() represents + * a valid SGURF + * \param[out] valid vector of booleans + */ + void + getValidTransformsVec (std::vector & valid) + { + valid = valid_transforms_; + } + + /** \brief Sets the min axis ratio between the SGURF axes to decide if disambiguition is feasible + * \param[in] f the ratio between axes + */ + void + setAxisRatio (float f) + { + axis_ratio_ = f; + } + + /** \brief Sets the min disambiguition axis value to generate several SGURFs for the cluster when disambiguition is difficult + * \param[in] f the min axis value + */ + void + setMinAxisValue (float f) + { + min_axis_value_ = f; + } + + /** \brief Overloaded computed method from pcl::Feature. + * \param[out] output the resultant point cloud model dataset containing the estimated features + */ + void + compute (PointCloudOut &output); + + private: + /** \brief Values describing the viewpoint ("pinhole" camera model assumed). + * By default, the viewpoint is set to 0,0,0. + */ + float vpx_, vpy_, vpz_; + + /** \brief Size of the voxels after voxel gridding. IMPORTANT: Must match the voxel + * size of the training data or the normalize_bins_ flag must be set to true. + */ + float leaf_size_; + + /** \brief Whether to normalize the signatures or not. Default: false. */ + bool normalize_bins_; + + /** \brief Curvature threshold for removing normals. */ + float curv_threshold_; + + /** \brief allowed Euclidean distance between points to be added to the cluster. */ + float cluster_tolerance_; + + /** \brief deviation of the normals between two points so they can be clustered together. */ + float eps_angle_threshold_; + + /** \brief Minimum amount of points in a clustered region to be considered stable for CVFH + * computation. + */ + std::size_t min_points_; + + /** \brief Radius for the normals computation. */ + float radius_normals_; + + /** \brief Factor for the cluster refinement */ + float refine_clusters_; + + std::vector > transforms_; + std::vector valid_transforms_; + + float axis_ratio_; + float min_axis_value_; + + /** \brief Estimate the OUR-CVFH descriptors at + * a set of points given by using the surface in + * setSearchSurface () + * + * \param[out] output the resultant point cloud model dataset that contains the OUR-CVFH + * feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief Region growing method using Euclidean distances and neighbors normals to + * add points to a region. + * \param[in] cloud point cloud to split into regions + * \param[in] normals are the normals of cloud + * \param[in] tolerance is the allowed Euclidean distance between points to be added to + * the cluster + * \param[in] tree is the spatial search structure for nearest neighbour search + * \param[out] clusters vector of indices representing the clustered regions + * \param[in] eps_angle deviation of the normals between two points so they can be + * clustered together + * \param[in] min_pts_per_cluster minimum cluster size. (default: 1 point) + * \param[in] max_pts_per_cluster maximum cluster size. (default: all the points) + */ + void + extractEuclideanClustersSmooth (const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + float tolerance, const pcl::search::Search::Ptr &tree, + std::vector &clusters, double eps_angle, unsigned int min_pts_per_cluster = 1, + unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()); + + protected: + /** \brief Centroids that were used to compute different OUR-CVFH descriptors */ + std::vector > centroids_dominant_orientations_; + /** \brief Normal centroids that were used to compute different OUR-CVFH descriptors */ + std::vector > dominant_normals_; + /** \brief Indices to the points representing the stable clusters */ + std::vector clusters_; + /** \brief Mapping from clusters to OUR-CVFH descriptors */ + std::vector cluster_axes_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/pfh.h b/deps_install/include/pcl-1.10/pcl/features/pfh.h new file mode 100755 index 0000000..13ec004 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/pfh.h @@ -0,0 +1,222 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief PFHEstimation estimates the Point Feature Histogram (PFH) descriptor for a given point cloud dataset + * containing points and normals. + * + * A commonly used type for PointOutT is pcl::PFHSignature125. + * + * \note If you use this code in any academic work, please cite: + * + * - R.B. Rusu, N. Blodow, Z.C. Marton, M. Beetz. + * Aligning Point Cloud Views using Persistent Feature Histograms. + * In Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), + * Nice, France, September 22-26 2008. + * - R.B. Rusu, Z.C. Marton, N. Blodow, M. Beetz. + * Learning Informative Point Classes for the Acquisition of Object Model Maps. + * In Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV), + * Hanoi, Vietnam, December 17-20 2008. + * + * \attention + * The convention for PFH features is: + * - if a query point's nearest neighbors cannot be estimated, the PFH feature will be set to NaN + * (not a number) + * - it is impossible to estimate a PFH descriptor for a point that + * doesn't have finite 3D coordinates. Therefore, any point that contains + * NaN data on x, y, or z, will have its PFH feature property set to NaN. + * + * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \ref FPFHEstimationOMP for examples on parallel implementations of the FPFH (Fast Point Feature Histogram). + * + * \author Radu B. Rusu + * \ingroup features + */ + template + class PFHEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::surface_; + using Feature::input_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + + /** \brief Empty constructor. + * Sets \a use_cache_ to false, \a nr_subdiv_ to 5, and the internal maximum cache size to 1GB. + */ + PFHEstimation () : + nr_subdiv_ (5), + d_pi_ (1.0f / (2.0f * static_cast (M_PI))), + key_list_ (), + // Default 1GB memory size. Need to set it to something more conservative. + max_cache_size_ ((1ul*1024ul*1024ul*1024ul) / sizeof (std::pair, Eigen::Vector4f>)), + use_cache_ (false) + { + feature_name_ = "PFHEstimation"; + }; + + /** \brief Set the maximum internal cache size. Defaults to 2GB worth of entries. + * \param[in] cache_size maximum cache size + */ + inline void + setMaximumCacheSize (unsigned int cache_size) + { + max_cache_size_ = cache_size; + } + + /** \brief Get the maximum internal cache size. */ + inline unsigned int + getMaximumCacheSize () + { + return (max_cache_size_); + } + + /** \brief Set whether to use an internal cache mechanism for removing redundant calculations or not. + * + * \note Depending on how the point cloud is ordered and how the nearest + * neighbors are estimated, using a cache could have a positive or a + * negative influence. Please test with and without a cache on your + * data, and choose whatever works best! + * + * See \ref setMaximumCacheSize for setting the maximum cache size + * + * \param[in] use_cache set to true to use the internal cache, false otherwise + */ + inline void + setUseInternalCache (bool use_cache) + { + use_cache_ = use_cache; + } + + /** \brief Get whether the internal cache is used or not for computing the PFH features. */ + inline bool + getUseInternalCache () + { + return (use_cache_); + } + + /** \brief Compute the 4-tuple representation containing the three angles and one distance between two points + * represented by Cartesian coordinates and normals. + * \note For explanations about the features, please see the literature mentioned above (the order of the + * features might be different). + * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points + * \param[in] normals the dataset containing the surface normals (assuming normalized vectors) at each point in cloud + * \param[in] p_idx the index of the first point (source) + * \param[in] q_idx the index of the second point (target) + * \param[out] f1 the first angular feature (angle between the projection of nq_idx and u) + * \param[out] f2 the second angular feature (angle between nq_idx and v) + * \param[out] f3 the third angular feature (angle between np_idx and |p_idx - q_idx|) + * \param[out] f4 the distance feature (p_idx - q_idx) + * \note For efficiency reasons, we assume that the point data passed to the method is finite. + */ + bool + computePairFeatures (const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4); + + /** \brief Estimate the PFH (Point Feature Histograms) individual signatures of the three angular (f1, f2, f3) + * features for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points + * \param[in] normals the dataset containing the surface normals at each point in \a cloud + * \param[in] indices the k-neighborhood point indices in the dataset + * \param[in] nr_split the number of subdivisions for each angular feature interval + * \param[out] pfh_histogram the resultant (combinatorial) PFH histogram representing the feature at the query point + */ + void + computePointPFHSignature (const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + const std::vector &indices, int nr_split, Eigen::VectorXf &pfh_histogram); + + protected: + /** \brief Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains the PFH feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief The number of subdivisions for each angular feature interval. */ + int nr_subdiv_; + + /** \brief Placeholder for a point's PFH signature. */ + Eigen::VectorXf pfh_histogram_; + + /** \brief Placeholder for a PFH 4-tuple. */ + Eigen::Vector4f pfh_tuple_; + + /** \brief Placeholder for a histogram index. */ + int f_index_[3]; + + /** \brief Float constant = 1.0 / (2.0 * M_PI) */ + float d_pi_; + + /** \brief Internal hashmap, used to optimize efficiency of redundant computations. */ + std::map, Eigen::Vector4f, std::less<>, Eigen::aligned_allocator, Eigen::Vector4f> > > feature_map_; + + /** \brief Queue of pairs saved, used to constrain memory usage. */ + std::queue > key_list_; + + /** \brief Maximum size of internal cache memory. */ + unsigned int max_cache_size_; + + /** \brief Set to true to use the internal cache for removing redundant computations. */ + bool use_cache_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/pfh_tools.h b/deps_install/include/pcl-1.10/pcl/features/pfh_tools.h new file mode 100755 index 0000000..d647f34 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/pfh_tools.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +namespace pcl +{ + /** \brief Compute the 4-tuple representation containing the three angles and one distance between two points + * represented by Cartesian coordinates and normals. + * \note For explanations about the features, please see the literature mentioned above (the order of the + * features might be different). + * \param[in] p1 the first XYZ point + * \param[in] n1 the first surface normal + * \param[in] p2 the second XYZ point + * \param[in] n2 the second surface normal + * \param[out] f1 the first angular feature (angle between the projection of nq_idx and u) + * \param[out] f2 the second angular feature (angle between nq_idx and v) + * \param[out] f3 the third angular feature (angle between np_idx and |p_idx - q_idx|) + * \param[out] f4 the distance feature (p_idx - q_idx) + * + * \note For efficiency reasons, we assume that the point data passed to the method is finite. + * \ingroup features + */ + PCL_EXPORTS bool + computePairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, + const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, + float &f1, float &f2, float &f3, float &f4); + + PCL_EXPORTS bool + computeRGBPairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &colors1, + const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &colors2, + float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7); + +} diff --git a/deps_install/include/pcl-1.10/pcl/features/pfhrgb.h b/deps_install/include/pcl-1.10/pcl/features/pfhrgb.h new file mode 100755 index 0000000..fd4bf29 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/pfhrgb.h @@ -0,0 +1,101 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + template + class PFHRGBEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PCLBase::indices_; + using Feature::feature_name_; + using Feature::surface_; + using Feature::k_; + using Feature::search_parameter_; + using FeatureFromNormals::normals_; + using PointCloudOut = typename Feature::PointCloudOut; + + + PFHRGBEstimation () + : nr_subdiv_ (5), d_pi_ (1.0f / (2.0f * static_cast (M_PI))) + { + feature_name_ = "PFHRGBEstimation"; + } + + bool + computeRGBPairFeatures (const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + int p_idx, int q_idx, + float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7); + + void + computePointPFHRGBSignature (const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + const std::vector &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram); + + protected: + void + computeFeature (PointCloudOut &output) override; + + private: + /** \brief The number of subdivisions for each angular feature interval. */ + int nr_subdiv_; + + /** \brief Placeholder for a point's PFHRGB signature. */ + Eigen::VectorXf pfhrgb_histogram_; + + /** \brief Placeholder for a PFHRGB 7-tuple. */ + Eigen::VectorXf pfhrgb_tuple_; + + /** \brief Placeholder for a histogram index. */ + int f_index_[7]; + + /** \brief Float constant = 1.0 / (2.0 * M_PI) */ + float d_pi_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/ppf.h b/deps_install/include/pcl-1.10/pcl/features/ppf.h new file mode 100755 index 0000000..87e0bef --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/ppf.h @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief + * \param[in] p1 + * \param[in] n1 + * \param[in] p2 + * \param[in] n2 + * \param[out] f1 + * \param[out] f2 + * \param[out] f3 + * \param[out] f4 + */ + PCL_EXPORTS bool + computePPFPairFeature (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, + const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, + float &f1, float &f2, float &f3, float &f4); + + + /** \brief Class that calculates the "surflet" features for each pair in the given + * pointcloud. Please refer to the following publication for more details: + * B. Drost, M. Ulrich, N. Navab, S. Ilic + * Model Globally, Match Locally: Efficient and Robust 3D Object Recognition + * 2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR) + * 13-18 June 2010, San Francisco, CA + * + * PointOutT is meant to be pcl::PPFSignature - contains the 4 values of the Surflet + * feature and in addition, alpha_m for the respective pair - optimization proposed by + * the authors (see above) + * + * \author Alexandru-Eugen Ichim + */ + template + class PPFEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PCLBase::indices_; + using Feature::input_; + using Feature::feature_name_; + using Feature::getClassName; + using FeatureFromNormals::normals_; + + using PointCloudOut = pcl::PointCloud; + + /** \brief Empty Constructor. */ + PPFEstimation (); + + + private: + /** \brief The method called for actually doing the computations + * \param[out] output the resulting point cloud (which should be of type pcl::PPFSignature); + * its size is the size of the input cloud, squared (i.e., one point for each pair in + * the input cloud); + */ + void + computeFeature (PointCloudOut &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/ppfrgb.h b/deps_install/include/pcl-1.10/pcl/features/ppfrgb.h new file mode 100755 index 0000000..def06b3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/ppfrgb.h @@ -0,0 +1,97 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + template + class PPFRGBEstimation : public FeatureFromNormals + { + public: + using PCLBase::indices_; + using Feature::input_; + using Feature::feature_name_; + using Feature::getClassName; + using FeatureFromNormals::normals_; + + using PointCloudOut = pcl::PointCloud; + + /** + * \brief Empty Constructor + */ + PPFRGBEstimation (); + + + private: + /** \brief The method called for actually doing the computations + * \param output the resulting point cloud (which should be of type pcl::PPFRGBSignature); + */ + void + computeFeature (PointCloudOut &output); + }; + + template + class PPFRGBRegionEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PCLBase::indices_; + using Feature::input_; + using Feature::feature_name_; + using Feature::search_radius_; + using Feature::tree_; + using Feature::getClassName; + using FeatureFromNormals::normals_; + + using PointCloudOut = pcl::PointCloud; + + PPFRGBRegionEstimation (); + + private: + void + computeFeature (PointCloudOut &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/principal_curvatures.h b/deps_install/include/pcl-1.10/pcl/features/principal_curvatures.h new file mode 100755 index 0000000..ff70d15 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/principal_curvatures.h @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief PrincipalCurvaturesEstimation estimates the directions (eigenvectors) and magnitudes (eigenvalues) of + * principal surface curvatures for a given point cloud dataset containing points and normals. + * + * The recommended PointOutT is pcl::PrincipalCurvatures. + * + * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \ref NormalEstimationOMP for an example on how to extend this to parallel implementations. + * + * \author Radu B. Rusu, Jared Glover + * \ingroup features + */ + template + class PrincipalCurvaturesEstimation : public FeatureFromNormals + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::surface_; + using Feature::input_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = pcl::PointCloud; + + /** \brief Empty constructor. */ + PrincipalCurvaturesEstimation () : + xyz_centroid_ (Eigen::Vector3f::Zero ()), + demean_ (Eigen::Vector3f::Zero ()), + covariance_matrix_ (Eigen::Matrix3f::Zero ()), + eigenvector_ (Eigen::Vector3f::Zero ()), + eigenvalues_ (Eigen::Vector3f::Zero ()) + { + feature_name_ = "PrincipalCurvaturesEstimation"; + }; + + /** \brief Perform Principal Components Analysis (PCA) on the point normals of a surface patch in the tangent + * plane of the given point normal, and return the principal curvature (eigenvector of the max eigenvalue), + * along with both the max (pc1) and min (pc2) eigenvalues + * \param[in] normals the point cloud normals + * \param[in] p_idx the query point at which the least-squares plane was estimated + * \param[in] indices the point cloud indices that need to be used + * \param[out] pcx the principal curvature X direction + * \param[out] pcy the principal curvature Y direction + * \param[out] pcz the principal curvature Z direction + * \param[out] pc1 the max eigenvalue of curvature + * \param[out] pc2 the min eigenvalue of curvature + */ + void + computePointPrincipalCurvatures (const pcl::PointCloud &normals, + int p_idx, const std::vector &indices, + float &pcx, float &pcy, float &pcz, float &pc1, float &pc2); + + protected: + + /** \brief Estimate the principal curvature (eigenvector of the max eigenvalue), along with both the max (pc1) + * and min (pc2) eigenvalues for all points given in using the surface in + * setSearchSurface () and the spatial locator in setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains the principal curvature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + private: + /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */ + std::vector > projected_normals_; + + /** \brief SSE aligned placeholder for the XYZ centroid of a surface patch. */ + Eigen::Vector3f xyz_centroid_; + + /** \brief Temporary point placeholder. */ + Eigen::Vector3f demean_; + + /** \brief Placeholder for the 3x3 covariance matrix at each surface patch. */ + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_; + + /** \brief SSE aligned eigenvectors placeholder for a covariance matrix. */ + Eigen::Vector3f eigenvector_; + /** \brief eigenvalues placeholder for a covariance matrix. */ + Eigen::Vector3f eigenvalues_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/range_image_border_extractor.h b/deps_install/include/pcl-1.10/pcl/features/range_image_border_extractor.h new file mode 100755 index 0000000..a87dcb3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/range_image_border_extractor.h @@ -0,0 +1,358 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +namespace pcl +{ + // FORWARD DECLARATIONS: + class RangeImage; + template + class PointCloud; + + /** \brief @b Extract obstacle borders from range images, meaning positions where there is a transition from foreground + * to background. + * \author Bastian Steder + * \ingroup features + */ + class PCL_EXPORTS RangeImageBorderExtractor : public Feature + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + // =====TYPEDEFS===== + using BaseClass = Feature; + + // =====PUBLIC STRUCTS===== + //! Stores some information extracted from the neighborhood of a point + struct LocalSurface + { + LocalSurface () : + max_neighbor_distance_squared () {} + + Eigen::Vector3f normal; + Eigen::Vector3f neighborhood_mean; + Eigen::Vector3f eigen_values; + Eigen::Vector3f normal_no_jumps; + Eigen::Vector3f neighborhood_mean_no_jumps; + Eigen::Vector3f eigen_values_no_jumps; + float max_neighbor_distance_squared; + }; + + //! Stores the indices of the shadow border corresponding to obstacle borders + struct ShadowBorderIndices + { + ShadowBorderIndices () : left (-1), right (-1), top (-1), bottom (-1) {} + int left, right, top, bottom; + }; + + //! Parameters used in this class + struct Parameters + { + Parameters () : max_no_of_threads(1), pixel_radius_borders (3), pixel_radius_plane_extraction (2), pixel_radius_border_direction (2), + minimum_border_probability (0.8f), pixel_radius_principal_curvature (2) {} + int max_no_of_threads; + int pixel_radius_borders; + int pixel_radius_plane_extraction; + int pixel_radius_border_direction; + float minimum_border_probability; + int pixel_radius_principal_curvature; + }; + + // =====STATIC METHODS===== + /** \brief Take the information from BorderTraits to calculate the local direction of the border + * \param border_traits contains the information needed to calculate the border angle + */ + static inline float + getObstacleBorderAngle (const BorderTraits& border_traits); + + // =====CONSTRUCTOR & DESTRUCTOR===== + /** Constructor */ + RangeImageBorderExtractor (const RangeImage* range_image=nullptr); + /** Destructor */ + ~RangeImageBorderExtractor (); + + // =====METHODS===== + /** \brief Provide a pointer to the range image + * \param range_image a pointer to the range_image + */ + void + setRangeImage (const RangeImage* range_image); + + /** \brief Erase all data calculated for the current range image */ + void + clearData (); + + /** \brief Get the 2D directions in the range image from the border directions - probably mainly useful for + * visualization + */ + float* + getAnglesImageForBorderDirections (); + + /** \brief Get the 2D directions in the range image from the surface change directions - probably mainly useful for + * visualization + */ + float* + getAnglesImageForSurfaceChangeDirections (); + + /** Overwrite the compute function of the base class */ + void + compute (PointCloudOut& output); + + // =====GETTER===== + Parameters& + getParameters () { return (parameters_); } + + bool + hasRangeImage () const { return range_image_ != nullptr; } + + const RangeImage& + getRangeImage () const { return *range_image_; } + + float* + getBorderScoresLeft () { extractBorderScoreImages (); return border_scores_left_.data (); } + + float* + getBorderScoresRight () { extractBorderScoreImages (); return border_scores_right_.data (); } + + float* + getBorderScoresTop () { extractBorderScoreImages (); return border_scores_top_.data (); } + + float* + getBorderScoresBottom () { extractBorderScoreImages (); return border_scores_bottom_.data (); } + + LocalSurface** + getSurfaceStructure () { extractLocalSurfaceStructure (); return surface_structure_; } + + PointCloudOut& + getBorderDescriptions () { classifyBorders (); return *border_descriptions_; } + + ShadowBorderIndices** + getShadowBorderInformations () { findAndEvaluateShadowBorders (); return shadow_border_informations_; } + + Eigen::Vector3f** + getBorderDirections () { calculateBorderDirections (); return border_directions_; } + + float* + getSurfaceChangeScores () { calculateSurfaceChanges (); return surface_change_scores_; } + + Eigen::Vector3f* + getSurfaceChangeDirections () { calculateSurfaceChanges (); return surface_change_directions_; } + + + protected: + // =====PROTECTED MEMBER VARIABLES===== + Parameters parameters_; + const RangeImage* range_image_; + int range_image_size_during_extraction_; + std::vector border_scores_left_, border_scores_right_; + std::vector border_scores_top_, border_scores_bottom_; + LocalSurface** surface_structure_; + PointCloudOut* border_descriptions_; + ShadowBorderIndices** shadow_border_informations_; + Eigen::Vector3f** border_directions_; + + float* surface_change_scores_; + Eigen::Vector3f* surface_change_directions_; + + + // =====PROTECTED METHODS===== + /** \brief Calculate a border score based on how distant the neighbor is, compared to the closest neighbors + * /param local_surface + * /param x + * /param y + * /param offset_x + * /param offset_y + * /param pixel_radius (defaults to 1) + * /return the resulting border score + */ + inline float + getNeighborDistanceChangeScore (const LocalSurface& local_surface, int x, int y, + int offset_x, int offset_y, int pixel_radius=1) const; + + /** \brief Calculate a border score based on how much the neighbor is away from the local surface plane + * \param local_surface + * \param x + * \param y + * \param offset_x + * \param offset_y + * \return the resulting border score + */ + inline float + getNormalBasedBorderScore (const LocalSurface& local_surface, int x, int y, + int offset_x, int offset_y) const; + + /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value + * \param x + * \param y + * \param offset_x + * \param offset_y + * \param border_scores + * \param border_scores_other_direction + * \param shadow_border_idx + * \return + */ + inline bool + changeScoreAccordingToShadowBorderValue (int x, int y, int offset_x, int offset_y, float* border_scores, + float* border_scores_other_direction, int& shadow_border_idx) const; + + /** \brief Returns a new score for the given pixel that is >= the original value, based on the neighbors values + * \param x the x-coordinate of the input pixel + * \param y the y-coordinate of the input pixel + * \param border_scores the input border scores + * \return the resulting updated border score + */ + inline float + updatedScoreAccordingToNeighborValues (int x, int y, const float* border_scores) const; + + /** \brief For all pixels, returns a new score that is >= the original value, based on the neighbors values + * \param border_scores the input border scores + * \return a pointer to the resulting array of updated scores + */ + float* + updatedScoresAccordingToNeighborValues (const float* border_scores) const; + + /** \brief Replace all border score values with updates according to \a updatedScoreAccordingToNeighborValues */ + void + updateScoresAccordingToNeighborValues (); + + /** \brief Check if a potential border point has a corresponding shadow border + * \param x the x-coordinate of the input point + * \param y the y-coordinate of the input point + * \param offset_x + * \param offset_y + * \param border_scores_left + * \param border_scores_right + * \param shadow_border_idx + * \return a boolean value indicating whether or not the point has a corresponding shadow border + */ + inline bool + checkPotentialBorder (int x, int y, int offset_x, int offset_y, float* border_scores_left, + float* border_scores_right, int& shadow_border_idx) const; + + /** \brief Check if a potential border point is a maximum regarding the border score + * \param x the x-coordinate of the input point + * \param y the y-coordinate of the input point + * \param offset_x + * \param offset_y + * \param border_scores + * \param shadow_border_idx + * \result a boolean value indicating whether or not the point is a maximum + */ + inline bool + checkIfMaximum (int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const; + + /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value */ + void + findAndEvaluateShadowBorders (); + + /** \brief Extract local plane information in every point (see getSurfaceStructure ()) */ + void + extractLocalSurfaceStructure (); + + /** \brief Get images representing the probability that the corresponding pixels are borders in that direction + * (see getBorderScores... ()) + */ + void + extractBorderScoreImages (); + + /** \brief Classify the pixels in the range image according to the different classes defined below in + * enum BorderClass. minImpactAngle (in radians) defines how flat the angle at which a surface was seen can be. + */ + void + classifyBorders (); + + /** \brief Calculate the 3D direction of the border just using the border traits at this position (facing away from + * the obstacle) + * \param x the x-coordinate of the input position + * \param y the y-coordinate of the input position + */ + inline void + calculateBorderDirection (int x, int y); + + /** \brief Call \a calculateBorderDirection for every point and average the result over + * parameters_.pixel_radius_border_direction + */ + void + calculateBorderDirections (); + + /** \brief Calculate a 3d direction from a border point by projecting the direction in the range image - returns + * false if direction could not be calculated + * \param border_description + * \param direction + * \param local_surface + * \return a boolean value indicating whether or not a direction could be calculated + */ + inline bool + get3dDirection (const BorderDescription& border_description, Eigen::Vector3f& direction, + const LocalSurface* local_surface=nullptr); + + /** \brief Calculate the main principal curvature (the largest eigenvalue and corresponding eigenvector for the + * normals in the area) in the given point + * \param x the x-coordinate of the input point + * \param y the y-coordinate of the input point + * \param radius the pixel radius that is used to find neighboring points + * \param magnitude the resulting magnitude + * \param main_direction the resulting direction + */ + inline bool + calculateMainPrincipalCurvature (int x, int y, int radius, float& magnitude, + Eigen::Vector3f& main_direction) const; + + /** \brief Uses either the border or principal curvature to define a score how much the surface changes in a point + (1 for a border) and what the main direction of that change is */ + void + calculateSurfaceChanges (); + + /** \brief Apply a blur to the surface change images */ + void + blurSurfaceChanges (); + + /** \brief Implementation of abstract derived function */ + void + computeFeature (PointCloudOut &output) override; + + private: + std::vector + updatedScoresAccordingToNeighborValues (const std::vector& border_scores) const; + }; +} // namespace end + +#include // Definitions of templated and inline functions diff --git a/deps_install/include/pcl-1.10/pcl/features/rift.h b/deps_install/include/pcl-1.10/pcl/features/rift.h new file mode 100755 index 0000000..36e94b5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/rift.h @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief RIFTEstimation estimates the Rotation Invariant Feature Transform descriptors for a given point cloud + * dataset containing points and intensity. For more information about the RIFT descriptor, see: + * + * Svetlana Lazebnik, Cordelia Schmid, and Jean Ponce. + * A sparse texture representation using local affine regions. + * In IEEE Transactions on Pattern Analysis and Machine Intelligence, volume 27, pages 1265-1278, August 2005. + * + * \author Michael Dixon + * \ingroup features + */ + + template + class RIFTEstimation: public Feature + { + public: + using Feature::feature_name_; + using Feature::getClassName; + + using Feature::surface_; + using Feature::indices_; + + using Feature::tree_; + using Feature::search_radius_; + + using PointCloudIn = pcl::PointCloud; + using PointCloudOut = typename Feature::PointCloudOut; + + using PointCloudGradient = pcl::PointCloud; + using PointCloudGradientPtr = typename PointCloudGradient::Ptr; + using PointCloudGradientConstPtr = typename PointCloudGradient::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Empty constructor. */ + RIFTEstimation () : gradient_ (), nr_distance_bins_ (4), nr_gradient_bins_ (8) + { + feature_name_ = "RIFTEstimation"; + }; + + /** \brief Provide a pointer to the input gradient data + * \param[in] gradient a pointer to the input gradient data + */ + inline void + setInputGradient (const PointCloudGradientConstPtr &gradient) { gradient_ = gradient; }; + + /** \brief Returns a shared pointer to the input gradient data */ + inline PointCloudGradientConstPtr + getInputGradient () const { return (gradient_); }; + + /** \brief Set the number of bins to use in the distance dimension of the RIFT descriptor + * \param[in] nr_distance_bins the number of bins to use in the distance dimension of the RIFT descriptor + */ + inline void + setNrDistanceBins (int nr_distance_bins) { nr_distance_bins_ = nr_distance_bins; }; + + /** \brief Returns the number of bins in the distance dimension of the RIFT descriptor. */ + inline int + getNrDistanceBins () const { return (nr_distance_bins_); }; + + /** \brief Set the number of bins to use in the gradient orientation dimension of the RIFT descriptor + * \param[in] nr_gradient_bins the number of bins to use in the gradient orientation dimension of the RIFT descriptor + */ + inline void + setNrGradientBins (int nr_gradient_bins) { nr_gradient_bins_ = nr_gradient_bins; }; + + /** \brief Returns the number of bins in the gradient orientation dimension of the RIFT descriptor. */ + inline int + getNrGradientBins () const { return (nr_gradient_bins_); }; + + /** \brief Estimate the Rotation Invariant Feature Transform (RIFT) descriptor for a given point based on its + * spatial neighborhood of 3D points and the corresponding intensity gradient vector field + * \param[in] cloud the dataset containing the Cartesian coordinates of the points + * \param[in] gradient the dataset containing the intensity gradient at each point in \a cloud + * \param[in] p_idx the index of the query point in \a cloud (i.e. the center of the neighborhood) + * \param[in] radius the radius of the RIFT feature + * \param[in] indices the indices of the points that comprise \a p_idx's neighborhood in \a cloud + * \param[in] squared_distances the squared distances from the query point to each point in the neighborhood + * \param[out] rift_descriptor the resultant RIFT descriptor + */ + void + computeRIFT (const PointCloudIn &cloud, const PointCloudGradient &gradient, int p_idx, float radius, + const std::vector &indices, const std::vector &squared_distances, + Eigen::MatrixXf &rift_descriptor); + + protected: + + /** \brief Estimate the Rotation Invariant Feature Transform (RIFT) descriptors at a set of points given by + * using the surface in setSearchSurface (), the gradient in + * setInputGradient (), and the spatial locator in setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains the RIFT feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief The intensity gradient of the input point cloud data*/ + PointCloudGradientConstPtr gradient_; + + /** \brief The number of distance bins in the descriptor. */ + int nr_distance_bins_; + + /** \brief The number of gradient orientation bins in the descriptor. */ + int nr_gradient_bins_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/rops_estimation.h b/deps_install/include/pcl-1.10/pcl/features/rops_estimation.h new file mode 100755 index 0000000..d679255 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/rops_estimation.h @@ -0,0 +1,233 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : sergey.s.ushakov@mail.ru + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * This class implements the method for extracting RoPS features presented in the article + * "Rotational Projection Statistics for 3D Local Surface Description and Object Recognition" by + * Yulan Guo, Ferdous Sohel, Mohammed Bennamoun, Min Lu and Jianwei Wan. + */ + template + class PCL_EXPORTS ROPSEstimation : public pcl::Feature + { + public: + + using Feature ::input_; + using Feature ::indices_; + using Feature ::surface_; + using Feature ::tree_; + + using PointCloudOut = typename pcl::Feature ::PointCloudOut; + using PointCloudIn = typename pcl::Feature ::PointCloudIn; + + public: + + /** \brief Simple constructor. */ + ROPSEstimation (); + + /** \brief Virtual destructor. */ + + ~ROPSEstimation (); + + /** \brief Allows to set the number of partition bins that is used for distribution matrix calculation. + * \param[in] number_of_bins number of partition bins + */ + void + setNumberOfPartitionBins (unsigned int number_of_bins); + + /** \brief Returns the number of partition bins. */ + unsigned int + getNumberOfPartitionBins () const; + + /** \brief This method sets the number of rotations. + * \param[in] number_of_rotations number of rotations + */ + void + setNumberOfRotations (unsigned int number_of_rotations); + + /** \brief returns the number of rotations. */ + unsigned int + getNumberOfRotations () const; + + /** \brief Allows to set the support radius that is used to crop the local surface of the point. + * \param[in] support_radius support radius + */ + void + setSupportRadius (float support_radius); + + /** \brief Returns the support radius. */ + float + getSupportRadius () const; + + /** \brief This method sets the triangles of the mesh. + * \param[in] triangles list of triangles of the mesh + */ + void + setTriangles (const std::vector & triangles); + + /** \brief Returns the triangles of the mesh. + * \param[out] triangles triangles of the mesh + */ + void + getTriangles (std::vector & triangles) const; + + private: + + /** \brief Abstract feature estimation method. + * \param[out] output the resultant features + */ + void + computeFeature (PointCloudOut& output) override; + + /** \brief This method simply builds the list of triangles for every point. + * The list of triangles for each point consists of indices of triangles it belongs to. + * The only purpose of this method is to improve performance of the algorithm. + */ + void + buildListOfPointsTriangles (); + + /** \brief This method crops all the triangles within the given radius of the given point. + * \param[in] point point for which the local surface is computed + * \param[out] local_triangles stores the indices of the triangles that belong to the local surface + * \param[out] local_points stores the indices of the points that belong to the local surface + */ + void + getLocalSurface (const PointInT& point, std::set & local_triangles, std::vector & local_points) const; + + /** \brief This method computes LRF (Local Reference Frame) matrix for the given point. + * \param[in] point point for which the LRF is computed + * \param[in] local_triangles list of triangles that represents the local surface of the point + * \paran[out] lrf_matrix stores computed LRF matrix for the given point + */ + void + computeLRF (const PointInT& point, const std::set & local_triangles, Eigen::Matrix3f& lrf_matrix) const; + + /** \brief This method calculates the eigen values and eigen vectors + * for the given covariance matrix. Note that it returns normalized eigen + * vectors that always form the right-handed coordinate system. + * \param[in] matrix covariance matrix of the cloud + * \param[out] major_axis eigen vector which corresponds to a major eigen value + * \param[out] middle_axis eigen vector which corresponds to a middle eigen value + * \param[out] minor_axis eigen vector which corresponds to a minor eigen value + */ + void + computeEigenVectors (const Eigen::Matrix3f& matrix, Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, + Eigen::Vector3f& minor_axis) const; + + /** \brief This method translates the cloud so that the given point becomes the origin. + * After that the cloud is rotated with the help of the given matrix. + * \param[in] point point which stores the translation information + * \param[in] matrix rotation matrix + * \param[in] local_points point to transform + * \param[out] transformed_cloud stores the transformed cloud + */ + void + transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const std::vector & local_points, PointCloudIn& transformed_cloud) const; + + /** \brief This method rotates the cloud around the given axis and computes AABB of the rotated cloud. + * \param[in] axis axis around which cloud must be rotated + * \param[in] angle angle in degrees + * \param[in] cloud cloud to rotate + * \param[out] rotated_cloud stores the rotated cloud + * \param[out] min stores the min point of the AABB + * \param[out] max stores the max point of the AABB + */ + void + rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, + Eigen::Vector3f& min, Eigen::Vector3f& max) const; + + /** \brief This method projects the local surface onto the XY, XZ or YZ plane + * and computes the distribution matrix. + * \param[in] projection represents the case of projection. 1 - XY, 2 - XZ, 3 - YZ + * \param[in] min min point of the AABB + * \param[in] max max point of the AABB + * \param[in] cloud cloud containing the points of the local surface + * \param[out] matrix stores computed distribution matrix + */ + void + getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const; + + /** \brief This method computes the set ofcentral moments for the given matrix. + * \param[in] matrix input matrix + * \param[out] moments set of computed moments + */ + void + computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector & moments) const; + + private: + + /** \brief Stores the number of partition bins that is used for distribution matrix calculation. */ + unsigned int number_of_bins_; + + /** \brief Stores number of rotations. Central moments are calculated for every rotation. */ + unsigned int number_of_rotations_; + + /** \brief Support radius that is used to crop the local surface of the point. */ + float support_radius_; + + /** \brief Stores the squared support radius. Used to improve performance. */ + float sqr_support_radius_; + + /** \brief Stores the angle step. Step is calculated with respect to number of rotations. */ + float step_; + + /** \brief Stores the set of triangles representing the mesh. */ + std::vector triangles_; + + /** \brief Stores the set of triangles for each point. Its purpose is to improve performance. */ + std::vector > triangles_of_the_point_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#define PCL_INSTANTIATE_ROPSEstimation(InT, OutT) template class pcl::ROPSEstimation; + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/rsd.h b/deps_install/include/pcl-1.10/pcl/features/rsd.h new file mode 100755 index 0000000..edda7a5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/rsd.h @@ -0,0 +1,256 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Transform a list of 2D matrices into a point cloud containing the values in a vector (Histogram). + * Can be used to transform the 2D histograms obtained in \ref RSDEstimation into a point cloud. + * @note The template parameter N should be (greater or) equal to the product of the number of rows and columns. + * \param[in] histograms2D the list of neighborhood 2D histograms + * \param[out] histogramsPC the dataset containing the linearized matrices + * \ingroup features + */ + template void + getFeaturePointCloud (const std::vector > &histograms2D, PointCloud > &histogramsPC) + { + histogramsPC.points.resize (histograms2D.size ()); + histogramsPC.width = histograms2D.size (); + histogramsPC.height = 1; + histogramsPC.is_dense = true; + + const int rows = histograms2D.at(0).rows(); + const int cols = histograms2D.at(0).cols(); + + typename PointCloud >::VectorType::iterator it = histogramsPC.points.begin (); + for (const Eigen::MatrixXf& h : histograms2D) + { + Eigen::Map histogram (&(it->histogram[0]), rows, cols); + histogram = h; + ++it; + } + } + + /** \brief Estimate the Radius-based Surface Descriptor (RSD) for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] surface the dataset containing the XYZ points + * \param[in] normals the dataset containing the surface normals at each point in the dataset + * \param[in] indices the neighborhood point indices in the dataset (first point is used as the reference) + * \param[in] max_dist the upper bound for the considered distance interval + * \param[in] nr_subdiv the number of subdivisions for the considered distance interval + * \param[in] plane_radius maximum radius, above which everything can be considered planar + * \param[in] radii the output point of a type that should have r_min and r_max fields + * \param[in] compute_histogram if not false, the full neighborhood histogram is provided, usable as a point signature + * \ingroup features + */ + template Eigen::MatrixXf + computeRSD (const pcl::PointCloud &surface, const pcl::PointCloud &normals, + const std::vector &indices, double max_dist, + int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false); + + template + PCL_DEPRECATED("use computeRSD() overload that takes input point clouds by const reference") + Eigen::MatrixXf + computeRSD (typename pcl::PointCloud::ConstPtr &surface, typename pcl::PointCloud::ConstPtr &normals, + const std::vector &indices, double max_dist, + int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false) + { + return computeRSD (*surface, *normals, indices, max_dist, nr_subdiv, plane_radius, radii, compute_histogram); + } + + /** \brief Estimate the Radius-based Surface Descriptor (RSD) for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] normals the dataset containing the surface normals at each point in the dataset + * \param[in] indices the neighborhood point indices in the dataset (first point is used as the reference) + * \param[in] sqr_dists the squared distances from the first to all points in the neighborhood + * \param[in] max_dist the upper bound for the considered distance interval + * \param[in] nr_subdiv the number of subdivisions for the considered distance interval + * \param[in] plane_radius maximum radius, above which everything can be considered planar + * \param[in] radii the output point of a type that should have r_min and r_max fields + * \param[in] compute_histogram if not false, the full neighborhood histogram is provided, usable as a point signature + * \ingroup features + */ + template Eigen::MatrixXf + computeRSD (const pcl::PointCloud &normals, + const std::vector &indices, const std::vector &sqr_dists, double max_dist, + int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false); + + template + PCL_DEPRECATED("use computeRSD() overload that takes input point cloud by const reference") + Eigen::MatrixXf + computeRSD (typename pcl::PointCloud::ConstPtr &normals, + const std::vector &indices, const std::vector &sqr_dists, double max_dist, + int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false) + { + return computeRSD (*normals, indices, sqr_dists, max_dist, nr_subdiv, plane_radius, radii, compute_histogram); + } + + /** \brief @b RSDEstimation estimates the Radius-based Surface Descriptor (minimal and maximal radius of the local surface's curves) + * for a given point cloud dataset containing points and normals. + * + * @note If you use this code in any academic work, please cite: + * + *
    + *
  • Z.C. Marton , D. Pangercic , N. Blodow , J. Kleinehellefort, M. Beetz + * General 3D Modelling of Novel Objects from a Single View + * In Proceedings of the 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) + * Taipei, Taiwan, October 18-22, 2010 + *
    • + *
  • Z.C. Marton, D. Pangercic, N. Blodow, Michael Beetz. + * Combined 2D-3D Categorization and Classification for Multimodal Perception Systems. + * In The International Journal of Robotics Research, Sage Publications + * pages 1378--1402, Volume 30, Number 11, September 2011. + *
    • + *
+ * + * @note The code is stateful as we do not expect this class to be multicore parallelized. + * \author Zoltan-Csaba Marton + * \ingroup features + */ + template + class RSDEstimation : public FeatureFromNormals + { + public: + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::search_radius_; + using Feature::search_parameter_; + using Feature::surface_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Empty constructor. */ + RSDEstimation () : nr_subdiv_ (5), plane_radius_ (0.2), save_histograms_ (false) + { + feature_name_ = "RadiusSurfaceDescriptor"; + }; + + /** \brief Set the number of subdivisions for the considered distance interval. + * \param[in] nr_subdiv the number of subdivisions + */ + inline void + setNrSubdivisions (int nr_subdiv) { nr_subdiv_ = nr_subdiv; } + + /** \brief Get the number of subdivisions for the considered distance interval. + * \return the number of subdivisions + */ + inline int + getNrSubdivisions () const { return (nr_subdiv_); } + + /** \brief Set the maximum radius, above which everything can be considered planar. + * \note the order of magnitude should be around 10-20 times the search radius (0.2 works well for typical datasets). + * \note on accurate 3D data (e.g. openni sernsors) a search radius as low as 0.01 still gives good results. + * \param[in] plane_radius the new plane radius + */ + inline void + setPlaneRadius (double plane_radius) { plane_radius_ = plane_radius; } + + /** \brief Get the maximum radius, above which everything can be considered planar. + * \return the plane_radius used + */ + inline double + getPlaneRadius () const { return (plane_radius_); } + + /** \brief Disables the setting of the number of k nearest neighbors to use for the feature estimation. */ + inline void + setKSearch (int) + { + PCL_ERROR ("[pcl::%s::setKSearch] RSD does not work with k nearest neighbor search. Use setRadiusSearch() instead!\n", getClassName ().c_str ()); + } + + /** \brief Set whether the full distance-angle histograms should be saved. + * @note Obtain the list of histograms by getHistograms () + * \param[in] save set to true if histograms should be saved + */ + inline void + setSaveHistograms (bool save) { save_histograms_ = save; } + + /** \brief Returns whether the full distance-angle histograms are being saved. + * \return true if the histograms are being be saved + */ + inline bool + getSaveHistograms () const { return (save_histograms_); } + + /** \brief Returns a pointer to the list of full distance-angle histograms for all points. + * \return the histogram being saved when computing RSD + */ + inline shared_ptr > > + getHistograms () const { return (histograms_); } + + protected: + + /** \brief Estimate the estimates the Radius-based Surface Descriptor (RSD) at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param output the resultant point cloud model dataset that contains the RSD feature estimates (r_min and r_max values) + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief The list of full distance-angle histograms for all points. */ + shared_ptr > > histograms_; + + private: + /** \brief The number of subdivisions for the considered distance interval. */ + int nr_subdiv_; + + /** \brief The maximum radius, above which everything can be considered planar. */ + double plane_radius_; + + /** \brief Signals whether the full distance-angle histograms are being saved. */ + bool save_histograms_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/shot.h b/deps_install/include/pcl-1.10/pcl/features/shot.h new file mode 100755 index 0000000..19a0247 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/shot.h @@ -0,0 +1,410 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SHOTEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for + * a given point cloud dataset containing points and normals. + * + * The suggested PointOutT is pcl::SHOT352. + * + * \note If you use this code in any academic work, please cite: + * + * - F. Tombari, S. Salti, L. Di Stefano + * Unique Signatures of Histograms for Local Surface Description. + * In Proceedings of the 11th European Conference on Computer Vision (ECCV), + * Heraklion, Greece, September 5-11 2010. + * - F. Tombari, S. Salti, L. Di Stefano + * A Combined Texture-Shape Descriptor For Enhanced 3D Feature Matching. + * In Proceedings of the 18th International Conference on Image Processing (ICIP), + * Brussels, Belgium, September 11-14 2011. + * + * \author Samuele Salti, Federico Tombari + * \ingroup features + */ + template + class SHOTEstimationBase : public FeatureFromNormals, + public FeatureWithLocalReferenceFrames + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::input_; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::search_radius_; + using Feature::surface_; + using Feature::fake_surface_; + using FeatureFromNormals::normals_; + using FeatureWithLocalReferenceFrames::frames_; + + using PointCloudIn = typename Feature::PointCloudIn; + + protected: + /** \brief Empty constructor. + * \param[in] nr_shape_bins the number of bins in the shape histogram + */ + SHOTEstimationBase (int nr_shape_bins = 10) : + nr_shape_bins_ (nr_shape_bins), + lrf_radius_ (0), + sqradius_ (0), radius3_4_ (0), radius1_4_ (0), radius1_2_ (0), + nr_grid_sector_ (32), + maxAngularSectors_ (32), + descLength_ (0) + { + feature_name_ = "SHOTEstimation"; + }; + + + public: + + /** \brief Empty destructor */ + ~SHOTEstimationBase () {} + + /** \brief Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] index the index of the point in indices_ + * \param[in] indices the k-neighborhood point indices in surface_ + * \param[in] sqr_dists the k-neighborhood point distances in surface_ + * \param[out] shot the resultant SHOT descriptor representing the feature at the query point + */ + virtual void + computePointSHOT (const int index, + const std::vector &indices, + const std::vector &sqr_dists, + Eigen::VectorXf &shot) = 0; + + /** \brief Set the radius used for local reference frame estimation if the frames are not set by the user */ + virtual void + setLRFRadius (float radius) { lrf_radius_ = radius; } + + /** \brief Get the radius used for local reference frame estimation */ + virtual float + getLRFRadius () const { return lrf_radius_; } + + protected: + + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief Quadrilinear interpolation used when color and shape descriptions are NOT activated simultaneously + * + * \param[in] indices the neighborhood point indices + * \param[in] sqr_dists the neighborhood point distances + * \param[in] index the index of the point in indices_ + * \param[out] binDistance the resultant distance shape histogram + * \param[in] nr_bins the number of bins in the shape histogram + * \param[out] shot the resultant SHOT histogram + */ + void + interpolateSingleChannel (const std::vector &indices, + const std::vector &sqr_dists, + const int index, + std::vector &binDistance, + const int nr_bins, + Eigen::VectorXf &shot); + + /** \brief Normalize the SHOT histogram. + * \param[in,out] shot the SHOT histogram + * \param[in] desc_length the length of the histogram + */ + void + normalizeHistogram (Eigen::VectorXf &shot, int desc_length); + + + /** \brief Create a binned distance shape histogram + * \param[in] index the index of the point in indices_ + * \param[in] indices the k-neighborhood point indices in surface_ + * \param[out] bin_distance_shape the resultant histogram + */ + void + createBinDistanceShape (int index, const std::vector &indices, + std::vector &bin_distance_shape); + + /** \brief The number of bins in each shape histogram. */ + int nr_shape_bins_; + + /** \brief Placeholder for a point's SHOT. */ + Eigen::VectorXf shot_; + + /** \brief The radius used for the LRF computation */ + float lrf_radius_; + + /** \brief The squared search radius. */ + double sqradius_; + + /** \brief 3/4 of the search radius. */ + double radius3_4_; + + /** \brief 1/4 of the search radius. */ + double radius1_4_; + + /** \brief 1/2 of the search radius. */ + double radius1_2_; + + /** \brief Number of azimuthal sectors. */ + const int nr_grid_sector_; + + /** \brief ... */ + const int maxAngularSectors_; + + /** \brief One SHOT length. */ + int descLength_; + }; + + /** \brief SHOTEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for + * a given point cloud dataset containing points and normals. + * + * The suggested PointOutT is pcl::SHOT352 + * + * \note If you use this code in any academic work, please cite: + * + * - F. Tombari, S. Salti, L. Di Stefano + * Unique Signatures of Histograms for Local Surface Description. + * In Proceedings of the 11th European Conference on Computer Vision (ECCV), + * Heraklion, Greece, September 5-11 2010. + * - F. Tombari, S. Salti, L. Di Stefano + * A Combined Texture-Shape Descriptor For Enhanced 3D Feature Matching. + * In Proceedings of the 18th International Conference on Image Processing (ICIP), + * Brussels, Belgium, September 11-14 2011. + * + * \author Samuele Salti, Federico Tombari + * \ingroup features + */ + template + class SHOTEstimation : public SHOTEstimationBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using SHOTEstimationBase::feature_name_; + using SHOTEstimationBase::getClassName; + using SHOTEstimationBase::indices_; + using SHOTEstimationBase::k_; + using SHOTEstimationBase::search_parameter_; + using SHOTEstimationBase::search_radius_; + using SHOTEstimationBase::surface_; + using SHOTEstimationBase::input_; + using SHOTEstimationBase::normals_; + using SHOTEstimationBase::descLength_; + using SHOTEstimationBase::nr_grid_sector_; + using SHOTEstimationBase::nr_shape_bins_; + using SHOTEstimationBase::sqradius_; + using SHOTEstimationBase::radius3_4_; + using SHOTEstimationBase::radius1_4_; + using SHOTEstimationBase::radius1_2_; + using SHOTEstimationBase::maxAngularSectors_; + using SHOTEstimationBase::interpolateSingleChannel; + using SHOTEstimationBase::shot_; + using FeatureWithLocalReferenceFrames::frames_; + + using PointCloudIn = typename Feature::PointCloudIn; + + /** \brief Empty constructor. */ + SHOTEstimation () : SHOTEstimationBase (10) + { + feature_name_ = "SHOTEstimation"; + }; + + /** \brief Empty destructor */ + ~SHOTEstimation () {} + + /** \brief Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] index the index of the point in indices_ + * \param[in] indices the k-neighborhood point indices in surface_ + * \param[in] sqr_dists the k-neighborhood point distances in surface_ + * \param[out] shot the resultant SHOT descriptor representing the feature at the query point + */ + void + computePointSHOT (const int index, + const std::vector &indices, + const std::vector &sqr_dists, + Eigen::VectorXf &shot) override; + protected: + /** \brief Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param output the resultant point cloud model dataset that contains the SHOT feature estimates + */ + void + computeFeature (pcl::PointCloud &output) override; + }; + + /** \brief SHOTColorEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a given point cloud dataset + * containing points, normals and colors. + * + * The suggested PointOutT is pcl::SHOT1344 + * + * \note If you use this code in any academic work, please cite: + * + * - F. Tombari, S. Salti, L. Di Stefano + * Unique Signatures of Histograms for Local Surface Description. + * In Proceedings of the 11th European Conference on Computer Vision (ECCV), + * Heraklion, Greece, September 5-11 2010. + * - F. Tombari, S. Salti, L. Di Stefano + * A Combined Texture-Shape Descriptor For Enhanced 3D Feature Matching. + * In Proceedings of the 18th International Conference on Image Processing (ICIP), + * Brussels, Belgium, September 11-14 2011. + * + * \author Samuele Salti, Federico Tombari + * \ingroup features + */ + template + class SHOTColorEstimation : public SHOTEstimationBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using SHOTEstimationBase::feature_name_; + using SHOTEstimationBase::getClassName; + using SHOTEstimationBase::indices_; + using SHOTEstimationBase::k_; + using SHOTEstimationBase::search_parameter_; + using SHOTEstimationBase::search_radius_; + using SHOTEstimationBase::surface_; + using SHOTEstimationBase::input_; + using SHOTEstimationBase::normals_; + using SHOTEstimationBase::descLength_; + using SHOTEstimationBase::nr_grid_sector_; + using SHOTEstimationBase::nr_shape_bins_; + using SHOTEstimationBase::sqradius_; + using SHOTEstimationBase::radius3_4_; + using SHOTEstimationBase::radius1_4_; + using SHOTEstimationBase::radius1_2_; + using SHOTEstimationBase::maxAngularSectors_; + using SHOTEstimationBase::interpolateSingleChannel; + using SHOTEstimationBase::shot_; + using FeatureWithLocalReferenceFrames::frames_; + + using PointCloudIn = typename Feature::PointCloudIn; + + /** \brief Empty constructor. + * \param[in] describe_shape + * \param[in] describe_color + */ + SHOTColorEstimation (bool describe_shape = true, + bool describe_color = true) + : SHOTEstimationBase (10), + b_describe_shape_ (describe_shape), + b_describe_color_ (describe_color), + nr_color_bins_ (30) + { + feature_name_ = "SHOTColorEstimation"; + }; + + /** \brief Empty destructor */ + ~SHOTColorEstimation () {} + + /** \brief Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] index the index of the point in indices_ + * \param[in] indices the k-neighborhood point indices in surface_ + * \param[in] sqr_dists the k-neighborhood point distances in surface_ + * \param[out] shot the resultant SHOT descriptor representing the feature at the query point + */ + void + computePointSHOT (const int index, + const std::vector &indices, + const std::vector &sqr_dists, + Eigen::VectorXf &shot) override; + protected: + /** \brief Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param output the resultant point cloud model dataset that contains the SHOT feature estimates + */ + void + computeFeature (pcl::PointCloud &output) override; + + /** \brief Quadrilinear interpolation; used when color and shape descriptions are both activated + * \param[in] indices the neighborhood point indices + * \param[in] sqr_dists the neighborhood point distances + * \param[in] index the index of the point in indices_ + * \param[out] binDistanceShape the resultant distance shape histogram + * \param[out] binDistanceColor the resultant color shape histogram + * \param[in] nr_bins_shape the number of bins in the shape histogram + * \param[in] nr_bins_color the number of bins in the color histogram + * \param[out] shot the resultant SHOT histogram + */ + void + interpolateDoubleChannel (const std::vector &indices, + const std::vector &sqr_dists, + const int index, + std::vector &binDistanceShape, + std::vector &binDistanceColor, + const int nr_bins_shape, + const int nr_bins_color, + Eigen::VectorXf &shot); + + /** \brief Compute shape descriptor. */ + bool b_describe_shape_; + + /** \brief Compute color descriptor. */ + bool b_describe_color_; + + /** \brief The number of bins in each color histogram. */ + int nr_color_bins_; + + public: + /** \brief Converts RGB triplets to CIELab space. + * \param[in] R the red channel + * \param[in] G the green channel + * \param[in] B the blue channel + * \param[out] L the lightness + * \param[out] A the first color-opponent dimension + * \param[out] B2 the second color-opponent dimension + */ + static void + RGB2CIELAB (unsigned char R, unsigned char G, unsigned char B, float &L, float &A, float &B2); + + static float sRGB_LUT[256]; + static float sXYZ_LUT[4000]; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/shot_lrf.h b/deps_install/include/pcl-1.10/pcl/features/shot_lrf.h new file mode 100755 index 0000000..35c28f1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/shot_lrf.h @@ -0,0 +1,109 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation + * of the (SHOT) descriptor. + * + * \note If you use this code in any academic work, please cite: + * + * - F. Tombari, S. Salti, L. Di Stefano + * Unique Signatures of Histograms for Local Surface Description. + * In Proceedings of the 11th European Conference on Computer Vision (ECCV), + * Heraklion, Greece, September 5-11 2010. + * - F. Tombari, S. Salti, L. Di Stefano + * A Combined Texture-Shape Descriptor For Enhanced 3D Feature Matching. + * In Proceedings of the 18th International Conference on Image Processing (ICIP), + * Brussels, Belgium, September 11-14 2011. + * + * \author Samuele Salti, Federico Tombari + * \ingroup features + */ + template + class SHOTLocalReferenceFrameEstimation : public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + /** \brief Constructor */ + SHOTLocalReferenceFrameEstimation () + { + feature_name_ = "SHOTLocalReferenceFrameEstimation"; + } + + /** \brief Empty destructor */ + ~SHOTLocalReferenceFrameEstimation () {} + + protected: + using Feature::feature_name_; + using Feature::getClassName; + //using Feature::searchForNeighbors; + using Feature::input_; + using Feature::indices_; + using Feature::surface_; + using Feature::tree_; + using Feature::search_parameter_; + + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Computes disambiguated local RF for a point index + * \param[in] index the index + * \param[out] rf reference frame to compute + */ + float + getLocalRF (const int &index, Eigen::Matrix3f &rf); + + /** \brief Feature estimation method. + * \param[out] output the resultant features + */ + void + computeFeature (PointCloudOut &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/shot_lrf_omp.h b/deps_install/include/pcl-1.10/pcl/features/shot_lrf_omp.h new file mode 100755 index 0000000..2100d09 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/shot_lrf_omp.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation + * of the (SHOT) descriptor. + * + * \note If you use this code in any academic work, please cite: + * + * - F. Tombari, S. Salti, L. Di Stefano + * Unique Signatures of Histograms for Local Surface Description. + * In Proceedings of the 11th European Conference on Computer Vision (ECCV), + * Heraklion, Greece, September 5-11 2010. + * - F. Tombari, S. Salti, L. Di Stefano + * A Combined Texture-Shape Descriptor For Enhanced 3D Feature Matching. + * In Proceedings of the 18th International Conference on Image Processing (ICIP), + * Brussels, Belgium, September 11-14 2011. + * + * \author Samuele Salti, Federico Tombari + * \ingroup features + */ + template + class SHOTLocalReferenceFrameEstimationOMP : public SHOTLocalReferenceFrameEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + /** \brief Constructor */ + SHOTLocalReferenceFrameEstimationOMP () + { + feature_name_ = "SHOTLocalReferenceFrameEstimationOMP"; + + setNumberOfThreads(0); + } + + /** \brief Empty destructor */ + ~SHOTLocalReferenceFrameEstimationOMP () {} + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + protected: + using Feature::feature_name_; + using Feature::getClassName; + //using Feature::searchForNeighbors; + using Feature::input_; + using Feature::indices_; + using Feature::surface_; + using Feature::tree_; + using Feature::search_parameter_; + using SHOTLocalReferenceFrameEstimation::getLocalRF; + using PointCloudIn = typename Feature::PointCloudIn; + using PointCloudOut = typename Feature::PointCloudOut; + + /** \brief Feature estimation method. + * \param[out] output the resultant features + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/shot_omp.h b/deps_install/include/pcl-1.10/pcl/features/shot_omp.h new file mode 100755 index 0000000..59d48ea --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/shot_omp.h @@ -0,0 +1,216 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief SHOTEstimationOMP estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a given point cloud dataset + * containing points and normals, in parallel, using the OpenMP standard. + * + * The suggested PointOutT is pcl::SHOT352. + * + * \note If you use this code in any academic work, please cite: + * + * - F. Tombari, S. Salti, L. Di Stefano + * Unique Signatures of Histograms for Local Surface Description. + * In Proceedings of the 11th European Conference on Computer Vision (ECCV), + * Heraklion, Greece, September 5-11 2010. + * - F. Tombari, S. Salti, L. Di Stefano + * A Combined Texture-Shape Descriptor For Enhanced 3D Feature Matching. + * In Proceedings of the 18th International Conference on Image Processing (ICIP), + * Brussels, Belgium, September 11-14 2011. + * + * \author Samuele Salti + * \ingroup features + */ + + template + class SHOTEstimationOMP : public SHOTEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::input_; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::search_radius_; + using Feature::surface_; + using Feature::fake_surface_; + using FeatureFromNormals::normals_; + using FeatureWithLocalReferenceFrames::frames_; + using SHOTEstimationBase::lrf_radius_; + using SHOTEstimation::descLength_; + using SHOTEstimation::nr_grid_sector_; + using SHOTEstimation::nr_shape_bins_; + using SHOTEstimation::sqradius_; + using SHOTEstimation::radius3_4_; + using SHOTEstimation::radius1_4_; + using SHOTEstimation::radius1_2_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + + /** \brief Empty constructor. */ + SHOTEstimationOMP (unsigned int nr_threads = 0) : SHOTEstimation () + { + setNumberOfThreads(nr_threads); + }; + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + protected: + + /** \brief Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param output the resultant point cloud model dataset that contains the SHOT feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + }; + + /** \brief SHOTColorEstimationOMP estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a given point cloud dataset + * containing points, normals and colors, in parallel, using the OpenMP standard. + * + * The suggested PointOutT is pcl::SHOT1344. + * + * \note If you use this code in any academic work, please cite: + * + * - F. Tombari, S. Salti, L. Di Stefano + * Unique Signatures of Histograms for Local Surface Description. + * In Proceedings of the 11th European Conference on Computer Vision (ECCV), + * Heraklion, Greece, September 5-11 2010. + * - F. Tombari, S. Salti, L. Di Stefano + * A Combined Texture-Shape Descriptor For Enhanced 3D Feature Matching. + * In Proceedings of the 18th International Conference on Image Processing (ICIP), + * Brussels, Belgium, September 11-14 2011. + * + * \author Samuele Salti + * \ingroup features + */ + + template + class SHOTColorEstimationOMP : public SHOTColorEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::input_; + using Feature::indices_; + using Feature::k_; + using Feature::search_parameter_; + using Feature::search_radius_; + using Feature::surface_; + using Feature::fake_surface_; + using FeatureFromNormals::normals_; + using FeatureWithLocalReferenceFrames::frames_; + using SHOTEstimationBase::lrf_radius_; + using SHOTColorEstimation::descLength_; + using SHOTColorEstimation::nr_grid_sector_; + using SHOTColorEstimation::nr_shape_bins_; + using SHOTColorEstimation::sqradius_; + using SHOTColorEstimation::radius3_4_; + using SHOTColorEstimation::radius1_4_; + using SHOTColorEstimation::radius1_2_; + using SHOTColorEstimation::b_describe_shape_; + using SHOTColorEstimation::b_describe_color_; + using SHOTColorEstimation::nr_color_bins_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + + /** \brief Empty constructor. */ + SHOTColorEstimationOMP (bool describe_shape = true, + bool describe_color = true, + unsigned int nr_threads = 0) + : SHOTColorEstimation (describe_shape, describe_color) + { + setNumberOfThreads(nr_threads); + } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + protected: + + /** \brief Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param output the resultant point cloud model dataset that contains the SHOT feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + }; + +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/spin_image.h b/deps_install/include/pcl-1.10/pcl/features/spin_image.h new file mode 100755 index 0000000..30d5b51 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/spin_image.h @@ -0,0 +1,284 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Estimates spin-image descriptors in the given input points. + * + * This class represents spin image descriptor. Spin image is + * a histogram of point locations summed along the bins of the image. + * A 2D accumulator indexed by a and b is created. Next, + * the coordinates (a, b) are computed for a vertex in + * the surface mesh that is within the support of the spin image + * (explained below). The bin indexed by (a, b) in + * the accumulator is then incremented; bilinear interpolation is used + * to smooth the contribution of the vertex. This procedure is repeated + * for all vertices within the support of the spin image. + * The resulting accumulator can be thought of as an image; + * dark areas in the image correspond to bins that contain many projected points. + * As long as the size of the bins in the accumulator is greater + * than the median distance between vertices in the mesh + * (the definition of mesh resolution), the position of individual + * vertices will be averaged out during spin image generation. + * + * \attention The input normals given by \ref setInputNormals have to match + * the input point cloud given by \ref setInputCloud. This behavior is + * different than feature estimation methods that extend \ref + * FeatureFromNormals, which match the normals with the search surface. + * + * With the default parameters, pcl::Histogram<153> is a good choice for PointOutT. + * Of course the dimension of this descriptor must change to match the number + * of bins set by the parameters. + * + * For further information please see: + * + * - Johnson, A. E., & Hebert, M. (1998). Surface Matching for Object + * Recognition in Complex 3D Scenes. Image and Vision Computing, 16, + * 635-651. + * + * The class also implements radial spin images and spin-images in angular domain + * (or both). + * + * \author Roman Shapovalov, Alexander Velizhev + * \ingroup features + */ + template + class SpinImageEstimation : public Feature + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::search_radius_; + using Feature::k_; + using Feature::surface_; + using Feature::fake_surface_; + using PCLBase::input_; + + using PointCloudOut = typename Feature::PointCloudOut; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + /** \brief Constructs empty spin image estimator. + * + * \param[in] image_width spin-image resolution, number of bins along one dimension + * \param[in] support_angle_cos minimal allowed cosine of the angle between + * the normals of input point and search surface point for the point + * to be retained in the support + * \param[in] min_pts_neighb min number of points in the support to correctly estimate + * spin-image. If at some point the support contains less points, exception is thrown + */ + SpinImageEstimation (unsigned int image_width = 8, + double support_angle_cos = 0.0, // when 0, this is bogus, so not applied + unsigned int min_pts_neighb = 0); + + /** \brief Empty destructor */ + ~SpinImageEstimation () {} + + /** \brief Sets spin-image resolution. + * + * \param[in] bin_count spin-image resolution, number of bins along one dimension + */ + void + setImageWidth (unsigned int bin_count) + { + image_width_ = bin_count; + } + + /** \brief Sets the maximum angle for the point normal to get to support region. + * + * \param[in] support_angle_cos minimal allowed cosine of the angle between + * the normals of input point and search surface point for the point + * to be retained in the support + */ + void + setSupportAngle (double support_angle_cos) + { + if (0.0 > support_angle_cos || support_angle_cos > 1.0) // may be permit negative cosine? + { + throw PCLException ("Cosine of support angle should be between 0 and 1", + "spin_image.h", "setSupportAngle"); + } + + support_angle_cos_ = support_angle_cos; + } + + /** \brief Sets minimal points count for spin image computation. + * + * \param[in] min_pts_neighb min number of points in the support to correctly estimate + * spin-image. If at some point the support contains less points, exception is thrown + */ + void + setMinPointCountInNeighbourhood (unsigned int min_pts_neighb) + { + min_pts_neighb_ = min_pts_neighb; + } + + /** \brief Provide a pointer to the input dataset that contains the point normals of + * the input XYZ dataset given by \ref setInputCloud + * + * \attention The input normals given by \ref setInputNormals have to match + * the input point cloud given by \ref setInputCloud. This behavior is + * different than feature estimation methods that extend \ref + * FeatureFromNormals, which match the normals with the search surface. + * \param[in] normals the const boost shared pointer to a PointCloud of normals. + * By convention, L2 norm of each normal should be 1. + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) + { + input_normals_ = normals; + } + + /** \brief Sets single vector a rotation axis for all input points. + * + * It could be useful e.g. when the vertical axis is known. + * \param[in] axis unit-length vector that serves as rotation axis for reference frame + */ + void + setRotationAxis (const PointNT& axis) + { + rotation_axis_ = axis; + use_custom_axis_ = true; + use_custom_axes_cloud_ = false; + } + + /** \brief Sets array of vectors as rotation axes for input points. + * + * Useful e.g. when one wants to use tangents instead of normals as rotation axes + * \param[in] axes unit-length vectors that serves as rotation axes for + * the corresponding input points' reference frames + */ + void + setInputRotationAxes (const PointCloudNConstPtr& axes) + { + rotation_axes_cloud_ = axes; + + use_custom_axes_cloud_ = true; + use_custom_axis_ = false; + } + + /** \brief Sets input normals as rotation axes (default setting). */ + void + useNormalsAsRotationAxis () + { + use_custom_axis_ = false; + use_custom_axes_cloud_ = false; + } + + /** \brief Sets/unsets flag for angular spin-image domain. + * + * Angular spin-image differs from the vanilla one in the way that not + * the points are collected in the bins but the angles between their + * normals and the normal to the reference point. For further + * information please see + * Endres, F., Plagemann, C., Stachniss, C., & Burgard, W. (2009). + * Unsupervised Discovery of Object Classes from Range Data using Latent Dirichlet Allocation. + * In Robotics: Science and Systems. Seattle, USA. + * \param[in] is_angular true for angular domain, false for point domain + */ + void + setAngularDomain (bool is_angular = true) { is_angular_ = is_angular; } + + /** \brief Sets/unsets flag for radial spin-image structure. + * + * Instead of rectangular coordinate system for reference frame + * polar coordinates are used. Binning is done depending on the distance and + * inclination angle from the reference point + * \param[in] is_radial true for radial spin-image structure, false for rectangular + */ + void + setRadialStructure (bool is_radial = true) { is_radial_ = is_radial; } + + protected: + /** \brief Estimate the Spin Image descriptors at a set of points given by + * setInputWithNormals() using the surface in setSearchSurfaceWithNormals() and the spatial locator + * \param[out] output the resultant point cloud that contains the Spin Image feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief initializes computations specific to spin-image. + * + * \return true iff input data and initialization are correct + */ + bool + initCompute () override; + + /** \brief Computes a spin-image for the point of the scan. + * \param[in] index the index of the reference point in the input cloud + * \return estimated spin-image (or its variant) as a matrix + */ + Eigen::ArrayXXd + computeSiForPoint (int index) const; + + private: + PointCloudNConstPtr input_normals_; + PointCloudNConstPtr rotation_axes_cloud_; + + bool is_angular_; + + PointNT rotation_axis_; + bool use_custom_axis_; + bool use_custom_axes_cloud_; + + bool is_radial_; + + unsigned int image_width_; + double support_angle_cos_; + unsigned int min_pts_neighb_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/statistical_multiscale_interest_region_extraction.h b/deps_install/include/pcl-1.10/pcl/features/statistical_multiscale_interest_region_extraction.h new file mode 100755 index 0000000..35345d5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/statistical_multiscale_interest_region_extraction.h @@ -0,0 +1,126 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Class for extracting interest regions from unstructured point clouds, based on a multi scale + * statistical approach. + * Please refer to the following publications for more details: + * Ranjith Unnikrishnan and Martial Hebert + * Multi-Scale Interest Regions from Unorganized Point Clouds + * Workshop on Search in 3D (S3D), IEEE Conf. on Computer Vision and Pattern Recognition (CVPR) + * June, 2008 + * + * Statistical Approaches to Multi-scale Point Cloud Processing + * Ranjith Unnikrishnan + * PhD Thesis + * The Robotics Institute Carnegie Mellon University + * May, 2008 + * + * \author Alexandru-Eugen Ichim + */ + template + class StatisticalMultiscaleInterestRegionExtraction : public PCLBase + { + public: + using IndicesPtr = shared_ptr >; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Empty constructor */ + StatisticalMultiscaleInterestRegionExtraction () + {}; + + /** \brief Method that generates the underlying nearest neighbor graph based on the + * input point cloud + */ + void + generateCloudGraph (); + + /** \brief The method to be called in order to run the algorithm and produce the resulting + * set of regions of interest + */ + void + computeRegionsOfInterest (std::list& rois); + + /** \brief Method for setting the scale parameters for the algorithm + * \param scale_values vector of scales to determine the size of each scaling step + */ + inline void + setScalesVector (std::vector &scale_values) { scale_values_ = scale_values; } + + /** \brief Method for getting the scale parameters vector */ + inline std::vector + getScalesVector () { return scale_values_; } + + + private: + /** \brief Checks if all the necessary input was given and the computations can successfully start */ + bool + initCompute (); + + void + geodesicFixedRadiusSearch (std::size_t &query_index, + float &radius, + std::vector &result_indices); + + void + computeF (); + + void + extractExtrema (std::list& rois); + + using PCLBase::initCompute; + using PCLBase::input_; + std::vector scale_values_; + std::vector > geodesic_distances_; + std::vector > F_scales_; + }; +} + + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/usc.h b/deps_install/include/pcl-1.10/pcl/features/usc.h new file mode 100755 index 0000000..03a402a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/usc.h @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief UniqueShapeContext implements the Unique Shape Context Descriptor + * described here: + * + * - F. Tombari, S. Salti, L. Di Stefano, + * "Unique Shape Context for 3D data description", + * International Workshop on 3D Object Retrieval (3DOR 10) - + * in conjunction with ACM Multimedia 2010 + * + * The suggested PointOutT is pcl::UniqueShapeContext1960 + * + * \author Alessandro Franchi, Federico Tombari, Samuele Salti (original code) + * \author Nizar Sallem (port to PCL) + * \ingroup features + */ + template + class UniqueShapeContext : public Feature, + public FeatureWithLocalReferenceFrames + { + public: + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::search_parameter_; + using Feature::search_radius_; + using Feature::surface_; + using Feature::fake_surface_; + using Feature::input_; + using Feature::searchForNeighbors; + using FeatureWithLocalReferenceFrames::frames_; + + using PointCloudOut = typename Feature::PointCloudOut; + using PointCloudIn = typename Feature::PointCloudIn; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Constructor. */ + UniqueShapeContext () : + radii_interval_(0), theta_divisions_(0), phi_divisions_(0), volume_lut_(0), + azimuth_bins_(14), elevation_bins_(14), radius_bins_(10), + min_radius_(0.1), point_density_radius_(0.1), descriptor_length_ (), local_radius_ (2.0) + { + feature_name_ = "UniqueShapeContext"; + search_radius_ = 2.0; + } + + ~UniqueShapeContext() { } + + /** \return The number of bins along the azimuth. */ + inline std::size_t + getAzimuthBins () const { return (azimuth_bins_); } + + /** \return The number of bins along the elevation */ + inline std::size_t + getElevationBins () const { return (elevation_bins_); } + + /** \return The number of bins along the radii direction. */ + inline std::size_t + getRadiusBins () const { return (radius_bins_); } + + /** The minimal radius value for the search sphere (rmin) in the original paper + * \param[in] radius the desired minimal radius + */ + inline void + setMinimalRadius (double radius) { min_radius_ = radius; } + + /** \return The minimal sphere radius. */ + inline double + getMinimalRadius () const { return (min_radius_); } + + /** This radius is used to compute local point density + * density = number of points within this radius + * \param[in] radius Value of the point density search radius + */ + inline void + setPointDensityRadius (double radius) { point_density_radius_ = radius; } + + /** \return The point density search radius. */ + inline double + getPointDensityRadius () const { return (point_density_radius_); } + + /** Set the local RF radius value + * \param[in] radius the desired local RF radius + */ + inline void + setLocalRadius (double radius) { local_radius_ = radius; } + + /** \return The local RF radius. */ + inline double + getLocalRadius () const { return (local_radius_); } + + protected: + /** Compute 3D shape context feature descriptor + * \param[in] index point index in input_ + * \param[out] desc descriptor to compute + */ + void + computePointDescriptor (std::size_t index, std::vector &desc); + + /** \brief Initialize computation by allocating all the intervals and the volume lookup table. */ + bool + initCompute () override; + + /** \brief The actual feature computation. + * \param[out] output the resultant features + */ + void + computeFeature (PointCloudOut &output) override; + + /** \brief values of the radii interval. */ + std::vector radii_interval_; + + /** \brief Theta divisions interval. */ + std::vector theta_divisions_; + + /** \brief Phi divisions interval. */ + std::vector phi_divisions_; + + /** \brief Volumes look up table. */ + std::vector volume_lut_; + + /** \brief Bins along the azimuth dimension. */ + std::size_t azimuth_bins_; + + /** \brief Bins along the elevation dimension. */ + std::size_t elevation_bins_; + + /** \brief Bins along the radius dimension. */ + std::size_t radius_bins_; + + /** \brief Minimal radius value. */ + double min_radius_; + + /** \brief Point density radius. */ + double point_density_radius_; + + /** \brief Descriptor length. */ + std::size_t descriptor_length_; + + /** \brief Radius to compute local RF. */ + double local_radius_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/features/vfh.h b/deps_install/include/pcl-1.10/pcl/features/vfh.h new file mode 100755 index 0000000..7a7b7f3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/features/vfh.h @@ -0,0 +1,269 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl +{ + /** \brief VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud + * dataset containing points and normals. The default VFH implementation uses 45 binning subdivisions for each of + * the three extended FPFH values, plus another 45 binning subdivisions for the distances between each point and + * the centroid and 128 binning subdivisions for the viewpoint component, which results in a + * 308-byte array of float values. These are stored in a pcl::VFHSignature308 point type. + * A major difference between the PFH/FPFH descriptors and VFH, is that for a given point cloud dataset, only a + * single VFH descriptor will be estimated (vfhs->points.size() should be 1), while the resultant PFH/FPFH data + * will have the same number of entries as the number of points in the cloud. + * + * \note If you use this code in any academic work, please cite: + * + * - R.B. Rusu, G. Bradski, R. Thibaux, J. Hsu. + * Fast 3D Recognition and Pose Using the Viewpoint Feature Histogram. + * In Proceedings of International Conference on Intelligent Robots and Systems (IROS) + * Taipei, Taiwan, October 18-22 2010. + * + * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at + * \ref FPFHEstimationOMP for an example of a parallel implementation of the FPFH (Fast Point Feature Histogram). + * \author Radu B. Rusu + * \ingroup features + */ + template + class VFHEstimation : public FeatureFromNormals + { + public: + using Feature::feature_name_; + using Feature::getClassName; + using Feature::indices_; + using Feature::k_; + using Feature::search_radius_; + using Feature::input_; + using Feature::surface_; + using FeatureFromNormals::normals_; + + using PointCloudOut = typename Feature::PointCloudOut; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Empty constructor. */ + VFHEstimation () : + nr_bins_f_ ({45, 45, 45, 45}), nr_bins_vp_ (128), + vpx_ (0), vpy_ (0), vpz_ (0), + use_given_normal_ (false), use_given_centroid_ (false), + normalize_bins_ (true), normalize_distances_ (false), size_component_ (false), + d_pi_ (1.0f / (2.0f * static_cast (M_PI))) + { + for (int i = 0; i < 4; ++i) + { + hist_f_[i].setZero (nr_bins_f_[i]); + } + search_radius_ = 0; + k_ = 0; + feature_name_ = "VFHEstimation"; + } + + /** \brief Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular + * (f1, f2, f3) and distance (f4) features for a given point from its neighborhood + * \param[in] centroid_p the centroid point + * \param[in] centroid_n the centroid normal + * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points + * \param[in] normals the dataset containing the surface normals at each point in \a cloud + * \param[in] indices the k-neighborhood point indices in the dataset + */ + void + computePointSPFHSignature (const Eigen::Vector4f ¢roid_p, const Eigen::Vector4f ¢roid_n, + const pcl::PointCloud &cloud, const pcl::PointCloud &normals, + const std::vector &indices); + + /** \brief Set the viewpoint. + * \param[in] vpx the X coordinate of the viewpoint + * \param[in] vpy the Y coordinate of the viewpoint + * \param[in] vpz the Z coordinate of the viewpoint + */ + inline void + setViewPoint (float vpx, float vpy, float vpz) + { + vpx_ = vpx; + vpy_ = vpy; + vpz_ = vpz; + } + + /** \brief Get the viewpoint. */ + inline void + getViewPoint (float &vpx, float &vpy, float &vpz) + { + vpx = vpx_; + vpy = vpy_; + vpz = vpz_; + } + + /** \brief Set use_given_normal_ + * \param[in] use Set to true if you want to use the normal passed to setNormalUse(normal) + */ + inline void + setUseGivenNormal (bool use) + { + use_given_normal_ = use; + } + + /** \brief Set the normal to use + * \param[in] normal Sets the normal to be used in the VFH computation. It is is used + * to build the Darboux Coordinate system. + */ + inline void + setNormalToUse (const Eigen::Vector3f &normal) + { + normal_to_use_ = Eigen::Vector4f (normal[0], normal[1], normal[2], 0); + } + + /** \brief Set use_given_centroid_ + * \param[in] use Set to true if you want to use the centroid passed through setCentroidToUse(centroid) + */ + inline void + setUseGivenCentroid (bool use) + { + use_given_centroid_ = use; + } + + /** \brief Set centroid_to_use_ + * \param[in] centroid Centroid to be used in the VFH computation. It is used to compute the distances + * from all points to this centroid. + */ + inline void + setCentroidToUse (const Eigen::Vector3f ¢roid) + { + centroid_to_use_ = Eigen::Vector4f (centroid[0], centroid[1], centroid[2], 0); + } + + /** \brief set normalize_bins_ + * \param[in] normalize If true, the VFH bins are normalized using the total number of points + */ + inline void + setNormalizeBins (bool normalize) + { + normalize_bins_ = normalize; + } + + /** \brief set normalize_distances_ + * \param[in] normalize If true, the 4th component of VFH (shape distribution component) get normalized + * by the maximum size between the centroid and the point cloud + */ + inline void + setNormalizeDistance (bool normalize) + { + normalize_distances_ = normalize; + } + + /** \brief set size_component_ + * \param[in] fill_size True if the 4th component of VFH (shape distribution component) needs to be filled. + * Otherwise, it is set to zero. + */ + inline void + setFillSizeComponent (bool fill_size) + { + size_component_ = fill_size; + } + + /** \brief Overloaded computed method from pcl::Feature. + * \param[out] output the resultant point cloud model dataset containing the estimated features + */ + void + compute (PointCloudOut &output); + + private: + + /** \brief The number of subdivisions for each feature interval. */ + std::array nr_bins_f_; + int nr_bins_vp_; + + /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit + * from VFHEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. + */ + float vpx_, vpy_, vpz_; + + /** \brief Estimate the Viewpoint Feature Histograms (VFH) descriptors at a set of points given by + * using the surface in setSearchSurface () and the spatial locator in + * setSearchMethod () + * \param[out] output the resultant point cloud model dataset that contains the VFH feature estimates + */ + void + computeFeature (PointCloudOut &output) override; + + protected: + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief Placeholder for the f1 histogram. */ + std::array hist_f_; + /** \brief Placeholder for the vp histogram. */ + Eigen::VectorXf hist_vp_; + + /** \brief Normal to be used to computed VFH. Default, the average normal of the whole point cloud */ + Eigen::Vector4f normal_to_use_; + /** \brief Centroid to be used to computed VFH. Default, the centroid of the whole point cloud */ + Eigen::Vector4f centroid_to_use_; + + // VFH configuration parameters because CVFH instantiates it. See constructor for default values. + + /** \brief Use the normal_to_use_ */ + bool use_given_normal_; + /** \brief Use the centroid_to_use_ */ + bool use_given_centroid_; + /** \brief Normalize bins by the number the total number of points. */ + bool normalize_bins_; + /** \brief Normalize the shape distribution component of VFH */ + bool normalize_distances_; + /** \brief Activate or deactivate the size component of VFH */ + bool size_component_; + + private: + /** \brief Float constant = 1.0 / (2.0 * M_PI) */ + float d_pi_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/approximate_voxel_grid.h b/deps_install/include/pcl-1.10/pcl/filters/approximate_voxel_grid.h new file mode 100755 index 0000000..1d9b4c8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/approximate_voxel_grid.h @@ -0,0 +1,247 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: voxel_grid.h 1374 2011-06-19 02:29:56Z bouffa $ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */ + template + struct xNdCopyEigenPointFunctor + { + using Pod = typename traits::POD::type; + + xNdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointT &p2) + : p1_ (p1), + p2_ (reinterpret_cast(p2)), + f_idx_ (0) { } + + template inline void operator() () + { + //boost::fusion::at_key (p2_) = p1_[f_idx_++]; + using T = typename pcl::traits::datatype::type; + std::uint8_t* data_ptr = reinterpret_cast(&p2_) + pcl::traits::offset::value; + *reinterpret_cast(data_ptr) = static_cast (p1_[f_idx_++]); + } + + private: + const Eigen::VectorXf &p1_; + Pod &p2_; + int f_idx_; + }; + + /** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */ + template + struct xNdCopyPointEigenFunctor + { + using Pod = typename traits::POD::type; + + xNdCopyPointEigenFunctor (const PointT &p1, Eigen::VectorXf &p2) + : p1_ (reinterpret_cast(p1)), p2_ (p2), f_idx_ (0) { } + + template inline void operator() () + { + //p2_[f_idx_++] = boost::fusion::at_key (p1_); + using T = typename pcl::traits::datatype::type; + const std::uint8_t* data_ptr = reinterpret_cast(&p1_) + pcl::traits::offset::value; + p2_[f_idx_++] = static_cast (*reinterpret_cast(data_ptr)); + } + + private: + const Pod &p1_; + Eigen::VectorXf &p2_; + int f_idx_; + }; + + /** \brief ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. + * + * \author James Bowman, Radu B. Rusu + * \ingroup filters + */ + template + class ApproximateVoxelGrid: public Filter + { + using Filter::filter_name_; + using Filter::getClassName; + using Filter::input_; + using Filter::indices_; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + private: + struct he + { + he () : ix (), iy (), iz (), count (0) {} + int ix, iy, iz; + int count; + Eigen::VectorXf centroid; + }; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Empty constructor. */ + ApproximateVoxelGrid () : + pcl::Filter (), + leaf_size_ (Eigen::Vector3f::Ones ()), + inverse_leaf_size_ (Eigen::Array3f::Ones ()), + downsample_all_data_ (true), histsize_ (512), + history_ (new he[histsize_]) + { + filter_name_ = "ApproximateVoxelGrid"; + } + + /** \brief Copy constructor. + * \param[in] src the approximate voxel grid to copy into this. + */ + ApproximateVoxelGrid (const ApproximateVoxelGrid &src) : + pcl::Filter (), + leaf_size_ (src.leaf_size_), + inverse_leaf_size_ (src.inverse_leaf_size_), + downsample_all_data_ (src.downsample_all_data_), + histsize_ (src.histsize_), + history_ () + { + history_ = new he[histsize_]; + for (std::size_t i = 0; i < histsize_; i++) + history_[i] = src.history_[i]; + } + + + /** \brief Destructor. + */ + ~ApproximateVoxelGrid () + { + delete [] history_; + } + + + /** \brief Copy operator. + * \param[in] src the approximate voxel grid to copy into this. + */ + inline ApproximateVoxelGrid& + operator = (const ApproximateVoxelGrid &src) + { + leaf_size_ = src.leaf_size_; + inverse_leaf_size_ = src.inverse_leaf_size_; + downsample_all_data_ = src.downsample_all_data_; + histsize_ = src.histsize_; + history_ = new he[histsize_]; + for (std::size_t i = 0; i < histsize_; i++) + history_[i] = src.history_[i]; + return (*this); + } + + /** \brief Set the voxel grid leaf size. + * \param[in] leaf_size the voxel grid leaf size + */ + inline void + setLeafSize (const Eigen::Vector3f &leaf_size) + { + leaf_size_ = leaf_size; + inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array (); + } + + /** \brief Set the voxel grid leaf size. + * \param[in] lx the leaf size for X + * \param[in] ly the leaf size for Y + * \param[in] lz the leaf size for Z + */ + inline void + setLeafSize (float lx, float ly, float lz) + { + setLeafSize (Eigen::Vector3f (lx, ly, lz)); + } + + /** \brief Get the voxel grid leaf size. */ + inline Eigen::Vector3f + getLeafSize () const { return (leaf_size_); } + + /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. + * \param downsample the new value (true/false) + */ + inline void + setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } + + /** \brief Get the state of the internal downsampling parameter (true if + * all fields need to be downsampled, false if just XYZ). + */ + inline bool + getDownsampleAllData () const { return (downsample_all_data_); } + + protected: + /** \brief The size of a leaf. */ + Eigen::Vector3f leaf_size_; + + /** \brief Compute 1/leaf_size_ to avoid division later */ + Eigen::Array3f inverse_leaf_size_; + + /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */ + bool downsample_all_data_; + + /** \brief history buffer size, power of 2 */ + std::size_t histsize_; + + /** \brief history buffer */ + struct he* history_; + + using FieldList = typename pcl::traits::fieldList::type; + + /** \brief Downsample a Point Cloud using a voxelized grid approach + * \param output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Write a single point from the hash to the output cloud + */ + void + flush (PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/bilateral.h b/deps_install/include/pcl-1.10/pcl/filters/bilateral.h new file mode 100755 index 0000000..ff9956a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/bilateral.h @@ -0,0 +1,147 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief A bilateral filter implementation for point cloud data. Uses the intensity data channel. + * \note For more information please see + * C. Tomasi and R. Manduchi. Bilateral Filtering for Gray and Color Images. + * In Proceedings of the IEEE International Conference on Computer Vision, + * 1998. + * \author Luca Penasa + * \ingroup filters + */ + template + class BilateralFilter : public Filter + { + using Filter::input_; + using Filter::indices_; + using PointCloud = typename Filter::PointCloud; + using KdTreePtr = typename pcl::search::Search::Ptr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Constructor. + * Sets sigma_s_ to 0 and sigma_r_ to MAXDBL + */ + BilateralFilter () : sigma_s_ (0), + sigma_r_ (std::numeric_limits::max ()), + tree_ () + { + } + + + /** \brief Filter the input data and store the results into output + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Compute the intensity average for a single point + * \param[in] pid the point index to compute the weight for + * \param[in] indices the set of nearest neighor indices + * \param[in] distances the set of nearest neighbor distances + * \return the intensity average at a given point index + */ + double + computePointWeight (const int pid, const std::vector &indices, const std::vector &distances); + + /** \brief Set the half size of the Gaussian bilateral filter window. + * \param[in] sigma_s the half size of the Gaussian bilateral filter window to use + */ + inline void + setHalfSize (const double sigma_s) + { sigma_s_ = sigma_s; } + + /** \brief Get the half size of the Gaussian bilateral filter window as set by the user. */ + inline double + getHalfSize () const + { return (sigma_s_); } + + /** \brief Set the standard deviation parameter + * \param[in] sigma_r the new standard deviation parameter + */ + inline void + setStdDev (const double sigma_r) + { sigma_r_ = sigma_r;} + + /** \brief Get the value of the current standard deviation parameter of the bilateral filter. */ + inline double + getStdDev () const + { return (sigma_r_); } + + /** \brief Provide a pointer to the search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) + { tree_ = tree; } + + private: + + /** \brief The bilateral filter Gaussian distance kernel. + * \param[in] x the spatial distance (distance or intensity) + * \param[in] sigma standard deviation + */ + inline double + kernel (double x, double sigma) + { return (std::exp (- (x*x)/(2*sigma*sigma))); } + + /** \brief The half size of the Gaussian bilateral filter window (e.g., spatial extents in Euclidean). */ + double sigma_s_; + /** \brief The standard deviation of the bilateral filter (e.g., standard deviation in intensity). */ + double sigma_r_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/boost.h b/deps_install/include/pcl-1.10/pcl/filters/boost.h new file mode 100755 index 0000000..1d4c475 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/boost.h @@ -0,0 +1,55 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: io.h 5850 2012-06-06 14:04:59Z stfox88 $ + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +// Marking all Boost headers as system headers to remove warnings +#include +#include +#include +#include +#include +#include +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/filters/box_clipper3D.h b/deps_install/include/pcl-1.10/pcl/filters/box_clipper3D.h new file mode 100755 index 0000000..0e68a41 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/box_clipper3D.h @@ -0,0 +1,126 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include "clipper3D.h" + +namespace pcl +{ + /** + * \author Suat Gedikli + * \brief Implementation of a box clipper in 3D. Actually it allows affine transformations, thus any parallelepiped in general pose. + * The affine transformation is used to transform the point before clipping it using the unit cube centered at origin and with an extend of -1 to +1 in each dimension + * \ingroup filters + */ + template + class BoxClipper3D : public Clipper3D + { + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** + * \author Suat Gedikli + * \brief Constructor taking an affine transformation matrix, which allows also shearing of the clipping area + * \param[in] transformation the 3x3 affine transformation matrix that is used to describe the unit cube + */ + BoxClipper3D (const Eigen::Affine3f& transformation); + + /** + * \brief creates a BoxClipper object with a scaled box in general pose + * \param[in] rodrigues the rotation axis and angle given by the vector direction and length respectively + * \param[in] translation the position of the box center + * \param[in] box_size the size of the box for each dimension + */ + BoxClipper3D (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size); + + /** + * \brief Set the affine transformation + * \param[in] transformation + */ + void setTransformation (const Eigen::Affine3f& transformation); + + /** + * \brief sets the box in general pose given by the orientation position and size + * \param[in] rodrigues the rotation axis and angle given by the vector direction and length respectively + * \param[in] translation the position of the box center + * \param[in] box_size the size of the box for each dimension + */ + void setTransformation (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size); + + /** + * \brief virtual destructor + */ + ~BoxClipper3D () noexcept; + + bool + clipPoint3D (const PointT& point) const override; + + bool + clipLineSegment3D (PointT& from, PointT& to) const override; + + void + clipPlanarPolygon3D (std::vector >& polygon) const override; + + void + clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const override; + + void + clipPointCloud3D (const pcl::PointCloud &cloud_in, std::vector& clipped, const std::vector& indices = std::vector ()) const override; + + Clipper3D* + clone () const override; + + protected: + float getDistance (const PointT& point) const; + void transformPoint (const PointT& pointIn, PointT& pointOut) const; + private: + /** + * \brief the affine transformation that is applied before clipping is done on the unit cube. + */ + Eigen::Affine3f transformation_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/filters/clipper3D.h b/deps_install/include/pcl-1.10/pcl/filters/clipper3D.h new file mode 100755 index 0000000..0196ca0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/clipper3D.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** + * \brief Base class for 3D clipper objects + * \author Suat Gedikli + * \ingroup filters + */ + template + class Clipper3D + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** + * \brief virtual destructor. Never throws an exception. + */ + virtual ~Clipper3D () noexcept {} + + /** + * \brief interface to clip a single point + * \param[in] point the point to check against + * \return true, it point still exists, false if its clipped + */ + virtual bool + clipPoint3D (const PointT& point) const = 0; + + /** + * \brief interface to clip a line segment given by two end points. The order of the end points is unimportant and will sty the same after clipping. + * This means basically, that the direction of the line will not flip after clipping. + * \param[in,out] pt1 start point of the line + * \param[in,out] pt2 end point of the line + * \return true if the clipped line is not empty, thus the parameters are still valid, false if line completely outside clipping space + */ + virtual bool + clipLineSegment3D (PointT& pt1, PointT& pt2) const = 0; + + /** + * \brief interface to clip a planar polygon given by an ordered list of points + * \param[in,out] polygon the polygon in any direction (ccw or cw) but ordered, thus two neighboring points define an edge of the polygon + */ + virtual void + clipPlanarPolygon3D (std::vector >& polygon) const = 0; + + /** + * \brief interface to clip a planar polygon given by an ordered list of points + * \param[in] polygon the polygon in any direction (ccw or cw) but ordered, thus two neighboring points define an edge of the polygon + * \param[out] clipped_polygon the clipped polygon + */ + virtual void + clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const = 0; + + /** + * \brief interface to clip a point cloud + * \param[in] cloud_in input point cloud + * \param[out] clipped indices of points that remain after clipping the input cloud + * \param[in] indices the indices of points in the point cloud to be clipped. + * \return list of indices of remaining points after clipping. + */ + virtual void + clipPointCloud3D (const pcl::PointCloud &cloud_in, std::vector& clipped, const std::vector& indices = std::vector ()) const = 0; + + /** + * \brief polymorphic method to clone the underlying clipper with its parameters. + * \return the new clipper object from the specific subclass with all its parameters. + */ + virtual Clipper3D* + clone () const = 0; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/filters/conditional_removal.h b/deps_install/include/pcl-1.10/pcl/filters/conditional_removal.h new file mode 100755 index 0000000..7fba1e9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/conditional_removal.h @@ -0,0 +1,688 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + ////////////////////////////////////////////////////////////////////////////////////////// + namespace ComparisonOps + { + /** \brief The kind of comparison operations that are possible within a + * comparison object + */ + enum CompareOp + { + GT, GE, LT, LE, EQ + }; + } + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief A datatype that enables type-correct comparisons. */ + template + class PointDataAtOffset + { + public: + /** \brief Constructor. */ + PointDataAtOffset (std::uint8_t datatype, std::uint32_t offset) : + datatype_ (datatype), offset_ (offset) + { + } + + /** \brief Compare function. + * \param p the point to compare + * \param val the value to compare the point to + */ + int + compare (const PointT& p, const double& val); + protected: + /** \brief The type of data. */ + std::uint8_t datatype_; + + /** \brief The data offset. */ + std::uint32_t offset_; + private: + PointDataAtOffset () : datatype_ (), offset_ () {} + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief The (abstract) base class for the comparison object. */ + template + class ComparisonBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + ComparisonBase () : capable_ (false), offset_ (), op_ () {} + + /** \brief Destructor. */ + virtual ~ComparisonBase () {} + + /** \brief Return if the comparison is capable. */ + inline bool + isCapable () const + { + return (capable_); + } + + /** \brief Evaluate function. */ + virtual bool + evaluate (const PointT &point) const = 0; + + protected: + /** \brief True if capable. */ + bool capable_; + + /** \brief Field name to compare data on. */ + std::string field_name_; + + /** \brief The data offset. */ + std::uint32_t offset_; + + /** \brief The comparison operator type. */ + ComparisonOps::CompareOp op_; + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief The field-based specialization of the comparison object. */ + template + class FieldComparison : public ComparisonBase + { + using ComparisonBase::field_name_; + using ComparisonBase::op_; + using ComparisonBase::capable_; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Construct a FieldComparison + * \param field_name the name of the field that contains the data we want to compare + * \param op the operator to use when making the comparison + * \param compare_val the constant value to compare the field value too + */ + FieldComparison (const std::string &field_name, ComparisonOps::CompareOp op, double compare_val); + + /** \brief Copy constructor. + * \param[in] src the field comparison object to copy into this + */ + FieldComparison (const FieldComparison &src) + : ComparisonBase () + , compare_val_ (src.compare_val_), point_data_ (src.point_data_) + { + } + + /** \brief Copy operator. + * \param[in] src the field comparison object to copy into this + */ + inline FieldComparison& + operator = (const FieldComparison &src) + { + compare_val_ = src.compare_val_; + point_data_ = src.point_data_; + return (*this); + } + + /** \brief Destructor. */ + ~FieldComparison (); + + /** \brief Determine the result of this comparison. + * \param point the point to evaluate + * \return the result of this comparison. + */ + bool + evaluate (const PointT &point) const override; + + protected: + /** \brief All types (that we care about) can be represented as a double. */ + double compare_val_; + + /** \brief The point data to compare. */ + PointDataAtOffset* point_data_; + + private: + FieldComparison () : + compare_val_ (), point_data_ () + { + } // not allowed + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief A packed rgb specialization of the comparison object. */ + template + class PackedRGBComparison : public ComparisonBase + { + using ComparisonBase::capable_; + using ComparisonBase::op_; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Construct a PackedRGBComparison + * \param component_name either "r", "g" or "b" + * \param op the operator to use when making the comparison + * \param compare_val the constant value to compare the component value too + */ + PackedRGBComparison (const std::string &component_name, ComparisonOps::CompareOp op, double compare_val); + + /** \brief Destructor. */ + ~PackedRGBComparison () {} + + /** \brief Determine the result of this comparison. + * \param point the point to evaluate + * \return the result of this comparison. + */ + bool + evaluate (const PointT &point) const override; + + protected: + /** \brief The name of the component. */ + std::string component_name_; + + /** \brief The offset of the component */ + std::uint32_t component_offset_; + + /** \brief All types (that we care about) can be represented as a double. */ + double compare_val_; + + private: + PackedRGBComparison () : + component_offset_ (), compare_val_ () + { + } // not allowed + + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief A packed HSI specialization of the comparison object. */ + template + class PackedHSIComparison : public ComparisonBase + { + using ComparisonBase::capable_; + using ComparisonBase::op_; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Construct a PackedHSIComparison + * \param component_name either "h", "s" or "i" + * \param op the operator to use when making the comparison + * \param compare_val the constant value to compare the component value too + */ + PackedHSIComparison (const std::string &component_name, ComparisonOps::CompareOp op, double compare_val); + + /** \brief Destructor. */ + ~PackedHSIComparison () {} + + /** \brief Determine the result of this comparison. + * \param point the point to evaluate + * \return the result of this comparison. + */ + bool + evaluate (const PointT &point) const override; + + enum ComponentId + { + H, // -128 to 127 corresponds to -pi to pi + S, // 0 to 255 + I // 0 to 255 + }; + + protected: + /** \brief The name of the component. */ + std::string component_name_; + + /** \brief The ID of the component. */ + ComponentId component_id_; + + /** \brief All types (that we care about) can be represented as a double. */ + double compare_val_; + + /** \brief The offset of the component */ + std::uint32_t rgb_offset_; + + private: + PackedHSIComparison () : + component_id_ (), compare_val_ (), rgb_offset_ () + { + } // not allowed + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /**\brief A comparison whether the (x,y,z) components of a given point satisfy (p'Ap + 2v'p + c [OP] 0). + * Here [OP] stands for the defined pcl::ComparisonOps, i.e. for GT, GE, LT, LE or EQ; + * p = (x,y,z) is a point of the point cloud; A is 3x3 matrix; v is the 3x1 vector; c is a scalar. + * + * One can also use TfQuadraticXYZComparison for simpler geometric shapes by defining the + * quadratic parts (i.e. the matrix A) to be zero. By combining different instances of + * TfQuadraticXYZComparison one can get more complex shapes. For example, to have a simple + * cylinder (along the x-axis) of specific length one needs three comparisons combined as AND condition: + * 1. The cylinder: A = [0 0 0, 0 1 0, 0 0 1]; v = [0, 0, 0]; c = radius²; OP = LT (meaning "<") + * 2. X-min limit: A = 0; v = [1, 0, 0]; c = x_min; OP = GT + * 3. X-max ... + * + * \author Julian Löchner + */ + template + class TfQuadraticXYZComparison : public pcl::ComparisonBase + { + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW // needed whenever there is a fixed size Eigen:: vector or matrix in a class + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + */ + TfQuadraticXYZComparison (); + + /** \brief Empty destructor */ + ~TfQuadraticXYZComparison () {} + + /** \brief Constructor. + * \param op the operator "[OP]" of the comparison "p'Ap + 2v'p + c [OP] 0". + * \param comparison_matrix the matrix "A" of the comparison "p'Ap + 2v'p + c [OP] 0". + * \param comparison_vector the vector "v" of the comparison "p'Ap + 2v'p + c [OP] 0". + * \param comparison_scalar the scalar "c" of the comparison "p'Ap + 2v'p + c [OP] 0". + * \param comparison_transform the transformation of the comparison. + */ + TfQuadraticXYZComparison (const pcl::ComparisonOps::CompareOp op, const Eigen::Matrix3f &comparison_matrix, + const Eigen::Vector3f &comparison_vector, const float &comparison_scalar, + const Eigen::Affine3f &comparison_transform = Eigen::Affine3f::Identity ()); + + /** \brief set the operator "[OP]" of the comparison "p'Ap + 2v'p + c [OP] 0". + */ + inline void + setComparisonOperator (const pcl::ComparisonOps::CompareOp op) + { + op_ = op; + } + + /** \brief set the matrix "A" of the comparison "p'Ap + 2v'p + c [OP] 0". + */ + inline void + setComparisonMatrix (const Eigen::Matrix3f &matrix) + { + //define comp_matr_ as an homogeneous matrix of the given matrix + comp_matr_.block<3, 3> (0, 0) = matrix; + comp_matr_.col (3) << 0, 0, 0, 1; + comp_matr_.block<1, 3> (3, 0) << 0, 0, 0; + tf_comp_matr_ = comp_matr_; + } + + /** \brief set the matrix "A" of the comparison "p'Ap + 2v'p + c [OP] 0". + */ + inline void + setComparisonMatrix (const Eigen::Matrix4f &homogeneousMatrix) + { + comp_matr_ = homogeneousMatrix; + tf_comp_matr_ = comp_matr_; + } + + /** \brief set the vector "v" of the comparison "p'Ap + 2v'p + c [OP] 0". + */ + inline void + setComparisonVector (const Eigen::Vector3f &vector) + { + comp_vect_ = vector.homogeneous (); + tf_comp_vect_ = comp_vect_; + } + + /** \brief set the vector "v" of the comparison "p'Ap + 2v'p + c [OP] 0". + */ + inline void + setComparisonVector (const Eigen::Vector4f &homogeneousVector) + { + comp_vect_ = homogeneousVector; + tf_comp_vect_ = comp_vect_; + } + + /** \brief set the scalar "c" of the comparison "p'Ap + 2v'p + c [OP] 0". + */ + inline void + setComparisonScalar (const float &scalar) + { + comp_scalar_ = scalar; + } + + /** \brief transform the coordinate system of the comparison. If you think of + * the transformation to be a translation and rotation of the comparison in the + * same coordinate system, you have to provide the inverse transformation. + * This function does not change the original definition of the comparison. Thus, + * each call of this function will assume the original definition of the comparison + * as starting point for the transformation. + * + * @param transform the transformation (rotation and translation) as an affine matrix. + */ + inline void + transformComparison (const Eigen::Matrix4f &transform) + { + tf_comp_matr_ = transform.transpose () * comp_matr_ * transform; + tf_comp_vect_ = comp_vect_.transpose () * transform; + } + + /** \brief transform the coordinate system of the comparison. If you think of + * the transformation to be a translation and rotation of the comparison in the + * same coordinate system, you have to provide the inverse transformation. + * This function does not change the original definition of the comparison. Thus, + * each call of this function will assume the original definition of the comparison + * as starting point for the transformation. + * + * @param transform the transformation (rotation and translation) as an affine matrix. + */ + inline void + transformComparison (const Eigen::Affine3f &transform) + { + transformComparison (transform.matrix ()); + } + + /** \brief Determine the result of this comparison. + * \param point the point to evaluate + * \return the result of this comparison. + */ + bool + evaluate (const PointT &point) const override; + + protected: + using pcl::ComparisonBase::capable_; + using pcl::ComparisonBase::op_; + + Eigen::Matrix4f comp_matr_; + Eigen::Vector4f comp_vect_; + + float comp_scalar_; + + private: + Eigen::Matrix4f tf_comp_matr_; + Eigen::Vector4f tf_comp_vect_; + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Base condition class. */ + template + class ConditionBase + { + public: + using ComparisonBase = pcl::ComparisonBase; + using ComparisonBasePtr = typename ComparisonBase::Ptr; + using ComparisonBaseConstPtr = typename ComparisonBase::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + ConditionBase () : capable_ (true), comparisons_ (), conditions_ () + { + } + + /** \brief Destructor. */ + virtual ~ConditionBase () = default; + + /** \brief Add a new comparison + * \param comparison the comparison operator to add + */ + void + addComparison (ComparisonBaseConstPtr comparison); + + /** \brief Add a nested condition to this condition. + * \param condition the nested condition to be added + */ + void + addCondition (Ptr condition); + + /** \brief Check if evaluation requirements are met. */ + inline bool + isCapable () const + { + return (capable_); + } + + /** \brief Determine if a point meets this condition. + * \return whether the point meets this condition. + */ + virtual bool + evaluate (const PointT &point) const = 0; + + protected: + /** \brief True if capable. */ + bool capable_; + + /** \brief The collection of all comparisons that need to be verified. */ + std::vector comparisons_; + + /** \brief The collection of all conditions that need to be verified. */ + std::vector conditions_; + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief AND condition. */ + template + class ConditionAnd : public ConditionBase + { + using ConditionBase::conditions_; + using ConditionBase::comparisons_; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + ConditionAnd () : + ConditionBase () + { + } + + /** \brief Determine if a point meets this condition. + * \return whether the point meets this condition. + * + * The ConditionAnd evaluates to true when ALL + * comparisons and nested conditions evaluate to true + */ + bool + evaluate (const PointT &point) const override; + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief OR condition. */ + template + class ConditionOr : public ConditionBase + { + using ConditionBase::conditions_; + using ConditionBase::comparisons_; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + ConditionOr () : + ConditionBase () + { + } + + /** \brief Determine if a point meets this condition. + * \return whether the point meets this condition. + * + * The ConditionOr evaluates to true when ANY + * comparisons or nested conditions evaluate to true + */ + bool + evaluate (const PointT &point) const override; + }; + + ////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ConditionalRemoval filters data that satisfies certain conditions. + * + * A ConditionalRemoval must be provided a condition. There are two types of + * conditions: ConditionAnd and ConditionOr. Conditions require one or more + * comparisons and/or other conditions. A comparison has a name, a + * comparison operator, and a value. + * + * An ConditionAnd will evaluate to true when ALL of its encapsulated + * comparisons and conditions are true. + * + * An ConditionOr will evaluate to true when ANY of its encapsulated + * comparisons and conditions are true. + * + * Depending on the derived type of the comparison, the name can correspond + * to a PointCloud field name, or a color component in rgb color space or + * hsi color space. + * + * Here is an example usage: + * \code + * // Build the condition + * pcl::ConditionAnd::Ptr range_cond (new pcl::ConditionAnd ()); + * range_cond->addComparison (pcl::FieldComparison::Ptr (new pcl::FieldComparison("z", pcl::ComparisonOps::LT, 2.0))); + * range_cond->addComparison (pcl::FieldComparison::Ptr (new pcl::FieldComparison("z", pcl::ComparisonOps::GT, 0.0))); + * // Build the filter + * pcl::ConditionalRemoval range_filt; + * range_filt.setCondition (range_cond); + * range_filt.setKeepOrganized (false); + * \endcode + * + * \author Louis LeGrand, Intel Labs Seattle + * \ingroup filters + */ + template + class ConditionalRemoval : public Filter + { + using Filter::input_; + using Filter::filter_name_; + using Filter::getClassName; + + using Filter::removed_indices_; + using Filter::extract_removed_indices_; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + using ConditionBase = pcl::ConditionBase; + using ConditionBasePtr = typename ConditionBase::Ptr; + using ConditionBaseConstPtr = typename ConditionBase::ConstPtr; + + /** \brief the default constructor. + * + * All ConditionalRemovals require a condition which can be set + * using the setCondition method + * \param extract_removed_indices extract filtered indices from indices vector + */ + ConditionalRemoval (int extract_removed_indices = false) : + Filter::Filter (extract_removed_indices), capable_ (false), keep_organized_ (false), condition_ (), + user_filter_value_ (std::numeric_limits::quiet_NaN ()) + { + filter_name_ = "ConditionalRemoval"; + } + + /** \brief Set whether the filtered points should be kept and set to the + * value given through \a setUserFilterValue (default: NaN), or removed + * from the PointCloud, thus potentially breaking its organized + * structure. By default, points are removed. + * + * \param val set to true whether the filtered points should be kept and + * set to a given user value (default: NaN) + */ + inline void + setKeepOrganized (bool val) + { + keep_organized_ = val; + } + + inline bool + getKeepOrganized () const + { + return (keep_organized_); + } + + /** \brief Provide a value that the filtered points should be set to + * instead of removing them. Used in conjunction with \a + * setKeepOrganized (). + * \param val the user given value that the filtered point dimensions should be set to + */ + inline void + setUserFilterValue (float val) + { + user_filter_value_ = val; + } + + /** \brief Set the condition that the filter will use. + * \param condition each point must satisfy this condition to avoid + * being removed by the filter + * + * All ConditionalRemovals require a condition + */ + void + setCondition (ConditionBasePtr condition); + + protected: + /** \brief Filter a Point Cloud. + * \param output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + + /** \brief True if capable. */ + bool capable_; + + /** \brief Keep the structure of the data organized, by setting the + * filtered points to the a user given value (NaN by default). + */ + bool keep_organized_; + + /** \brief The condition to use for filtering */ + ConditionBasePtr condition_; + + /** \brief User given value to be set to any filtered point. Casted to + * the correct field type. + */ + float user_filter_value_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/convolution.h b/deps_install/include/pcl-1.10/pcl/filters/convolution.h new file mode 100755 index 0000000..3658a3d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/convolution.h @@ -0,0 +1,236 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace filters + { + /** Convolution is a mathematical operation on two functions f and g, + * producing a third function that is typically viewed as a modified + * version of one of the original functions. + * see http://en.wikipedia.org/wiki/Convolution. + * + * The class provides rows, column and separate convolving operations + * of a point cloud. + * Columns and separate convolution is only allowed on organised + * point clouds. + * + * When convolving, computing the rows and cols elements at 1/2 kernel + * width distance from the borders is not defined. We allow for 3 + * policies: + * - Ignoring: elements at special locations are filled with zero + * (default behaviour) + * - Mirroring: the missing rows or columns are obtained through mirroring + * - Duplicating: the missing rows or columns are obtained through + * duplicating + * + * \author Nizar Sallem + * \ingroup filters + */ + + template + class Convolution + { + public: + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + using PointCloudOut = pcl::PointCloud; + using Ptr = shared_ptr< Convolution >; + using ConstPtr = shared_ptr< const Convolution >; + + + /// The borders policy available + enum BORDERS_POLICY + { + BORDERS_POLICY_IGNORE = -1, + BORDERS_POLICY_MIRROR = 0, + BORDERS_POLICY_DUPLICATE = 1 + }; + /// Constructor + Convolution (); + /// Empty destructor + ~Convolution () {} + /** \brief Provide a pointer to the input dataset + * \param cloud the const boost shared pointer to a PointCloud message + * \remark Will perform a deep copy + */ + inline void + setInputCloud (const PointCloudInConstPtr& cloud) { input_ = cloud; } + /** Set convolving kernel + * \param[in] kernel convolving element + */ + inline void + setKernel (const Eigen::ArrayXf& kernel) { kernel_ = kernel; } + /// Set the borders policy + void + setBordersPolicy (int policy) { borders_policy_ = policy; } + /// Get the borders policy + int + getBordersPolicy () { return (borders_policy_); } + /** \remark this is critical so please read it carefully. + * In 3D the next point in (u,v) coordinate can be really far so a distance + * threshold is used to keep us from ghost points. + * The value you set here is strongly related to the sensor. A good value for + * kinect data is 0.001. Default is std::numeric::infinity () + * \param[in] threshold maximum allowed distance between 2 juxtaposed points + */ + inline void + setDistanceThreshold (const float& threshold) { distance_threshold_ = threshold; } + /// \return the distance threshold + inline const float & + getDistanceThreshold () const { return (distance_threshold_); } + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + /** Convolve a float image rows by a given kernel. + * \param[out] output the convolved cloud + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + inline void + convolveRows (PointCloudOut& output); + /** Convolve a float image columns by a given kernel. + * \param[out] output the convolved image + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + inline void + convolveCols (PointCloudOut& output); + /** Convolve point cloud with an horizontal kernel along rows + * then vertical kernel along columns : convolve separately. + * \param[in] h_kernel kernel for convolving rows + * \param[in] v_kernel kernel for convolving columns + * \param[out] output the convolved cloud + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + inline void + convolve (const Eigen::ArrayXf& h_kernel, const Eigen::ArrayXf& v_kernel, PointCloudOut& output); + /** Convolve point cloud with same kernel along rows and columns separately. + * \param[out] output the convolved cloud + * \note if output doesn't fit in input i.e. output.rows () < input.rows () or + * output.cols () < input.cols () then output is resized to input sizes. + */ + inline void + convolve (PointCloudOut& output); + + protected: + /// \brief convolve rows and ignore borders + void + convolve_rows (PointCloudOut& output); + /// \brief convolve cols and ignore borders + void + convolve_cols (PointCloudOut& output); + /// \brief convolve rows and mirror borders + void + convolve_rows_mirror (PointCloudOut& output); + /// \brief convolve cols and mirror borders + void + convolve_cols_mirror (PointCloudOut& output); + /// \brief convolve rows and duplicate borders + void + convolve_rows_duplicate (PointCloudOut& output); + /// \brief convolve cols and duplicate borders + void + convolve_cols_duplicate (PointCloudOut& output); + /** init compute is an internal method called before computation + * \param[in] output + * \throw pcl::InitFailedException + */ + void + initCompute (PointCloudOut& output); + private: + /** \return the result of convolution of point at (\ai, \aj) + * \note no test on finity is performed + */ + inline PointOut + convolveOneRowDense (int i, int j); + /** \return the result of convolution of point at (\ai, \aj) + * \note no test on finity is performed + */ + inline PointOut + convolveOneColDense (int i, int j); + /** \return the result of convolution of point at (\ai, \aj) + * \note only finite points within \a distance_threshold_ are accounted + */ + inline PointOut + convolveOneRowNonDense (int i, int j); + /** \return the result of convolution of point at (\ai, \aj) + * \note only finite points within \a distance_threshold_ are accounted + */ + inline PointOut + convolveOneColNonDense (int i, int j); + + /// Border policy + int borders_policy_; + /// Threshold distance between adjacent points + float distance_threshold_; + /// Pointer to the input cloud + PointCloudInConstPtr input_; + /// convolution kernel + Eigen::ArrayXf kernel_; + /// half kernel size + int half_width_; + /// kernel size - 1 + int kernel_width_; + protected: + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + + void + makeInfinite (PointOut& p) + { + p.x = p.y = p.z = std::numeric_limits::quiet_NaN (); + } + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/filters/convolution_3d.h b/deps_install/include/pcl-1.10/pcl/filters/convolution_3d.h new file mode 100755 index 0000000..b8ba2a7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/convolution_3d.h @@ -0,0 +1,289 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace filters + { + /** \brief Class ConvolvingKernel base class for all convolving kernels + * \ingroup filters + */ + template + class ConvolvingKernel + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudInConstPtr = typename PointCloud::ConstPtr; + + /// \brief empty constructor + ConvolvingKernel () {} + + /// \brief empty destructor + virtual ~ConvolvingKernel () {} + + /** \brief Set input cloud + * \param[in] input source point cloud + */ + void + setInputCloud (const PointCloudInConstPtr& input) { input_ = input; } + + /** \brief Convolve point at the center of this local information + * \param[in] indices indices of the point in the source point cloud + * \param[in] distances euclidean distance squared from the query point + * \return the convolved point + */ + virtual PointOutT + operator() (const std::vector& indices, const std::vector& distances) = 0; + + /** \brief Must call this method before doing any computation + * \note make sure to override this with at least + * \code + * bool initCompute () + * { + * return (true); + * } + * \endcode + * in your kernel interface, else you are going nowhere! + */ + virtual bool + initCompute () { return false; } + + /** \brief Utility function that annihilates a point making it fail the \ref pcl::isFinite test + * \param p point to annihilate + */ + static void + makeInfinite (PointOutT& p) + { + p.x = p.y = p.z = std::numeric_limits::quiet_NaN (); + } + + protected: + /// source cloud + PointCloudInConstPtr input_; + }; + + /** \brief Gaussian kernel implementation interface + * Use this as implementation reference + * \ingroup filters + */ + template + class GaussianKernel : public ConvolvingKernel + { + public: + using ConvolvingKernel::initCompute; + using ConvolvingKernel::input_; + using ConvolvingKernel::operator (); + using ConvolvingKernel::makeInfinite; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** Default constructor */ + GaussianKernel () + : ConvolvingKernel () + , sigma_ (0) + , threshold_ (std::numeric_limits::infinity ()) + {} + + virtual ~GaussianKernel () {} + + /** Set the sigma parameter of the Gaussian + * \param[in] sigma + */ + inline void + setSigma (float sigma) { sigma_ = sigma; } + + /** Set the distance threshold relative to a sigma factor i.e. points such as + * ||pi - q|| > sigma_coefficient^2 * sigma^2 are not considered. + */ + inline void + setThresholdRelativeToSigma (float sigma_coefficient) + { + sigma_coefficient_.reset (sigma_coefficient); + } + + /** Set the distance threshold such as pi, ||pi - q|| > threshold are not considered. */ + inline void + setThreshold (float threshold) { threshold_ = threshold; } + + /** Must call this method before doing any computation */ + bool initCompute (); + + virtual PointOutT + operator() (const std::vector& indices, const std::vector& distances); + + protected: + float sigma_; + float sigma_sqr_; + float threshold_; + boost::optional sigma_coefficient_; + }; + + /** \brief Gaussian kernel implementation interface with RGB channel handling + * Use this as implementation reference + * \ingroup filters + */ + template + class GaussianKernelRGB : public GaussianKernel + { + public: + using GaussianKernel::initCompute; + using GaussianKernel::input_; + using GaussianKernel::operator (); + using GaussianKernel::makeInfinite; + using GaussianKernel::sigma_sqr_; + using GaussianKernel::threshold_; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** Default constructor */ + GaussianKernelRGB () + : GaussianKernel () + {} + + ~GaussianKernelRGB () {} + + PointOutT + operator() (const std::vector& indices, const std::vector& distances); + }; + + /** Convolution3D handles the non organized case where width and height are unknown or if you + * are only interested in convolving based on local neighborhood information. + * The convolving kernel MUST be a radial symmetric and implement \ref ConvolvingKernel + * interface. + */ + template + class Convolution3D : public pcl::PCLBase + { + public: + using PointCloudIn = pcl::PointCloud; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + using PointCloudOut = pcl::PointCloud; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::PCLBase::indices_; + using pcl::PCLBase::input_; + + /** \brief Constructor */ + Convolution3D (); + + /** \brief Empty destructor */ + ~Convolution3D () {} + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /** \brief Set convolving kernel + * \param[in] kernel convolving element + */ + inline void + setKernel (const KernelT& kernel) { kernel_ = kernel; } + + /** \brief Provide a pointer to the input dataset that we need to estimate features at every point for. + * \param cloud the const boost shared pointer to a PointCloud message + */ + inline void + setSearchSurface (const PointCloudInConstPtr &cloud) { surface_ = cloud; } + + /** \brief Get a pointer to the surface point cloud dataset. */ + inline PointCloudInConstPtr + getSearchSurface () { return (surface_); } + + /** \brief Provide a pointer to the search object. + * \param tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () { return (tree_); } + + /** \brief Set the sphere radius that is to be used for determining the nearest neighbors + * \param radius the sphere radius used as the maximum distance to consider a point a neighbor + */ + inline void + setRadiusSearch (double radius) { search_radius_ = radius; } + + /** \brief Get the sphere radius used for determining the neighbors. */ + inline double + getRadiusSearch () { return (search_radius_); } + + /** Convolve point cloud. + * \param[out] output the convolved cloud + */ + void + convolve (PointCloudOut& output); + + protected: + /** \brief initialize computation */ + bool initCompute (); + + /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ + PointCloudInConstPtr surface_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief number of threads */ + unsigned int threads_; + + /** \brief convlving kernel */ + KernelT kernel_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/filters/covariance_sampling.h b/deps_install/include/pcl-1.10/pcl/filters/covariance_sampling.h new file mode 100755 index 0000000..5bba6dd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/covariance_sampling.h @@ -0,0 +1,168 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Point Cloud sampling based on the 6D covariances. It selects the points such that the resulting cloud is + * as stable as possible for being registered (against a copy of itself) with ICP. The algorithm adds points to the + * resulting cloud incrementally, while trying to keep all the 6 eigenvalues of the covariance matrix as close to each + * other as possible. + * This class also comes with the \a computeConditionNumber method that returns a number which shows how stable a point + * cloud will be when used as input for ICP (the closer the value it is to 1.0, the better). + * + * Based on the following publication: + * * "Geometrically Stable Sampling for the ICP Algorithm" - N. Gelfand, L. Ikemoto, S. Rusinkiewicz, M. Levoy + * + * \author Alexandru E. Ichim, alex.e.ichim@gmail.com + */ + template + class CovarianceSampling : public FilterIndices + { + using FilterIndices::filter_name_; + using FilterIndices::getClassName; + using FilterIndices::indices_; + using FilterIndices::input_; + using FilterIndices::initCompute; + + using Cloud = typename FilterIndices::PointCloud; + using CloudPtr = typename Cloud::Ptr; + using CloudConstPtr = typename Cloud::ConstPtr; + using NormalsConstPtr = typename pcl::PointCloud::ConstPtr; + + public: + using Ptr = shared_ptr< CovarianceSampling >; + using ConstPtr = shared_ptr< const CovarianceSampling >; + + /** \brief Empty constructor. */ + CovarianceSampling () + { filter_name_ = "CovarianceSampling"; } + + /** \brief Set number of indices to be sampled. + * \param[in] samples the number of sample indices + */ + inline void + setNumberOfSamples (unsigned int samples) + { num_samples_ = samples; } + + /** \brief Get the value of the internal \a num_samples_ parameter. */ + inline unsigned int + getNumberOfSamples () const + { return (num_samples_); } + + /** \brief Set the normals computed on the input point cloud + * \param[in] normals the normals computed for the input cloud + */ + inline void + setNormals (const NormalsConstPtr &normals) + { input_normals_ = normals; } + + /** \brief Get the normals computed on the input point cloud */ + inline NormalsConstPtr + getNormals () const + { return (input_normals_); } + + + + /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the + * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0, + * the more stable the cloud is for ICP registration. + * \return the condition number + */ + double + computeConditionNumber (); + + /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the + * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0, + * the more stable the cloud is for ICP registration. + * \param[in] covariance_matrix user given covariance matrix. Assumed to be self adjoint/symmetric. + * \return the condition number + */ + static double + computeConditionNumber (const Eigen::Matrix &covariance_matrix); + + /** \brief Computes the covariance matrix of the input cloud. + * \param[out] covariance_matrix the computed covariance matrix. + * \return whether the computation succeeded or not + */ + bool + computeCovarianceMatrix (Eigen::Matrix &covariance_matrix); + + protected: + /** \brief Number of indices that will be returned. */ + unsigned int num_samples_; + + /** \brief The normals computed at each point in the input cloud */ + NormalsConstPtr input_normals_; + + std::vector > scaled_points_; + + bool + initCompute (); + + /** \brief Sample of point indices into a separate PointCloud + * \param[out] output the resultant point cloud + */ + void + applyFilter (Cloud &output) override; + + /** \brief Sample of point indices + * \param[out] indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + + static bool + sort_dot_list_function (std::pair a, + std::pair b) + { return (a.second > b.second); } + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/crop_box.h b/deps_install/include/pcl-1.10/pcl/filters/crop_box.h new file mode 100755 index 0000000..3ba04e4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/crop_box.h @@ -0,0 +1,345 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: crop_box.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief CropBox is a filter that allows the user to filter all the data + * inside of a given box. + * + * \author Justin Rosen + * \ingroup filters + */ + template + class CropBox : public FilterIndices + { + using Filter::getClassName; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + CropBox (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + min_pt_ (Eigen::Vector4f (-1, -1, -1, 1)), + max_pt_ (Eigen::Vector4f (1, 1, 1, 1)), + rotation_ (Eigen::Vector3f::Zero ()), + translation_ (Eigen::Vector3f::Zero ()), + transform_ (Eigen::Affine3f::Identity ()) + { + filter_name_ = "CropBox"; + } + + /** \brief Set the minimum point of the box + * \param[in] min_pt the minimum point of the box + */ + inline void + setMin (const Eigen::Vector4f &min_pt) + { + min_pt_ = min_pt; + } + + /** \brief Get the value of the minimum point of the box, as set by the user + * \return the value of the internal \a min_pt parameter. + */ + inline Eigen::Vector4f + getMin () const + { + return (min_pt_); + } + + /** \brief Set the maximum point of the box + * \param[in] max_pt the maximum point of the box + */ + inline void + setMax (const Eigen::Vector4f &max_pt) + { + max_pt_ = max_pt; + } + + /** \brief Get the value of the maximum point of the box, as set by the user + * \return the value of the internal \a max_pt parameter. + */ + inline Eigen::Vector4f + getMax () const + { + return (max_pt_); + } + + /** \brief Set a translation value for the box + * \param[in] translation the (tx,ty,tz) values that the box should be translated by + */ + inline void + setTranslation (const Eigen::Vector3f &translation) + { + translation_ = translation; + } + + /** \brief Get the value of the box translation parameter as set by the user. */ + Eigen::Vector3f + getTranslation () const + { + return (translation_); + } + + /** \brief Set a rotation value for the box + * \param[in] rotation the (rx,ry,rz) values that the box should be rotated by + */ + inline void + setRotation (const Eigen::Vector3f &rotation) + { + rotation_ = rotation; + } + + /** \brief Get the value of the box rotatation parameter, as set by the user. */ + inline Eigen::Vector3f + getRotation () const + { + return (rotation_); + } + + /** \brief Set a transformation that should be applied to the cloud before filtering + * \param[in] transform an affine transformation that needs to be applied to the cloud before filtering + */ + inline void + setTransform (const Eigen::Affine3f &transform) + { + transform_ = transform; + } + + /** \brief Get the value of the transformation parameter, as set by the user. */ + inline Eigen::Affine3f + getTransform () const + { + return (transform_); + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Sample of point indices into a separate PointCloud + * \param[out] output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Sample of point indices + * \param[out] indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + private: + /** \brief The minimum point of the box. */ + Eigen::Vector4f min_pt_; + /** \brief The maximum point of the box. */ + Eigen::Vector4f max_pt_; + /** \brief The 3D rotation for the box. */ + Eigen::Vector3f rotation_; + /** \brief The 3D translation for the box. */ + Eigen::Vector3f translation_; + /** \brief The affine transform applied to the cloud. */ + Eigen::Affine3f transform_; + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief CropBox is a filter that allows the user to filter all the data + * inside of a given box. + * + * \author Justin Rosen + * \ingroup filters + */ + template<> + class PCL_EXPORTS CropBox : public FilterIndices + { + using Filter::filter_name_; + using Filter::getClassName; + + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + public: + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + CropBox (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + min_pt_(Eigen::Vector4f (-1, -1, -1, 1)), + max_pt_(Eigen::Vector4f (1, 1, 1, 1)), + translation_ (Eigen::Vector3f::Zero ()), + rotation_ (Eigen::Vector3f::Zero ()), + transform_(Eigen::Affine3f::Identity ()) + { + filter_name_ = "CropBox"; + } + + /** \brief Set the minimum point of the box + * \param[in] min_pt the minimum point of the box + */ + inline void + setMin (const Eigen::Vector4f& min_pt) + { + min_pt_ = min_pt; + } + + /** \brief Get the value of the minimum point of the box, as set by the user + * \return the value of the internal \a min_pt parameter. + */ + inline Eigen::Vector4f + getMin () const + { + return (min_pt_); + } + + /** \brief Set the maximum point of the box + * \param[in] max_pt the maximum point of the box + */ + inline void + setMax (const Eigen::Vector4f &max_pt) + { + max_pt_ = max_pt; + } + + /** \brief Get the value of the maxiomum point of the box, as set by the user + * \return the value of the internal \a max_pt parameter. + */ + inline Eigen::Vector4f + getMax () const + { + return (max_pt_); + } + + /** \brief Set a translation value for the box + * \param[in] translation the (tx,ty,tz) values that the box should be translated by + */ + inline void + setTranslation (const Eigen::Vector3f &translation) + { + translation_ = translation; + } + + /** \brief Get the value of the box translation parameter as set by the user. */ + inline Eigen::Vector3f + getTranslation () const + { + return (translation_); + } + + /** \brief Set a rotation value for the box + * \param[in] rotation the (rx,ry,rz) values that the box should be rotated by + */ + inline void + setRotation (const Eigen::Vector3f &rotation) + { + rotation_ = rotation; + } + + /** \brief Get the value of the box rotatation parameter, as set by the user. */ + inline Eigen::Vector3f + getRotation () const + { + return (rotation_); + } + + /** \brief Set a transformation that should be applied to the cloud before filtering + * \param[in] transform an affine transformation that needs to be applied to the cloud before filtering + */ + inline void + setTransform (const Eigen::Affine3f &transform) + { + transform_ = transform; + } + + /** \brief Get the value of the transformation parameter, as set by the user. */ + inline Eigen::Affine3f + getTransform () const + { + return (transform_); + } + + protected: + /** \brief Sample of point indices into a separate PointCloud + * \param output the resultant point cloud + */ + void + applyFilter (PCLPointCloud2 &output) override; + + /** \brief Sample of point indices + * \param indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + + /** \brief The minimum point of the box. */ + Eigen::Vector4f min_pt_; + /** \brief The maximum point of the box. */ + Eigen::Vector4f max_pt_; + /** \brief The 3D translation for the box. */ + Eigen::Vector3f translation_; + /** \brief The 3D rotation for the box. */ + Eigen::Vector3f rotation_; + /** \brief The affine transform applied to the cloud. */ + Eigen::Affine3f transform_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/crop_hull.h b/deps_install/include/pcl-1.10/pcl/filters/crop_hull.h new file mode 100755 index 0000000..62bf259 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/crop_hull.h @@ -0,0 +1,239 @@ + /* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Filter points that lie inside or outside a 3D closed surface or 2D + * closed polygon, as generated by the ConvexHull or ConcaveHull classes. + * \author James Crosby + * \ingroup filters + */ + template + class CropHull: public FilterIndices + { + using Filter::filter_name_; + using Filter::indices_; + using Filter::input_; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty Constructor. */ + CropHull () : + hull_cloud_(), + dim_(3), + crop_outside_(true) + { + filter_name_ = "CropHull"; + } + + /** \brief Set the vertices of the hull used to filter points. + * \param[in] polygons Vector of polygons (Vertices structures) forming + * the hull used for filtering points. + */ + inline void + setHullIndices (const std::vector& polygons) + { + hull_polygons_ = polygons; + } + + /** \brief Get the vertices of the hull used to filter points. + */ + std::vector + getHullIndices () const + { + return (hull_polygons_); + } + + /** \brief Set the point cloud that the hull indices refer to + * \param[in] points the point cloud that the hull indices refer to + */ + inline void + setHullCloud (PointCloudPtr points) + { + hull_cloud_ = points; + } + + /** \brief Get the point cloud that the hull indices refer to. */ + PointCloudPtr + getHullCloud () const + { + return (hull_cloud_); + } + + /** \brief Set the dimensionality of the hull to be used. + * This should be set to correspond to the dimensionality of the + * convex/concave hull produced by the pcl::ConvexHull and + * pcl::ConcaveHull classes. + * \param[in] dim Dimensionailty of the hull used to filter points. + */ + inline void + setDim (int dim) + { + dim_ = dim; + } + + /** \brief Remove points outside the hull (default), or those inside the hull. + * \param[in] crop_outside If true, the filter will remove points + * outside the hull. If false, those inside will be removed. + */ + inline void + setCropOutside(bool crop_outside) + { + crop_outside_ = crop_outside; + } + + protected: + /** \brief Filter the input points using the 2D or 3D polygon hull. + * \param[out] output The set of points that passed the filter + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filter the input points using the 2D or 3D polygon hull. + * \param[out] indices the indices of the set of points that passed the filter. + */ + void + applyFilter (std::vector &indices) override; + + private: + /** \brief Return the size of the hull point cloud in line with coordinate axes. + * This is used to choose the 2D projection to use when cropping to a 2d + * polygon. + */ + Eigen::Vector3f + getHullCloudRange (); + + /** \brief Apply the two-dimensional hull filter. + * All points are assumed to lie in the same plane as the 2D hull, an + * axis-aligned 2D coordinate system using the two dimensions specified + * (PlaneDim1, PlaneDim2) is used for calculations. + * \param[out] output The set of points that pass the 2D polygon filter. + */ + template void + applyFilter2D (PointCloud &output); + + /** \brief Apply the two-dimensional hull filter. + * All points are assumed to lie in the same plane as the 2D hull, an + * axis-aligned 2D coordinate system using the two dimensions specified + * (PlaneDim1, PlaneDim2) is used for calculations. + * \param[out] indices The indices of the set of points that pass the + * 2D polygon filter. + */ + template void + applyFilter2D (std::vector &indices); + + /** \brief Apply the three-dimensional hull filter. + * Polygon-ray crossings are used for three rays cast from each point + * being tested, and a majority vote of the resulting + * polygon-crossings is used to decide whether the point lies inside + * or outside the hull. + * \param[out] output The set of points that pass the 3D polygon hull + * filter. + */ + void + applyFilter3D (PointCloud &output); + + /** \brief Apply the three-dimensional hull filter. + * Polygon-ray crossings are used for three rays cast from each point + * being tested, and a majority vote of the resulting + * polygon-crossings is used to decide whether the point lies inside + * or outside the hull. + * \param[out] indices The indices of the set of points that pass the 3D + * polygon hull filter. + */ + void + applyFilter3D (std::vector &indices); + + /** \brief Test an individual point against a 2D polygon. + * PlaneDim1 and PlaneDim2 specify the x/y/z coordinate axes to use. + * \param[in] point Point to test against the polygon. + * \param[in] verts Vertex indices of polygon. + * \param[in] cloud Cloud from which the vertex indices are drawn. + */ + template inline static bool + isPointIn2DPolyWithVertIndices (const PointT& point, + const Vertices& verts, + const PointCloud& cloud); + + /** \brief Does a ray cast from a point intersect with an arbitrary + * triangle in 3D? + * See: http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle() + * \param[in] point Point from which the ray is cast. + * \param[in] ray Vector in direction of ray. + * \param[in] verts Indices of vertices making the polygon. + * \param[in] cloud Cloud from which the vertex indices are drawn. + */ + inline static bool + rayTriangleIntersect (const PointT& point, + const Eigen::Vector3f& ray, + const Vertices& verts, + const PointCloud& cloud); + + + /** \brief The vertices of the hull used to filter points. */ + std::vector hull_polygons_; + + /** \brief The point cloud that the hull indices refer to. */ + PointCloudPtr hull_cloud_; + + /** \brief The dimensionality of the hull to be used. */ + int dim_; + + /** \brief If true, the filter will remove points outside the hull. If + * false, those inside will be removed. + */ + bool crop_outside_; + }; + +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/extract_indices.h b/deps_install/include/pcl-1.10/pcl/filters/extract_indices.h new file mode 100755 index 0000000..e046318 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/extract_indices.h @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief @b ExtractIndices extracts a set of indices from a point cloud. + * \details Usage example: + * \code + * pcl::ExtractIndices eifilter (true); // Initializing with true will allow us to extract the removed indices + * eifilter.setInputCloud (cloud_in); + * eifilter.setIndices (indices_in); + * eifilter.filter (*cloud_out); + * // The resulting cloud_out contains all points of cloud_in that are indexed by indices_in + * indices_rem = eifilter.getRemovedIndices (); + * // The indices_rem array indexes all points of cloud_in that are not indexed by indices_in + * eifilter.setNegative (true); + * eifilter.filter (*indices_out); + * // Alternatively: the indices_out array is identical to indices_rem + * eifilter.setNegative (false); + * eifilter.setUserFilterValue (1337.0); + * eifilter.filterDirectly (cloud_in); + * // This will directly modify cloud_in instead of creating a copy of the cloud + * // It will overwrite all fields of the filtered points by the user value: 1337 + * \endcode + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template + class ExtractIndices : public FilterIndices + { + protected: + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using FieldList = typename pcl::traits::fieldList::type; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + ExtractIndices (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices) + { + use_indices_ = true; + filter_name_ = "ExtractIndices"; + } + + /** \brief Apply the filter and store the results directly in the input cloud. + * \details This method will save the time and memory copy of an output cloud but can not alter the original size of the input cloud: + * It operates as though setKeepOrganized() is true and will overwrite the filtered points instead of remove them. + * All fields of filtered points are replaced with the value set by setUserFilterValue() (default = NaN). + * This method also automatically alters the input cloud set via setInputCloud(). + * It does not alter the value of the internal keep organized boolean as set by setKeepOrganized(). + * \param cloud The point cloud used for input and output. + */ + void + filterDirectly (PointCloudPtr &cloud); + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::use_indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) override + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ExtractIndices extracts a set of indices from a point cloud. + *
+ * Usage examples: + * \code + * pcl::ExtractIndices filter; + * filter.setInputCloud (cloud_in); + * filter.setIndices (indices_in); + * // Extract the points in cloud_in referenced by indices_in as a separate point cloud: + * filter.filter (*cloud_out); + * // Retrieve indices to all points in cloud_in except those referenced by indices_in: + * filter.setNegative (true); + * filter.filter (*indices_out); + * // The resulting cloud_out is identical to cloud_in, but all points referenced by indices_in are made NaN: + * filter.setNegative (true); + * filter.setKeepOrganized (true); + * filter.filter (*cloud_out); + * \endcode + * \note Does not inherently remove NaNs from results, hence the \a extract_removed_indices_ system is not used. + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template<> + class PCL_EXPORTS ExtractIndices : public FilterIndices + { + public: + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + /** \brief Empty constructor. */ + ExtractIndices () + { + use_indices_ = true; + filter_name_ = "ExtractIndices"; + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::use_indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + + /** \brief Extract point indices into a separate PointCloud + * \param[out] output the resultant point cloud + */ + void + applyFilter (PCLPointCloud2 &output) override; + + /** \brief Extract point indices + * \param indices the resultant indices + */ + void + applyFilter (std::vector &indices) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/fast_bilateral.h b/deps_install/include/pcl-1.10/pcl/filters/fast_bilateral.h new file mode 100755 index 0000000..607bc0b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/fast_bilateral.h @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2004, Sylvain Paris and Francois Sillion + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Implementation of a fast bilateral filter for smoothing depth information in organized point clouds + * Based on the following paper: + * * Sylvain Paris and Fredo Durand + * "A Fast Approximation of the Bilateral Filter using a Signal Processing Approach" + * European Conference on Computer Vision (ECCV'06) + * + * More details on the webpage: http://people.csail.mit.edu/sparis/bf/ + */ + template + class FastBilateralFilter : public Filter + { + protected: + using Filter::input_; + using PointCloud = typename Filter::PointCloud; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + FastBilateralFilter () + : sigma_s_ (15.0f) + , sigma_r_ (0.05f) + , early_division_ (false) + { } + + /** \brief Empty destructor */ + ~FastBilateralFilter () {} + + /** \brief Set the standard deviation of the Gaussian used by the bilateral filter for + * the spatial neighborhood/window. + * \param[in] sigma_s the size of the Gaussian bilateral filter window to use + */ + inline void + setSigmaS (float sigma_s) + { sigma_s_ = sigma_s; } + + /** \brief Get the size of the Gaussian bilateral filter window as set by the user. */ + inline float + getSigmaS () const + { return sigma_s_; } + + + /** \brief Set the standard deviation of the Gaussian used to control how much an adjacent + * pixel is downweighted because of the intensity difference (depth in our case). + * \param[in] sigma_r the standard deviation of the Gaussian for the intensity difference + */ + inline void + setSigmaR (float sigma_r) + { sigma_r_ = sigma_r; } + + /** \brief Get the standard deviation of the Gaussian for the intensity difference */ + inline float + getSigmaR () const + { return sigma_r_; } + + /** \brief Filter the input data and store the results into output. + * \param[out] output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + protected: + float sigma_s_; + float sigma_r_; + bool early_division_; + + class Array3D + { + public: + Array3D (const std::size_t width, const std::size_t height, const std::size_t depth) + { + x_dim_ = width; + y_dim_ = height; + z_dim_ = depth; + v_ = std::vector > (width*height*depth, Eigen::Vector2f (0.0f, 0.0f)); + } + + inline Eigen::Vector2f& + operator () (const std::size_t x, const std::size_t y, const std::size_t z) + { return v_[(x * y_dim_ + y) * z_dim_ + z]; } + + inline const Eigen::Vector2f& + operator () (const std::size_t x, const std::size_t y, const std::size_t z) const + { return v_[(x * y_dim_ + y) * z_dim_ + z]; } + + inline void + resize (const std::size_t width, const std::size_t height, const std::size_t depth) + { + x_dim_ = width; + y_dim_ = height; + z_dim_ = depth; + v_.resize (x_dim_ * y_dim_ * z_dim_); + } + + Eigen::Vector2f + trilinear_interpolation (const float x, + const float y, + const float z); + + static inline std::size_t + clamp (const std::size_t min_value, + const std::size_t max_value, + const std::size_t x); + + inline std::size_t + x_size () const + { return x_dim_; } + + inline std::size_t + y_size () const + { return y_dim_; } + + inline std::size_t + z_size () const + { return z_dim_; } + + inline std::vector >::iterator + begin () + { return v_.begin (); } + + inline std::vector >::iterator + end () + { return v_.end (); } + + inline std::vector >::const_iterator + begin () const + { return v_.begin (); } + + inline std::vector >::const_iterator + end () const + { return v_.end (); } + + private: + std::vector > v_; + std::size_t x_dim_, y_dim_, z_dim_; + }; + + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#else +#define PCL_INSTANTIATE_FastBilateralFilter(T) template class PCL_EXPORTS pcl::FastBilateralFilter; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/fast_bilateral_omp.h b/deps_install/include/pcl-1.10/pcl/filters/fast_bilateral_omp.h new file mode 100755 index 0000000..7725a99 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/fast_bilateral_omp.h @@ -0,0 +1,102 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2004, Sylvain Paris and Francois Sillion + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: fast_bilateral_omp.h 8379 2013-01-02 23:12:21Z sdmiller $ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Implementation of a fast bilateral filter for smoothing depth information in organized point clouds + * Based on the following paper: + * * Sylvain Paris and Fredo Durand + * "A Fast Approximation of the Bilateral Filter using a Signal Processing Approach" + * European Conference on Computer Vision (ECCV'06) + * + * More details on the webpage: http://people.csail.mit.edu/sparis/bf/ + */ + template + class FastBilateralFilterOMP : public FastBilateralFilter + { + protected: + using FastBilateralFilter::input_; + using FastBilateralFilter::sigma_s_; + using FastBilateralFilter::sigma_r_; + using FastBilateralFilter::early_division_; + using Array3D = typename FastBilateralFilter::Array3D; + + using PointCloud = typename Filter::PointCloud; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + FastBilateralFilterOMP (unsigned int nr_threads = 0) + { + setNumberOfThreads(nr_threads); + } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + /** \brief Filter the input data and store the results into output. + * \param[out] output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + protected: + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#else +#define PCL_INSTANTIATE_FastBilateralFilterOMP(T) template class PCL_EXPORTS pcl::FastBilateralFilterOMP; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/filter.h b/deps_install/include/pcl-1.10/pcl/filters/filter.h new file mode 100755 index 0000000..159573c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/filter.h @@ -0,0 +1,268 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Removes points with x, y, or z equal to NaN + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the output point cloud + * \param[out] index the mapping (ordered): cloud_out.points[i] = cloud_in.points[index[i]] + * \note The density of the point cloud is lost. + * \note Can be called with cloud_in == cloud_out + * \ingroup filters + */ + template void + removeNaNFromPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + std::vector &index); + + /** \brief Removes points that have their normals invalid (i.e., equal to NaN) + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the output point cloud + * \param[out] index the mapping (ordered): cloud_out.points[i] = cloud_in.points[index[i]] + * \note The density of the point cloud is lost. + * \note Can be called with cloud_in == cloud_out + * \ingroup filters + */ + template void + removeNaNNormalsFromPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + std::vector &index); + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Filter represents the base filter class. All filters must inherit from this interface. + * \author Radu B. Rusu + * \ingroup filters + */ + template + class Filter : public PCLBase + { + public: + using PCLBase::indices_; + using PCLBase::input_; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + /** \brief Empty constructor. + * \param[in] extract_removed_indices set to true if the filtered data indices should be saved in a + * separate list. Default: false. + */ + Filter (bool extract_removed_indices = false) : + removed_indices_ (new std::vector), + extract_removed_indices_ (extract_removed_indices) + { + } + + /** \brief Empty destructor */ + ~Filter () {} + + /** \brief Get the point indices being removed */ + inline IndicesConstPtr const + getRemovedIndices () const + { + return (removed_indices_); + } + + /** \brief Get the point indices being removed + * \param[out] pi the resultant point indices that have been removed + */ + inline void + getRemovedIndices (PointIndices &pi) + { + pi.indices = *removed_indices_; + } + + /** \brief Calls the filtering method and returns the filtered dataset in output. + * \param[out] output the resultant filtered point cloud dataset + */ + inline void + filter (PointCloud &output) + { + if (!initCompute ()) + return; + + if (input_.get () == &output) // cloud_in = cloud_out + { + PointCloud output_temp; + applyFilter (output_temp); + output_temp.header = input_->header; + output_temp.sensor_origin_ = input_->sensor_origin_; + output_temp.sensor_orientation_ = input_->sensor_orientation_; + pcl::copyPointCloud (output_temp, output); + } + else + { + output.header = input_->header; + output.sensor_origin_ = input_->sensor_origin_; + output.sensor_orientation_ = input_->sensor_orientation_; + applyFilter (output); + } + + deinitCompute (); + } + + protected: + + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + /** \brief Indices of the points that are removed */ + IndicesPtr removed_indices_; + + /** \brief The filter name. */ + std::string filter_name_; + + /** \brief Set to true if we want to return the indices of the removed points. */ + bool extract_removed_indices_; + + /** \brief Abstract filter method. + * + * The implementation needs to set output.{points, width, height, is_dense}. + * + * \param[out] output the resultant filtered point cloud + */ + virtual void + applyFilter (PointCloud &output) = 0; + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const + { + return (filter_name_); + } + }; + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Filter represents the base filter class. All filters must inherit from this interface. + * \author Radu B. Rusu + * \ingroup filters + */ + template<> + class PCL_EXPORTS Filter : public PCLBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + /** \brief Empty constructor. + * \param[in] extract_removed_indices set to true if the filtered data indices should be saved in a + * separate list. Default: false. + */ + Filter (bool extract_removed_indices = false) : + removed_indices_ (new std::vector), + extract_removed_indices_ (extract_removed_indices) + { + } + + /** \brief Empty destructor */ + ~Filter () {} + + /** \brief Get the point indices being removed */ + inline IndicesConstPtr const + getRemovedIndices () const + { + return (removed_indices_); + } + + /** \brief Get the point indices being removed + * \param[out] pi the resultant point indices that have been removed + */ + inline void + getRemovedIndices (PointIndices &pi) + { + pi.indices = *removed_indices_; + } + + /** \brief Calls the filtering method and returns the filtered dataset in output. + * \param[out] output the resultant filtered point cloud dataset + */ + void + filter (PCLPointCloud2 &output); + + protected: + + /** \brief Indices of the points that are removed */ + IndicesPtr removed_indices_; + + /** \brief Set to true if we want to return the indices of the removed points. */ + bool extract_removed_indices_; + + /** \brief The filter name. */ + std::string filter_name_; + + /** \brief Abstract filter method. + * + * The implementation needs to set output.{data, row_step, point_step, width, height, is_dense}. + * + * \param[out] output the resultant filtered point cloud + */ + virtual void + applyFilter (PCLPointCloud2 &output) = 0; + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const + { + return (filter_name_); + } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/filter_indices.h b/deps_install/include/pcl-1.10/pcl/filters/filter_indices.h new file mode 100755 index 0000000..bd87ae1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/filter_indices.h @@ -0,0 +1,309 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: filter_indices.h 4707 2012-02-23 10:34:17Z florentinus $ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Removes points with x, y, or z equal to NaN (dry run). + * + * This function only computes the mapping between the points in the input + * cloud and the cloud that would result from filtering. It does not + * actually construct and output the filtered cloud. + * + * \note This function does not modify the input point cloud! + * + * \param cloud_in the input point cloud + * \param index the mapping (ordered): filtered_cloud.points[i] = cloud_in.points[index[i]] + * + * \see removeNaNFromPointCloud + * \ingroup filters + */ + template void + removeNaNFromPointCloud (const pcl::PointCloud &cloud_in, std::vector &index); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b FilterIndices represents the base class for filters that are about binary point removal. + *
+ * All derived classes have to implement the \a filter (PointCloud &output) and the \a filter (std::vector &indices) methods. + * Ideally they also make use of the \a negative_, \a keep_organized_ and \a extract_removed_indices_ systems. + * The distinguishment between the \a negative_ and \a extract_removed_indices_ systems only makes sense if the class automatically + * filters non-finite entries in the filtering methods (recommended). + * \author Justin Rosen + * \ingroup filters + */ + template + class FilterIndices : public Filter + { + public: + using Filter::extract_removed_indices_; + using PointCloud = pcl::PointCloud; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + FilterIndices (bool extract_removed_indices = false) : + negative_ (false), + keep_organized_ (false), + user_filter_value_ (std::numeric_limits::quiet_NaN ()) + { + extract_removed_indices_ = extract_removed_indices; + } + + /** \brief Empty virtual destructor. */ + + ~FilterIndices () + { + } + + inline void + filter (PointCloud &output) + { + pcl::Filter::filter (output); + } + + /** \brief Calls the filtering method and returns the filtered point cloud indices. + * \param[out] indices the resultant filtered point cloud indices + */ + inline void + filter (std::vector &indices) + { + if (!initCompute ()) + return; + + // Apply the actual filter + applyFilter (indices); + + deinitCompute (); + } + + /** \brief Set whether the regular conditions for points filtering should apply, or the inverted conditions. + * \param[in] negative false = normal filter behavior (default), true = inverted behavior. + */ + inline void + setNegative (bool negative) + { + negative_ = negative; + } + + /** \brief Get whether the regular conditions for points filtering should apply, or the inverted conditions. + * \return The value of the internal \a negative_ parameter; false = normal filter behavior (default), true = inverted behavior. + */ + inline bool + getNegative () const + { + return (negative_); + } + + /** \brief Set whether the filtered points should be kept and set to the value given through \a setUserFilterValue (default: NaN), + * or removed from the PointCloud, thus potentially breaking its organized structure. + * \param[in] keep_organized false = remove points (default), true = redefine points, keep structure. + */ + inline void + setKeepOrganized (bool keep_organized) + { + keep_organized_ = keep_organized; + } + + /** \brief Get whether the filtered points should be kept and set to the value given through \a setUserFilterValue (default = NaN), + * or removed from the PointCloud, thus potentially breaking its organized structure. + * \return The value of the internal \a keep_organized_ parameter; false = remove points (default), true = redefine points, keep structure. + */ + inline bool + getKeepOrganized () const + { + return (keep_organized_); + } + + /** \brief Provide a value that the filtered points should be set to instead of removing them. + * Used in conjunction with \a setKeepOrganized (). + * \param[in] value the user given value that the filtered point dimensions should be set to (default = NaN). + */ + inline void + setUserFilterValue (float value) + { + user_filter_value_ = value; + } + + protected: + + using Filter::initCompute; + using Filter::deinitCompute; + + /** \brief False = normal filter behavior (default), true = inverted behavior. */ + bool negative_; + + /** \brief False = remove points (default), true = redefine points, keep structure. */ + bool keep_organized_; + + /** \brief The user given value that the filtered point dimensions should be set to (default = NaN). */ + float user_filter_value_; + + /** \brief Abstract filter method for point cloud indices. */ + virtual void + applyFilter (std::vector &indices) = 0; + + /** \brief Abstract filter method for point cloud. */ + void + applyFilter (PointCloud &output) override = 0; + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b FilterIndices represents the base class for filters that are about binary point removal. + *
+ * All derived classes have to implement the \a filter (PointCloud &output) and the \a filter (std::vector &indices) methods. + * Ideally they also make use of the \a negative_, \a keep_organized_ and \a extract_removed_indices_ systems. + * The distinguishment between the \a negative_ and \a extract_removed_indices_ systems only makes sense if the class automatically + * filters non-finite entries in the filtering methods (recommended). + * \author Justin Rosen + * \ingroup filters + */ + template<> + class PCL_EXPORTS FilterIndices : public Filter + { + public: + using PCLPointCloud2 = pcl::PCLPointCloud2; + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to extract the indices of points being removed (default = false). + */ + FilterIndices (bool extract_removed_indices = false) : + negative_ (false), + keep_organized_ (false), + user_filter_value_ (std::numeric_limits::quiet_NaN ()) + { + extract_removed_indices_ = extract_removed_indices; + } + + /** \brief Empty virtual destructor. */ + + ~FilterIndices () + { + } + + virtual void + filter (PCLPointCloud2 &output) + { + pcl::Filter::filter (output); + } + + /** \brief Calls the filtering method and returns the filtered point cloud indices. + * \param[out] indices the resultant filtered point cloud indices + */ + void + filter (std::vector &indices); + + /** \brief Set whether the regular conditions for points filtering should apply, or the inverted conditions. + * \param[in] negative false = normal filter behavior (default), true = inverted behavior. + */ + inline void + setNegative (bool negative) + { + negative_ = negative; + } + + /** \brief Get whether the regular conditions for points filtering should apply, or the inverted conditions. + * \return The value of the internal \a negative_ parameter; false = normal filter behavior (default), true = inverted behavior. + */ + inline bool + getNegative () const + { + return (negative_); + } + + /** \brief Set whether the filtered points should be kept and set to the value given through \a setUserFilterValue (default: NaN), + * or removed from the PointCloud, thus potentially breaking its organized structure. + * \param[in] keep_organized false = remove points (default), true = redefine points, keep structure. + */ + inline void + setKeepOrganized (bool keep_organized) + { + keep_organized_ = keep_organized; + } + + /** \brief Get whether the filtered points should be kept and set to the value given through \a setUserFilterValue (default = NaN), + * or removed from the PointCloud, thus potentially breaking its organized structure. + * \return The value of the internal \a keep_organized_ parameter; false = remove points (default), true = redefine points, keep structure. + */ + inline bool + getKeepOrganized () const + { + return (keep_organized_); + } + + /** \brief Provide a value that the filtered points should be set to instead of removing them. + * Used in conjunction with \a setKeepOrganized (). + * \param[in] value the user given value that the filtered point dimensions should be set to (default = NaN). + */ + inline void + setUserFilterValue (float value) + { + user_filter_value_ = value; + } + + protected: + + /** \brief False = normal filter behavior (default), true = inverted behavior. */ + bool negative_; + + /** \brief False = remove points (default), true = redefine points, keep structure. */ + bool keep_organized_; + + /** \brief The user given value that the filtered point dimensions should be set to (default = NaN). */ + float user_filter_value_; + + /** \brief Abstract filter method for point cloud indices. */ + virtual void + applyFilter (std::vector &indices) = 0; + + /** \brief Abstract filter method for point cloud. */ + void + applyFilter (PCLPointCloud2 &output) override = 0; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/frustum_culling.h b/deps_install/include/pcl-1.10/pcl/filters/frustum_culling.h new file mode 100755 index 0000000..18b64a8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/frustum_culling.h @@ -0,0 +1,239 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief FrustumCulling filters points inside a frustum + * given by pose and field of view of the camera. + * + * Code example: + * + * \code + * pcl::PointCloud ::Ptr source; + * // .. read or fill the source cloud + * + * pcl::FrustumCulling fc; + * fc.setInputCloud (source); + * fc.setVerticalFOV (45); + * fc.setHorizontalFOV (60); + * fc.setNearPlaneDistance (5.0); + * fc.setFarPlaneDistance (15); + * + * Eigen::Matrix4f camera_pose; + * // .. read or input the camera pose from a registration algorithm. + * fc.setCameraPose (camera_pose); + * + * pcl::PointCloud target; + * fc.filter (target); + * \endcode + * + * + * \author Aravindhan K Krishnan + * \ingroup filters + */ + template + class FrustumCulling : public FilterIndices + { + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + using Filter::getClassName; + + FrustumCulling (bool extract_removed_indices = false) + : FilterIndices::FilterIndices (extract_removed_indices) + , camera_pose_ (Eigen::Matrix4f::Identity ()) + , hfov_ (60.0f) + , vfov_ (60.0f) + , np_dist_ (0.1f) + , fp_dist_ (5.0f) + { + filter_name_ = "FrustumCulling"; + } + + /** \brief Set the pose of the camera w.r.t the origin + * \param[in] camera_pose the camera pose + * + * Note: This assumes a coordinate system where X is forward, + * Y is up, and Z is right. To convert from the traditional camera + * coordinate system (X right, Y down, Z forward), one can use: + * + * \code + * Eigen::Matrix4f pose_orig = //pose in camera coordinates + * Eigen::Matrix4f cam2robot; + * cam2robot << 0, 0, 1, 0 + * 0,-1, 0, 0 + * 1, 0, 0, 0 + * 0, 0, 0, 1; + * Eigen::Matrix4f pose_new = pose_orig * cam2robot; + * fc.setCameraPose (pose_new); + * \endcode + */ + void + setCameraPose (const Eigen::Matrix4f& camera_pose) + { + camera_pose_ = camera_pose; + } + + /** \brief Get the pose of the camera w.r.t the origin */ + Eigen::Matrix4f + getCameraPose () const + { + return (camera_pose_); + } + + /** \brief Set the horizontal field of view for the camera in degrees + * \param[in] hfov the field of view + */ + void + setHorizontalFOV (float hfov) + { + hfov_ = hfov; + } + + /** \brief Get the horizontal field of view for the camera in degrees */ + float + getHorizontalFOV () const + { + return (hfov_); + } + + /** \brief Set the vertical field of view for the camera in degrees + * \param[in] vfov the field of view + */ + void + setVerticalFOV (float vfov) + { + vfov_ = vfov; + } + + /** \brief Get the vertical field of view for the camera in degrees */ + float + getVerticalFOV () const + { + return (vfov_); + } + + /** \brief Set the near plane distance + * \param[in] np_dist the near plane distance + */ + void + setNearPlaneDistance (float np_dist) + { + np_dist_ = np_dist; + } + + /** \brief Get the near plane distance. */ + float + getNearPlaneDistance () const + { + return (np_dist_); + } + + /** \brief Set the far plane distance + * \param[in] fp_dist the far plane distance + */ + void + setFarPlaneDistance (float fp_dist) + { + fp_dist_ = fp_dist; + } + + /** \brief Get the far plane distance */ + float + getFarPlaneDistance () const + { + return (fp_dist_); + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Sample of point indices into a separate PointCloud + * \param[out] output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Sample of point indices + * \param[out] indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + + private: + + /** \brief The camera pose */ + Eigen::Matrix4f camera_pose_; + /** \brief Horizontal field of view */ + float hfov_; + /** \brief Vertical field of view */ + float vfov_; + /** \brief Near plane distance */ + float np_dist_; + /** \brief Far plane distance */ + float fp_dist_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/grid_minimum.h b/deps_install/include/pcl-1.10/pcl/filters/grid_minimum.h new file mode 100755 index 0000000..49c3565 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/grid_minimum.h @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief GridMinimum assembles a local 2D grid over a given PointCloud, and downsamples the data. + * + * The GridMinimum class creates a *2D grid* over the input point cloud + * data. Then, in each *cell* (i.e., 2D grid element), all the points + * present will be *downsampled* with the minimum z value. This grid minimum + * can be useful in a number of topographic processing tasks such as crudely + * estimating ground returns, especially under foliage. + * + * \author Bradley J Chambers + * \ingroup filters + */ + template + class GridMinimum: public FilterIndices + { + protected: + using Filter::filter_name_; + using Filter::getClassName; + using Filter::input_; + using Filter::indices_; + + using PointCloud = typename FilterIndices::PointCloud; + + public: + /** \brief Empty constructor. */ + GridMinimum (const float resolution) + { + setResolution (resolution); + filter_name_ = "GridMinimum"; + } + + /** \brief Destructor. */ + ~GridMinimum () + { + } + + /** \brief Set the grid resolution. + * \param[in] resolution the grid resolution + */ + inline void + setResolution (const float resolution) + { + resolution_ = resolution; + // Use multiplications instead of divisions + inverse_resolution_ = 1.0f / resolution_; + } + + /** \brief Get the grid resolution. */ + inline float + getResolution () { return (resolution_); } + + protected: + /** \brief The resolution. */ + float resolution_; + + /** \brief Internal resolution stored as 1/resolution_ for efficiency reasons. */ + float inverse_resolution_; + + /** \brief Downsample a Point Cloud using a 2D grid approach + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) override + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/approximate_voxel_grid.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/approximate_voxel_grid.hpp new file mode 100755 index 0000000..50e8278 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/approximate_voxel_grid.hpp @@ -0,0 +1,138 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: voxel_grid.hpp 1600 2011-07-07 16:55:51Z shapovalov $ + * + */ + +#ifndef PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_ +#define PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ApproximateVoxelGrid::flush (PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size) +{ + hhe->centroid /= static_cast (hhe->count); + pcl::for_each_type (pcl::xNdCopyEigenPointFunctor (hhe->centroid, output.points[op])); + // ---[ RGB special case + if (rgba_index >= 0) + { + // pack r/g/b into rgb + float r = hhe->centroid[centroid_size-3], + g = hhe->centroid[centroid_size-2], + b = hhe->centroid[centroid_size-1]; + int rgb = (static_cast (r)) << 16 | (static_cast (g)) << 8 | (static_cast (b)); + memcpy (reinterpret_cast (&output.points[op]) + rgba_index, &rgb, sizeof (float)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ApproximateVoxelGrid::applyFilter (PointCloud &output) +{ + int centroid_size = 4; + if (downsample_all_data_) + centroid_size = boost::mpl::size::value; + + // ---[ RGB special case + std::vector fields; + int rgba_index = -1; + rgba_index = pcl::getFieldIndex ("rgb", fields); + if (rgba_index == -1) + rgba_index = pcl::getFieldIndex ("rgba", fields); + if (rgba_index >= 0) + { + rgba_index = fields[rgba_index].offset; + centroid_size += 3; + } + + for (std::size_t i = 0; i < histsize_; i++) + { + history_[i].count = 0; + history_[i].centroid = Eigen::VectorXf::Zero (centroid_size); + } + Eigen::VectorXf scratch = Eigen::VectorXf::Zero (centroid_size); + + output.points.resize (input_->points.size ()); // size output for worst case + std::size_t op = 0; // output pointer + for (std::size_t cp = 0; cp < input_->points.size (); ++cp) + { + int ix = static_cast (std::floor (input_->points[cp].x * inverse_leaf_size_[0])); + int iy = static_cast (std::floor (input_->points[cp].y * inverse_leaf_size_[1])); + int iz = static_cast (std::floor (input_->points[cp].z * inverse_leaf_size_[2])); + unsigned int hash = static_cast ((ix * 7171 + iy * 3079 + iz * 4231) & (histsize_ - 1)); + he *hhe = &history_[hash]; + if (hhe->count && ((ix != hhe->ix) || (iy != hhe->iy) || (iz != hhe->iz))) + { + flush (output, op++, hhe, rgba_index, centroid_size); + hhe->count = 0; + hhe->centroid.setZero ();// = Eigen::VectorXf::Zero (centroid_size); + } + hhe->ix = ix; + hhe->iy = iy; + hhe->iz = iz; + hhe->count++; + + // Unpack the point into scratch, then accumulate + // ---[ RGB special case + if (rgba_index >= 0) + { + // fill r/g/b data + pcl::RGB rgb; + memcpy (&rgb, (reinterpret_cast (&input_->points[cp])) + rgba_index, sizeof (RGB)); + scratch[centroid_size-3] = rgb.r; + scratch[centroid_size-2] = rgb.g; + scratch[centroid_size-1] = rgb.b; + } + pcl::for_each_type (xNdCopyPointEigenFunctor (input_->points[cp], scratch)); + hhe->centroid += scratch; + } + for (std::size_t i = 0; i < histsize_; i++) + { + he *hhe = &history_[i]; + if (hhe->count) + flush (output, op++, hhe, rgba_index, centroid_size); + } + output.points.resize (op); + output.width = static_cast (output.points.size ()); + output.height = 1; // downsampling breaks the organized structure + output.is_dense = false; // we filter out invalid points +} + +#define PCL_INSTANTIATE_ApproximateVoxelGrid(T) template class PCL_EXPORTS pcl::ApproximateVoxelGrid; + +#endif // PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/bilateral.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/bilateral.hpp new file mode 100755 index 0000000..b8d79fc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/bilateral.hpp @@ -0,0 +1,113 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_BILATERAL_IMPL_H_ +#define PCL_FILTERS_BILATERAL_IMPL_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::BilateralFilter::computePointWeight (const int pid, + const std::vector &indices, + const std::vector &distances) +{ + double BF = 0, W = 0; + + // For each neighbor + for (std::size_t n_id = 0; n_id < indices.size (); ++n_id) + { + int id = indices[n_id]; + // Compute the difference in intensity + double intensity_dist = std::abs (input_->points[pid].intensity - input_->points[id].intensity); + + // Compute the Gaussian intensity weights both in Euclidean and in intensity space + double dist = std::sqrt (distances[n_id]); + double weight = kernel (dist, sigma_s_) * kernel (intensity_dist, sigma_r_); + + // Calculate the bilateral filter response + BF += weight * input_->points[id].intensity; + W += weight; + } + return (BF / W); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BilateralFilter::applyFilter (PointCloud &output) +{ + // Check if sigma_s has been given by the user + if (sigma_s_ == 0) + { + PCL_ERROR ("[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n"); + return; + } + // In case a search method has not been given, initialize it using some defaults + if (!tree_) + { + // For organized datasets, use an OrganizedNeighbor + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + // For unorganized data, use a FLANN kdtree + else + tree_.reset (new pcl::search::KdTree (false)); + } + tree_->setInputCloud (input_); + + std::vector k_indices; + std::vector k_distances; + + // Copy the input data into the output + output = *input_; + + // For all the indices given (equal to the entire cloud if none given) + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Perform a radius search to find the nearest neighbors + tree_->radiusSearch ((*indices_)[i], sigma_s_ * 2, k_indices, k_distances); + + // Overwrite the intensity value with the computed average + output.points[(*indices_)[i]].intensity = static_cast (computePointWeight ((*indices_)[i], k_indices, k_distances)); + } +} + +#define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter; + +#endif // PCL_FILTERS_BILATERAL_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/box_clipper3D.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/box_clipper3D.hpp new file mode 100755 index 0000000..2cb641c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/box_clipper3D.hpp @@ -0,0 +1,216 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP +#define PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP + +#include + +template +pcl::BoxClipper3D::BoxClipper3D (const Eigen::Affine3f& transformation) +: transformation_ (transformation) +{ + //inverse_transformation_ = transformation_.inverse (); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::BoxClipper3D::BoxClipper3D (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size) +{ + setTransformation (rodrigues, translation, box_size); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::BoxClipper3D::~BoxClipper3D () noexcept +{ +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BoxClipper3D::setTransformation (const Eigen::Affine3f& transformation) +{ + transformation_ = transformation; + //inverse_transformation_ = transformation_.inverse (); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BoxClipper3D::setTransformation (const Eigen::Vector3f& rodrigues, const Eigen::Vector3f& translation, const Eigen::Vector3f& box_size) +{ + transformation_ = Eigen::Translation3f (translation) * Eigen::AngleAxisf(rodrigues.norm (), rodrigues.normalized ()) * Eigen::Scaling (box_size); + //inverse_transformation_ = transformation_.inverse (); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::Clipper3D* +pcl::BoxClipper3D::clone () const +{ + return new BoxClipper3D (transformation_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BoxClipper3D::transformPoint (const PointT& pointIn, PointT& pointOut) const +{ + const Eigen::Vector4f& point = pointIn.getVector4fMap (); + pointOut.getVector4fMap () = transformation_ * point; + + // homogeneous value might not be 1 + if (point [3] != 1) + { + // homogeneous component might be uninitialized -> invalid + if (point [3] != 0) + { + pointOut.x += (1 - point [3]) * transformation_.data () [ 9]; + pointOut.y += (1 - point [3]) * transformation_.data () [10]; + pointOut.z += (1 - point [3]) * transformation_.data () [11]; + } + else + { + pointOut.x += transformation_.data () [ 9]; + pointOut.y += transformation_.data () [10]; + pointOut.z += transformation_.data () [11]; + } + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::BoxClipper3D::clipPoint3D (const PointT& point) const +{ + Eigen::Vector4f point_coordinates (transformation_.matrix () + * point.getVector4fMap ()); + return (point_coordinates.array ().abs () <= 1).all (); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * @attention untested code + */ +template bool +pcl::BoxClipper3D::clipLineSegment3D (PointT&, PointT&) const +{ + /* + PointT pt1, pt2; + transformPoint (point1, pt1); + transformPoint (point2, pt2); + + // + bool pt1InBox = (std::abs(pt1.x) <= 1.0 && std::abs (pt1.y) <= 1.0 && std::abs (pt1.z) <= 1.0); + bool pt2InBox = (std::abs(pt2.x) <= 1.0 && std::abs (pt2.y) <= 1.0 && std::abs (pt2.z) <= 1.0); + + // one is outside the other one inside the box + //if (pt1InBox ^ pt2InBox) + if (pt1InBox && !pt2InBox) + { + PointT diff; + PointT lambda; + diff.getVector3fMap () = pt2.getVector3fMap () - pt1.getVector3fMap (); + + if (diff.x > 0) + lambda.x = (1.0 - pt1.x) / diff.x; + else + lambda.x = (-1.0 - pt1.x) / diff.x; + + if (diff.y > 0) + lambda.y = (1.0 - pt1.y) / diff.y; + else + lambda.y = (-1.0 - pt1.y) / diff.y; + + if (diff.z > 0) + lambda.z = (1.0 - pt1.z) / diff.z; + else + lambda.z = (-1.0 - pt1.z) / diff.z; + + pt2 = pt1 + std::min(std::min(lambda.x, lambda.y), lambda.z) * diff; + + // inverse transformation + inverseTransformPoint (pt2, point2); + return true; + } + else if (!pt1InBox && pt2InBox) + { + return true; + } + */ + throw std::logic_error ("Not implemented"); + return false; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * @attention untested code + */ +template void +pcl::BoxClipper3D::clipPlanarPolygon3D (const std::vector >&, std::vector >& clipped_polygon) const +{ + // not implemented -> clip everything + clipped_polygon.clear (); + throw std::logic_error ("Not implemented"); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * @attention untested code + */ +template void +pcl::BoxClipper3D::clipPlanarPolygon3D (std::vector >& polygon) const +{ + // not implemented -> clip everything + polygon.clear (); + throw std::logic_error ("Not implemented"); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations. +template void +pcl::BoxClipper3D::clipPointCloud3D (const pcl::PointCloud& cloud_in, std::vector& clipped, const std::vector& indices) const +{ + clipped.clear (); + if (indices.empty ()) + { + clipped.reserve (cloud_in.size ()); + for (std::size_t pIdx = 0; pIdx < cloud_in.size (); ++pIdx) + if (clipPoint3D (cloud_in[pIdx])) + clipped.push_back (pIdx); + } + else + { + for (const int &index : indices) + if (clipPoint3D (cloud_in[index])) + clipped.push_back (index); + } +} +#endif //PCL_FILTERS_IMPL_BOX_CLIPPER3D_HPP diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/conditional_removal.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/conditional_removal.hpp new file mode 100755 index 0000000..bce677a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/conditional_removal.hpp @@ -0,0 +1,789 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_ +#define PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template +pcl::FieldComparison::FieldComparison ( + const std::string &field_name, ComparisonOps::CompareOp op, double compare_val) + : ComparisonBase () + , compare_val_ (compare_val), point_data_ (nullptr) +{ + field_name_ = field_name; + op_ = op; + + // Get all fields + const auto point_fields = pcl::getFields (); + + // Find field_name + if (point_fields.empty ()) + { + PCL_WARN ("[pcl::FieldComparison::FieldComparison] no fields found!\n"); + capable_ = false; + return; + } + + // Get the field index + std::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; + } + std::uint8_t datatype = point_fields[d].datatype; + std::uint32_t offset = point_fields[d].offset; + + point_data_ = new PointDataAtOffset(datatype, offset); + capable_ = true; +} + +////////////////////////////////////////////////////////////////////////// +template +pcl::FieldComparison::~FieldComparison () +{ + if (point_data_ != nullptr) + { + delete point_data_; + point_data_ = nullptr; + } +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::FieldComparison::evaluate (const PointT &point) const +{ + if (!this->capable_) + { + PCL_WARN ("[pcl::FieldComparison::evaluate] invalid comparison!\n"); + return (false); + } + + // if p(data) > val then compare_result = 1 + // if p(data) == val then compare_result = 0 + // if p(data) < ival then compare_result = -1 + int compare_result = point_data_->compare (point, compare_val_); + + switch (this->op_) + { + case pcl::ComparisonOps::GT : + return (compare_result > 0); + case pcl::ComparisonOps::GE : + return (compare_result >= 0); + case pcl::ComparisonOps::LT : + return (compare_result < 0); + case pcl::ComparisonOps::LE : + return (compare_result <= 0); + case pcl::ComparisonOps::EQ : + return (compare_result == 0); + default: + PCL_WARN ("[pcl::FieldComparison::evaluate] unrecognized op_!\n"); + return (false); + } +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template +pcl::PackedRGBComparison::PackedRGBComparison ( + const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) : + component_name_ (component_name), component_offset_ (), compare_val_ (compare_val) +{ + // get all the fields + const auto point_fields = pcl::getFields (); + + // Locate the "rgb" field + std::size_t d; + for (d = 0; d < point_fields.size (); ++d) + { + if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba") + break; + } + if (d == point_fields.size ()) + { + PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n"); + capable_ = false; + return; + } + + // Verify the datatype + std::uint8_t datatype = point_fields[d].datatype; + if (datatype != pcl::PCLPointField::FLOAT32 && + datatype != pcl::PCLPointField::UINT32 && + datatype != pcl::PCLPointField::INT32) + { + PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n"); + capable_ = false; + return; + } + + // Verify the component name + if (component_name == "r") + { + component_offset_ = point_fields[d].offset + 2; + } + else if (component_name == "g") + { + component_offset_ = point_fields[d].offset + 1; + } + else if (component_name == "b") + { + component_offset_ = point_fields[d].offset; + } + else + { + PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n"); + capable_ = false; + return; + } + + // save the rest of the context + capable_ = true; + op_ = op; +} + + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::PackedRGBComparison::evaluate (const PointT &point) const +{ + // extract the component value + const std::uint8_t* pt_data = reinterpret_cast (&point); + std::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 +pcl::PackedHSIComparison::PackedHSIComparison ( + const std::string &component_name, ComparisonOps::CompareOp op, double compare_val) : + component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ () +{ + // Get all the fields + const auto point_fields = pcl::getFields (); + + // Locate the "rgb" field + std::size_t d; + for (d = 0; d < point_fields.size (); ++d) + if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba") + break; + if (d == point_fields.size ()) + { + PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n"); + capable_ = false; + return; + } + + // Verify the datatype + std::uint8_t datatype = point_fields[d].datatype; + if (datatype != pcl::PCLPointField::FLOAT32 && + datatype != pcl::PCLPointField::UINT32 && + datatype != pcl::PCLPointField::INT32) + { + PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n"); + capable_ = false; + return; + } + + // verify the offset + std::uint32_t offset = point_fields[d].offset; + if (offset % 4 != 0) + { + PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n"); + capable_ = false; + return; + } + rgb_offset_ = point_fields[d].offset; + + // verify the component name + if (component_name == "h" ) + { + component_id_ = H; + } + else if (component_name == "s") + { + component_id_ = S; + } + else if (component_name == "i") + { + component_id_ = I; + } + else + { + PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] unrecognized component name!\n"); + capable_ = false; + return; + } + + // Save the context + capable_ = true; + op_ = op; +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::PackedHSIComparison::evaluate (const PointT &point) const +{ + // Since this is a const function, we can't make these data members because we change them here + static std::uint32_t rgb_val_ = 0; + static std::uint8_t r_ = 0; + static std::uint8_t g_ = 0; + static std::uint8_t b_ = 0; + static std::int8_t h_ = 0; + static std::uint8_t s_ = 0; + static std::uint8_t i_ = 0; + + // We know that rgb data is 32 bit aligned (verified in the ctor) so... + const std::uint8_t* pt_data = reinterpret_cast (&point); + const std::uint32_t* rgb_data = reinterpret_cast (pt_data + rgb_offset_); + std::uint32_t new_rgb_val = *rgb_data; + + if (rgb_val_ != new_rgb_val) + { // avoid having to redo this calc, if possible + rgb_val_ = new_rgb_val; + // extract r,g,b + r_ = static_cast (rgb_val_ >> 16); + g_ = static_cast (rgb_val_ >> 8); + b_ = static_cast (rgb_val_); + + // definitions taken from http://en.wikipedia.org/wiki/HSL_and_HSI + float hx = (2.0f * r_ - g_ - b_) / 4.0f; // hue x component -127 to 127 + float hy = static_cast (g_ - b_) * 111.0f / 255.0f; // hue y component -111 to 111 + h_ = static_cast (std::atan2(hy, hx) * 128.0f / M_PI); + + std::int32_t i = (r_+g_+b_)/3; // 0 to 255 + i_ = static_cast (i); + + std::int32_t m; // min(r,g,b) + m = (r_ < g_) ? r_ : g_; + m = (m < b_) ? m : b_; + + s_ = static_cast ((i == 0) ? 0 : 255 - (m * 255) / i); // saturation 0 to 255 + } + + float my_val = 0; + + switch (component_id_) + { + case H: + my_val = static_cast (h_); + break; + case S: + my_val = static_cast (s_); + break; + case I: + my_val = static_cast (i_); + break; + default: + assert (false); + } + + // 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::PackedHSIComparison::evaluate] unrecognized op_!\n"); + return (false); + } +} + + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template +pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison () : + comp_scalar_ (0.0) +{ + // get all the fields + const auto point_fields = pcl::getFields (); + + // Locate the "x" field + std::size_t dX; + for (dX = 0; dX < point_fields.size (); ++dX) + { + if (point_fields[dX].name == "x") + break; + } + if (dX == point_fields.size ()) + { + PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n"); + capable_ = false; + return; + } + + // Locate the "y" field + std::size_t dY; + for (dY = 0; dY < point_fields.size (); ++dY) + { + if (point_fields[dY].name == "y") + break; + } + if (dY == point_fields.size ()) + { + PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n"); + capable_ = false; + return; + } + + // Locate the "z" field + std::size_t dZ; + for (dZ = 0; dZ < point_fields.size (); ++dZ) + { + if (point_fields[dZ].name == "z") + break; + } + if (dZ == point_fields.size ()) + { + PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n"); + capable_ = false; + return; + } + + comp_matr_ << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + comp_vect_ << 0.0, 0.0, 0.0, 1.0; + tf_comp_matr_ = comp_matr_; + tf_comp_vect_ = comp_vect_; + op_ = pcl::ComparisonOps::EQ; + capable_ = true; +} + +////////////////////////////////////////////////////////////////////////// +template +pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison (const pcl::ComparisonOps::CompareOp op, + const Eigen::Matrix3f &comparison_matrix, + const Eigen::Vector3f &comparison_vector, + const float &comparison_scalar, + const Eigen::Affine3f &comparison_transform) : + comp_scalar_ (comparison_scalar) +{ + // get all the fields + const auto point_fields = pcl::getFields (); + + // Locate the "x" field + std::size_t dX; + for (dX = 0; dX < point_fields.size (); ++dX) + { + if (point_fields[dX].name == "x") + break; + } + if (dX == point_fields.size ()) + { + PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n"); + capable_ = false; + return; + } + + // Locate the "y" field + std::size_t dY; + for (dY = 0; dY < point_fields.size (); ++dY) + { + if (point_fields[dY].name == "y") + break; + } + if (dY == point_fields.size ()) + { + PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n"); + capable_ = false; + return; + } + + // Locate the "z" field + std::size_t dZ; + for (dZ = 0; dZ < point_fields.size (); ++dZ) + { + if (point_fields[dZ].name == "z") + break; + } + if (dZ == point_fields.size ()) + { + PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n"); + capable_ = false; + return; + } + + capable_ = true; + op_ = op; + setComparisonMatrix (comparison_matrix); + setComparisonVector (comparison_vector); + if (!comparison_transform.matrix ().isIdentity ()) + transformComparison (comparison_transform); +} + +////////////////////////////////////////////////////////////////////////// +template +bool +pcl::TfQuadraticXYZComparison::evaluate (const PointT &point) const +{ + Eigen::Vector4f pointAffine; + pointAffine << point.x, point.y, point.z, 1; + + float myVal = static_cast(2.0f * tf_comp_vect_.transpose () * pointAffine) + static_cast(pointAffine.transpose () * tf_comp_matr_ * pointAffine) + comp_scalar_ - 3.0f; + + // now do the comparison + switch (this->op_) + { + case pcl::ComparisonOps::GT: + return (myVal > 0); + case pcl::ComparisonOps::GE: + return (myVal >= 0); + case pcl::ComparisonOps::LT: + return (myVal < 0); + case pcl::ComparisonOps::LE: + return (myVal <= 0); + case pcl::ComparisonOps::EQ: + return (myVal == 0); + default: + PCL_WARN ("[pcl::TfQuadraticXYZComparison::evaluate] unrecognized op_!\n"); + return (false); + } +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template int +pcl::PointDataAtOffset::compare (const PointT& p, const double& val) +{ + // if p(data) > val return 1 + // if p(data) == val return 0 + // if p(data) < val return -1 + + const std::uint8_t* pt_data = reinterpret_cast (&p); + + switch (datatype_) + { + case pcl::PCLPointField::INT8 : + { + std::int8_t pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (std::int8_t)); + return (pt_val > static_cast(val)) - (pt_val < static_cast (val)); + } + case pcl::PCLPointField::UINT8 : + { + std::uint8_t pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (std::uint8_t)); + return (pt_val > static_cast(val)) - (pt_val < static_cast (val)); + } + case pcl::PCLPointField::INT16 : + { + std::int16_t pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (std::int16_t)); + return (pt_val > static_cast(val)) - (pt_val < static_cast (val)); + } + case pcl::PCLPointField::UINT16 : + { + std::uint16_t pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (std::uint16_t)); + return (pt_val > static_cast (val)) - (pt_val < static_cast (val)); + } + case pcl::PCLPointField::INT32 : + { + std::int32_t pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (std::int32_t)); + return (pt_val > static_cast (val)) - (pt_val < static_cast (val)); + } + case pcl::PCLPointField::UINT32 : + { + std::uint32_t pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (std::uint32_t)); + return (pt_val > static_cast (val)) - (pt_val < static_cast (val)); + } + case pcl::PCLPointField::FLOAT32 : + { + float pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (float)); + return (pt_val > static_cast (val)) - (pt_val < static_cast (val)); + } + case pcl::PCLPointField::FLOAT64 : + { + double pt_val; + memcpy (&pt_val, pt_data + this->offset_, sizeof (double)); + return (pt_val > val) - (pt_val < val); + } + default : + PCL_WARN ("[pcl::PointDataAtOffset::compare] unknown data_type!\n"); + return (0); + } +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConditionBase::addComparison (ComparisonBaseConstPtr comparison) +{ + if (!comparison->isCapable ()) + capable_ = false; + comparisons_.push_back (comparison); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConditionBase::addCondition (Ptr condition) +{ + if (!condition->isCapable ()) + capable_ = false; + conditions_.push_back (condition); +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template bool +pcl::ConditionAnd::evaluate (const PointT &point) const +{ + for (std::size_t i = 0; i < comparisons_.size (); ++i) + if (!comparisons_[i]->evaluate (point)) + return (false); + + for (std::size_t i = 0; i < conditions_.size (); ++i) + if (!conditions_[i]->evaluate (point)) + return (false); + + return (true); +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template bool +pcl::ConditionOr::evaluate (const PointT &point) const +{ + if (comparisons_.empty () && conditions_.empty ()) + return (true); + for (std::size_t i = 0; i < comparisons_.size (); ++i) + if (comparisons_[i]->evaluate(point)) + return (true); + + for (std::size_t i = 0; i < conditions_.size (); ++i) + if (conditions_[i]->evaluate (point)) + return (true); + + return (false); +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConditionalRemoval::setCondition (ConditionBasePtr condition) +{ + condition_ = condition; + capable_ = condition_->isCapable (); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConditionalRemoval::applyFilter (PointCloud &output) +{ + if (capable_ == false) + { + PCL_WARN ("[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ()); + return; + } + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + return; + } + + if (condition_.get () == nullptr) + { + PCL_WARN ("[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ()); + return; + } + + // Copy the header (and thus the frame_id) + allocate enough space for points + output.header = input_->header; + if (!keep_organized_) + { + output.height = 1; // filtering breaks the organized structure + output.is_dense = true; + } + else + { + output.height = this->input_->height; + output.width = this->input_->width; + output.is_dense = this->input_->is_dense; + } + output.points.resize (input_->points.size ()); + removed_indices_->resize (input_->points.size ()); + + int nr_removed_p = 0; + + if (!keep_organized_) + { + int nr_p = 0; + for (std::size_t index: (*Filter::indices_)) + { + + const PointT& point = input_->points[index]; + // Check if the point is invalid + if (!std::isfinite (point.x) + || !std::isfinite (point.y) + || !std::isfinite (point.z)) + { + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = index; + nr_removed_p++; + } + continue; + } + + if (condition_->evaluate (point)) + { + copyPoint (point, output.points[nr_p]); + nr_p++; + } + else + { + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = index; + nr_removed_p++; + } + } + } + + output.width = nr_p; + output.points.resize (nr_p); + } + else + { + std::vector indices = *Filter::indices_; + std::sort (indices.begin (), indices.end ()); //TODO: is this necessary or can we assume the indices to be sorted? + bool removed_p = false; + std::size_t ci = 0; + for (std::size_t cp = 0; cp < input_->points.size (); ++cp) + { + if (cp == static_cast (indices[ci])) + { + if (ci < indices.size () - 1) + { + ci++; + if (cp == static_cast (indices[ci])) //check whether the next index will have the same value. TODO: necessary? + continue; + } + + // copy all the fields + copyPoint (input_->points[cp], output.points[cp]); + + if (!condition_->evaluate (input_->points[cp])) + { + output.points[cp].getVector4fMap ().setConstant (user_filter_value_); + removed_p = true; + + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = static_cast (cp); + nr_removed_p++; + } + } + } + else + { + output.points[cp].getVector4fMap ().setConstant (user_filter_value_); + removed_p = true; + //as for !keep_organized_: removed points due to setIndices are not considered as removed_indices_ + } + } + + if (removed_p && !std::isfinite (user_filter_value_)) + output.is_dense = false; + } + removed_indices_->resize (nr_removed_p); +} + +#define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset; +#define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase; +#define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison; +#define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison; +#define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison; +#define PCL_INSTANTIATE_TfQuadraticXYZComparison(T) template class PCL_EXPORTS pcl::TfQuadraticXYZComparison; +#define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase; +#define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd; +#define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr; +#define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/convolution.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/convolution.hpp new file mode 100755 index 0000000..bf2817d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/convolution.hpp @@ -0,0 +1,671 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_CONVOLUTION_IMPL_HPP +#define PCL_FILTERS_CONVOLUTION_IMPL_HPP + +#include +#include + +template +pcl::filters::Convolution::Convolution () + : borders_policy_ (BORDERS_POLICY_IGNORE) + , distance_threshold_ (std::numeric_limits::infinity ()) + , input_ () + , half_width_ () + , kernel_width_ () + , threads_ (1) +{} + +template void +pcl::filters::Convolution::initCompute (PointCloud& output) +{ + if (borders_policy_ != BORDERS_POLICY_IGNORE && + borders_policy_ != BORDERS_POLICY_MIRROR && + borders_policy_ != BORDERS_POLICY_DUPLICATE) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::filters::Convolution::initCompute] unknown borders policy."); + + if(kernel_.size () % 2 == 0) + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::filters::Convolution::initCompute] convolving element width must be odd."); + + if (distance_threshold_ != std::numeric_limits::infinity ()) + distance_threshold_ *= static_cast (kernel_.size () % 2) * distance_threshold_; + + half_width_ = static_cast (kernel_.size ()) / 2; + kernel_width_ = static_cast (kernel_.size () - 1); + + if (&(*input_) != &output) + { + if (output.height != input_->height || output.width != input_->width) + { + output.resize (input_->width * input_->height); + output.width = input_->width; + output.height = input_->height; + } + } + output.is_dense = input_->is_dense; +} + +template inline void +pcl::filters::Convolution::convolveRows (PointCloudOut& output) +{ + try + { + initCompute (output); + switch (borders_policy_) + { + case BORDERS_POLICY_MIRROR : convolve_rows_mirror (output); break; + case BORDERS_POLICY_DUPLICATE : convolve_rows_duplicate (output); break; + case BORDERS_POLICY_IGNORE : convolve_rows (output); + } + } + catch (InitFailedException& e) + { + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::filters::Convolution::convolveRows] init failed " << e.what ()); + } +} + +template inline void +pcl::filters::Convolution::convolveCols (PointCloudOut& output) +{ + try + { + initCompute (output); + switch (borders_policy_) + { + case BORDERS_POLICY_MIRROR : convolve_cols_mirror (output); break; + case BORDERS_POLICY_DUPLICATE : convolve_cols_duplicate (output); break; + case BORDERS_POLICY_IGNORE : convolve_cols (output); + } + } + catch (InitFailedException& e) + { + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::filters::Convolution::convolveCols] init failed " << e.what ()); + } +} + +template inline void +pcl::filters::Convolution::convolve (const Eigen::ArrayXf& h_kernel, + const Eigen::ArrayXf& v_kernel, + PointCloud& output) +{ + try + { + PointCloudInPtr tmp (new PointCloud ()); + setKernel (h_kernel); + convolveRows (*tmp); + setInputCloud (tmp); + setKernel (v_kernel); + convolveCols (output); + } + catch (InitFailedException& e) + { + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::filters::Convolution::convolve] init failed " << e.what ()); + } +} + +template inline void +pcl::filters::Convolution::convolve (PointCloud& output) +{ + try + { + PointCloudInPtr tmp (new PointCloud ()); + convolveRows (*tmp); + setInputCloud (tmp); + convolveCols (output); + } + catch (InitFailedException& e) + { + PCL_THROW_EXCEPTION (InitFailedException, + "[pcl::filters::Convolution::convolve] init failed " << e.what ()); + } +} + +template inline PointOut +pcl::filters::Convolution::convolveOneRowDense (int i, int j) +{ + using namespace pcl::common; + PointOut result; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) + result+= (*input_) (l,j) * kernel_[k]; + return (result); +} + +template inline PointOut +pcl::filters::Convolution::convolveOneColDense (int i, int j) +{ + using namespace pcl::common; + PointOut result; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) + result+= (*input_) (i,l) * kernel_[k]; + return (result); +} + +template inline PointOut +pcl::filters::Convolution::convolveOneRowNonDense (int i, int j) +{ + using namespace pcl::common; + PointOut result; + float weight = 0; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) + { + if (!isFinite ((*input_) (l,j))) + continue; + if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_) + { + result+= (*input_) (l,j) * kernel_[k]; + weight += kernel_[k]; + } + } + if (weight == 0) + result.x = result.y = result.z = std::numeric_limits::quiet_NaN (); + else + { + weight = 1.f/weight; + result*= weight; + } + return (result); +} + +template inline PointOut +pcl::filters::Convolution::convolveOneColNonDense (int i, int j) +{ + using namespace pcl::common; + PointOut result; + float weight = 0; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) + { + if (!isFinite ((*input_) (i,l))) + continue; + if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_) + { + result+= (*input_) (i,l) * kernel_[k]; + weight += kernel_[k]; + } + } + if (weight == 0) + result.x = result.y = result.z = std::numeric_limits::quiet_NaN (); + else + { + weight = 1.f/weight; + result*= weight; + } + return (result); +} + +namespace pcl +{ + namespace filters + { + template<> pcl::PointXYZRGB + Convolution::convolveOneRowDense (int i, int j) + { + pcl::PointXYZRGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) + { + result.x += (*input_) (l,j).x * kernel_[k]; + result.y += (*input_) (l,j).y * kernel_[k]; + result.z += (*input_) (l,j).z * kernel_[k]; + r += kernel_[k] * static_cast ((*input_) (l,j).r); + g += kernel_[k] * static_cast ((*input_) (l,j).g); + b += kernel_[k] * static_cast ((*input_) (l,j).b); + } + result.r = static_cast (r); + result.g = static_cast (g); + result.b = static_cast (b); + return (result); + } + + template<> pcl::PointXYZRGB + Convolution::convolveOneColDense (int i, int j) + { + pcl::PointXYZRGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) + { + result.x += (*input_) (i,l).x * kernel_[k]; + result.y += (*input_) (i,l).y * kernel_[k]; + result.z += (*input_) (i,l).z * kernel_[k]; + r += kernel_[k] * static_cast ((*input_) (i,l).r); + g += kernel_[k] * static_cast ((*input_) (i,l).g); + b += kernel_[k] * static_cast ((*input_) (i,l).b); + } + result.r = static_cast (r); + result.g = static_cast (g); + result.b = static_cast (b); + return (result); + } + + template<> pcl::PointXYZRGB + Convolution::convolveOneRowNonDense (int i, int j) + { + pcl::PointXYZRGB result; + float weight = 0; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) + { + if (!isFinite ((*input_) (l,j))) + continue; + if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_) + { + result.x += (*input_) (l,j).x * kernel_[k]; result.y += (*input_) (l,j).y * kernel_[k]; result.z += (*input_) (l,j).z * kernel_[k]; + r+= kernel_[k] * static_cast ((*input_) (l,j).r); + g+= kernel_[k] * static_cast ((*input_) (l,j).g); + b+= kernel_[k] * static_cast ((*input_) (l,j).b); + weight += kernel_[k]; + } + } + + if (weight == 0) + result.x = result.y = result.z = std::numeric_limits::quiet_NaN (); + else + { + weight = 1.f/weight; + r*= weight; g*= weight; b*= weight; + result.x*= weight; result.y*= weight; result.z*= weight; + result.r = static_cast (r); + result.g = static_cast (g); + result.b = static_cast (b); + } + return (result); + } + + template<> pcl::PointXYZRGB + Convolution::convolveOneColNonDense (int i, int j) + { + pcl::PointXYZRGB result; + float weight = 0; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) + { + if (!isFinite ((*input_) (i,l))) + continue; + if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_) + { + result.x += (*input_) (i,l).x * kernel_[k]; result.y += (*input_) (i,l).y * kernel_[k]; result.z += (*input_) (i,l).z * kernel_[k]; + r+= kernel_[k] * static_cast ((*input_) (i,l).r); + g+= kernel_[k] * static_cast ((*input_) (i,l).g); + b+= kernel_[k] * static_cast ((*input_) (i,l).b); + weight+= kernel_[k]; + } + } + if (weight == 0) + result.x = result.y = result.z = std::numeric_limits::quiet_NaN (); + else + { + weight = 1.f/weight; + r*= weight; g*= weight; b*= weight; + result.x*= weight; result.y*= weight; result.z*= weight; + result.r = static_cast (r); + result.g = static_cast (g); + result.b = static_cast (b); + } + return (result); + } + + /////////////////////////////////////////////////////////////////////////////////////////////// + template<> pcl::RGB + Convolution::convolveOneRowDense (int i, int j) + { + pcl::RGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) + { + r += kernel_[k] * static_cast ((*input_) (l,j).r); + g += kernel_[k] * static_cast ((*input_) (l,j).g); + b += kernel_[k] * static_cast ((*input_) (l,j).b); + } + result.r = static_cast (r); + result.g = static_cast (g); + result.b = static_cast (b); + return (result); + } + + template<> pcl::RGB + Convolution::convolveOneColDense (int i, int j) + { + pcl::RGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) + { + r += kernel_[k] * static_cast ((*input_) (i,l).r); + g += kernel_[k] * static_cast ((*input_) (i,l).g); + b += kernel_[k] * static_cast ((*input_) (i,l).b); + } + result.r = static_cast (r); + result.g = static_cast (g); + result.b = static_cast (b); + return (result); + } + + template<> pcl::RGB + Convolution::convolveOneRowNonDense (int i, int j) + { + return (convolveOneRowDense (i,j)); + } + + template<> pcl::RGB + Convolution::convolveOneColNonDense (int i, int j) + { + return (convolveOneColDense (i,j)); + } + + template<> void + Convolution::makeInfinite (pcl::RGB& p) + { + p.r = 0; p.g = 0; p.b = 0; + } + } +} + +template void +pcl::filters::Convolution::convolve_rows (PointCloudOut& output) +{ + using namespace pcl::common; + + int width = input_->width; + int height = input_->height; + int last = input_->width - half_width_; + if (input_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int j = 0; j < height; ++j) + { + for (int i = 0; i < half_width_; ++i) + makeInfinite (output (i,j)); + + for (int i = half_width_; i < last; ++i) + output (i,j) = convolveOneRowDense (i,j); + + for (int i = last; i < width; ++i) + makeInfinite (output (i,j)); + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int j = 0; j < height; ++j) + { + for (int i = 0; i < half_width_; ++i) + makeInfinite (output (i,j)); + + for (int i = half_width_; i < last; ++i) + output (i,j) = convolveOneRowNonDense (i,j); + + for (int i = last; i < width; ++i) + makeInfinite (output (i,j)); + } + } +} + +template void +pcl::filters::Convolution::convolve_rows_duplicate (PointCloudOut& output) +{ + using namespace pcl::common; + + int width = input_->width; + int height = input_->height; + int last = input_->width - half_width_; + int w = last - 1; + if (input_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int j = 0; j < height; ++j) + { + for (int i = half_width_; i < last; ++i) + output (i,j) = convolveOneRowDense (i,j); + + for (int i = last; i < width; ++i) + output (i,j) = output (w, j); + + for (int i = 0; i < half_width_; ++i) + output (i,j) = output (half_width_, j); + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int j = 0; j < height; ++j) + { + for (int i = half_width_; i < last; ++i) + output (i,j) = convolveOneRowNonDense (i,j); + + for (int i = last; i < width; ++i) + output (i,j) = output (w, j); + + for (int i = 0; i < half_width_; ++i) + output (i,j) = output (half_width_, j); + } + } +} + +template void +pcl::filters::Convolution::convolve_rows_mirror (PointCloudOut& output) +{ + using namespace pcl::common; + + int width = input_->width; + int height = input_->height; + int last = input_->width - half_width_; + int w = last - 1; + if (input_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int j = 0; j < height; ++j) + { + for (int i = half_width_; i < last; ++i) + output (i,j) = convolveOneRowDense (i,j); + + for (int i = last, l = 0; i < width; ++i, ++l) + output (i,j) = output (w-l, j); + + for (int i = 0; i < half_width_; ++i) + output (i,j) = output (half_width_+1-i, j); + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int j = 0; j < height; ++j) + { + for (int i = half_width_; i < last; ++i) + output (i,j) = convolveOneRowNonDense (i,j); + + for (int i = last, l = 0; i < width; ++i, ++l) + output (i,j) = output (w-l, j); + + for (int i = 0; i < half_width_; ++i) + output (i,j) = output (half_width_+1-i, j); + } + } +} + +template void +pcl::filters::Convolution::convolve_cols (PointCloudOut& output) +{ + using namespace pcl::common; + + int width = input_->width; + int height = input_->height; + int last = input_->height - half_width_; + if (input_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int i = 0; i < width; ++i) + { + for (int j = 0; j < half_width_; ++j) + makeInfinite (output (i,j)); + + for (int j = half_width_; j < last; ++j) + output (i,j) = convolveOneColDense (i,j); + + for (int j = last; j < height; ++j) + makeInfinite (output (i,j)); + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int i = 0; i < width; ++i) + { + for (int j = 0; j < half_width_; ++j) + makeInfinite (output (i,j)); + + for (int j = half_width_; j < last; ++j) + output (i,j) = convolveOneColNonDense (i,j); + + for (int j = last; j < height; ++j) + makeInfinite (output (i,j)); + } + } +} + +template void +pcl::filters::Convolution::convolve_cols_duplicate (PointCloudOut& output) +{ + using namespace pcl::common; + + int width = input_->width; + int height = input_->height; + int last = input_->height - half_width_; + int h = last -1; + if (input_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int i = 0; i < width; ++i) + { + for (int j = half_width_; j < last; ++j) + output (i,j) = convolveOneColDense (i,j); + + for (int j = last; j < height; ++j) + output (i,j) = output (i,h); + + for (int j = 0; j < half_width_; ++j) + output (i,j) = output (i, half_width_); + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int i = 0; i < width; ++i) + { + for (int j = half_width_; j < last; ++j) + output (i,j) = convolveOneColNonDense (i,j); + + for (int j = last; j < height; ++j) + output (i,j) = output (i,h); + + for (int j = 0; j < half_width_; ++j) + output (i,j) = output (i,half_width_); + } + } +} + +template void +pcl::filters::Convolution::convolve_cols_mirror (PointCloudOut& output) +{ + using namespace pcl::common; + + int width = input_->width; + int height = input_->height; + int last = input_->height - half_width_; + int h = last -1; + if (input_->is_dense) + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int i = 0; i < width; ++i) + { + for (int j = half_width_; j < last; ++j) + output (i,j) = convolveOneColDense (i,j); + + for (int j = last, l = 0; j < height; ++j, ++l) + output (i,j) = output (i,h-l); + + for (int j = 0; j < half_width_; ++j) + output (i,j) = output (i, half_width_+1-j); + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for(int i = 0; i < width; ++i) + { + for (int j = half_width_; j < last; ++j) + output (i,j) = convolveOneColNonDense (i,j); + + for (int j = last, l = 0; j < height; ++j, ++l) + output (i,j) = output (i,h-l); + + for (int j = 0; j < half_width_; ++j) + output (i,j) = output (i,half_width_+1-j); + } + } +} + +#endif //PCL_FILTERS_CONVOLUTION_IMPL_HPP diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/convolution_3d.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/convolution_3d.hpp new file mode 100755 index 0000000..2421dda --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/convolution_3d.hpp @@ -0,0 +1,265 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP +#define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP + +#include +#include +#include + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl +{ + namespace filters + { + template + class ConvolvingKernel + { + void + makeInfinite (pcl::Normal& n) + { + n.normal_x = n.normal_y = n.normal_z = std::numeric_limits::quiet_NaN (); + } + }; + + template class + ConvolvingKernel + { + void + makeInfinite (pcl::PointXY& p) + { + p.x = p.y = std::numeric_limits::quiet_NaN (); + } + }; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::filters::GaussianKernel::initCompute () +{ + if (sigma_ == 0) + { + PCL_ERROR ("Sigma is not set or equal to 0!\n", sigma_); + return (false); + } + sigma_sqr_ = sigma_ * sigma_; + + if (sigma_coefficient_) + { + if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3) + { + PCL_ERROR ("Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_)); + return (false); + } + else + threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_; + } + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template PointOutT +pcl::filters::GaussianKernel::operator() (const std::vector& indices, + const std::vector& distances) +{ + using namespace pcl::common; + PointOutT result; + float total_weight = 0; + std::vector::const_iterator dist_it = distances.begin (); + + for (std::vector::const_iterator idx_it = indices.begin (); + idx_it != indices.end (); + ++idx_it, ++dist_it) + { + if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it])) + { + float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_); + result += weight * (*input_) [*idx_it]; + total_weight += weight; + } + } + if (total_weight != 0) + result /= total_weight; + else + makeInfinite (result); + + return (result); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////// +template PointOutT +pcl::filters::GaussianKernelRGB::operator() (const std::vector& indices, const std::vector& distances) +{ + using namespace pcl::common; + PointOutT result; + float total_weight = 0; + float r = 0, g = 0, b = 0; + std::vector::const_iterator dist_it = distances.begin (); + + for (std::vector::const_iterator idx_it = indices.begin (); + idx_it != indices.end (); + ++idx_it, ++dist_it) + { + if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it])) + { + float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_); + result.x += weight * (*input_) [*idx_it].x; + result.y += weight * (*input_) [*idx_it].y; + result.z += weight * (*input_) [*idx_it].z; + r += weight * static_cast ((*input_) [*idx_it].r); + g += weight * static_cast ((*input_) [*idx_it].g); + b += weight * static_cast ((*input_) [*idx_it].b); + total_weight += weight; + } + } + if (total_weight != 0) + { + total_weight = 1.f/total_weight; + r*= total_weight; g*= total_weight; b*= total_weight; + result.x*= total_weight; result.y*= total_weight; result.z*= total_weight; + result.r = static_cast (r); + result.g = static_cast (g); + result.b = static_cast (b); + } + else + makeInfinite (result); + + return (result); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::filters::Convolution3D::Convolution3D () + : PCLBase () + , surface_ () + , tree_ () + , search_radius_ (0) +{} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::filters::Convolution3D::initCompute () +{ + if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed!\n"); + return (false); + } + // Initialize the spatial locator + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + // If no search surface has been defined, use the input dataset as the search surface itself + if (!surface_) + surface_ = input_; + // Send the surface dataset to the spatial locator + tree_->setInputCloud (surface_); + // Do a fast check to see if the search parameters are well defined + if (search_radius_ <= 0.0) + { + PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0", + search_radius_); + return (false); + } + // Make sure the provided kernel implements the required interface + if (dynamic_cast* > (&kernel_) == 0) + { + PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed"); + PCL_ERROR ("kernel_ must implement ConvolvingKernel interface\n!"); + return (false); + } + kernel_.setInputCloud (surface_); + // Initialize convolving kernel + if (!kernel_.initCompute ()) + { + PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n"); + return (false); + } + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::filters::Convolution3D::convolve (PointCloudOut& output) +{ + if (!initCompute ()) + { + PCL_ERROR ("[pcl::filters::Convlution3D::convolve] init failed!\n"); + return; + } + output.resize (surface_->size ()); + output.width = surface_->width; + output.height = surface_->height; + output.is_dense = surface_->is_dense; + std::vector nn_indices; + std::vector nn_distances; + +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_) +#endif + for (std::int64_t point_idx = 0; point_idx < static_cast (surface_->size ()); ++point_idx) + { + const PointInT& point_in = surface_->points [point_idx]; + PointOutT& point_out = output [point_idx]; + if (isFinite (point_in) && + tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances)) + { + point_out = kernel_ (nn_indices, nn_distances); + } + else + { + kernel_.makeInfinite (point_out); + output.is_dense = false; + } + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/covariance_sampling.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/covariance_sampling.hpp new file mode 100755 index 0000000..4e17ec1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/covariance_sampling.hpp @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ +#define PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ + +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////// +template bool +pcl::CovarianceSampling::initCompute () +{ + if (!FilterIndices::initCompute ()) + return false; + + if (num_samples_ > indices_->size ()) + { + PCL_ERROR ("[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%lu)\n", + num_samples_, indices_->size ()); + return false; + } + + // Prepare the point cloud by centering at the origin and then scaling the points such that the average distance from + // the origin is 1.0 => rotations and translations will have the same magnitude + Eigen::Vector3f centroid (0.f, 0.f, 0.f); + for (std::size_t p_i = 0; p_i < indices_->size (); ++p_i) + centroid += (*input_)[(*indices_)[p_i]].getVector3fMap (); + centroid /= float (indices_->size ()); + + scaled_points_.resize (indices_->size ()); + double average_norm = 0.0; + for (std::size_t p_i = 0; p_i < indices_->size (); ++p_i) + { + scaled_points_[p_i] = (*input_)[(*indices_)[p_i]].getVector3fMap () - centroid; + average_norm += scaled_points_[p_i].norm (); + } + average_norm /= double (scaled_points_.size ()); + for (std::size_t p_i = 0; p_i < scaled_points_.size (); ++p_i) + scaled_points_[p_i] /= float (average_norm); + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////// +template double +pcl::CovarianceSampling::computeConditionNumber () +{ + Eigen::Matrix covariance_matrix; + if (!computeCovarianceMatrix (covariance_matrix)) + return (-1.); + + return computeConditionNumber (covariance_matrix); +} + + +/////////////////////////////////////////////////////////////////////////////// +template double +pcl::CovarianceSampling::computeConditionNumber (const Eigen::Matrix &covariance_matrix) +{ + const Eigen::SelfAdjointEigenSolver > solver (covariance_matrix, Eigen::EigenvaluesOnly); + const double max_ev = solver.eigenvalues (). maxCoeff (); + const double min_ev = solver.eigenvalues (). minCoeff (); + return (max_ev / min_ev); +} + + +/////////////////////////////////////////////////////////////////////////////// +template bool +pcl::CovarianceSampling::computeCovarianceMatrix (Eigen::Matrix &covariance_matrix) +{ + if (!initCompute ()) + return false; + + //--- Part A from the paper + // Set up matrix F + Eigen::Matrix f_mat = Eigen::Matrix (6, indices_->size ()); + for (std::size_t p_i = 0; p_i < scaled_points_.size (); ++p_i) + { + f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross ( + (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast (); + f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast (); + } + + // Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix) + covariance_matrix = f_mat * f_mat.transpose (); + return true; +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::CovarianceSampling::applyFilter (std::vector &sampled_indices) +{ + Eigen::Matrix c_mat; + // Invokes initCompute() + if (!computeCovarianceMatrix (c_mat)) + return; + + const Eigen::SelfAdjointEigenSolver > solver (c_mat); + const Eigen::Matrix x = solver.eigenvectors (); + + //--- Part B from the paper + /// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs + std::vector candidate_indices; + candidate_indices.resize (indices_->size ()); + for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i) + candidate_indices[p_i] = p_i; + + // Compute the v 6-vectors + using Vector6d = Eigen::Matrix; + std::vector > v; + v.resize (candidate_indices.size ()); + for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i) + { + v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross ( + (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast (); + v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast (); + } + + + // Set up the lists to be sorted + std::vector > > L; + L.resize (6); + + for (std::size_t i = 0; i < 6; ++i) + { + for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i) + L[i].push_back (std::make_pair (p_i, std::abs (v[p_i].dot (x.block<6, 1> (0, i))))); + + // Sort in decreasing order + L[i].sort (sort_dot_list_function); + } + + // Initialize the 6 t's + std::vector t (6, 0.0); + + sampled_indices.resize (num_samples_); + std::vector point_sampled (candidate_indices.size (), false); + // Now select the actual points + for (std::size_t sample_i = 0; sample_i < num_samples_; ++sample_i) + { + // Find the most unconstrained dimension, i.e., the minimum t + std::size_t min_t_i = 0; + for (std::size_t i = 0; i < 6; ++i) + { + if (t[min_t_i] > t[i]) + min_t_i = i; + } + + // Add the point from the top of the list corresponding to the dimension to the set of samples + while (point_sampled [L[min_t_i].front ().first]) + L[min_t_i].pop_front (); + + sampled_indices[sample_i] = L[min_t_i].front ().first; + point_sampled[L[min_t_i].front ().first] = true; + L[min_t_i].pop_front (); + + // Update the running totals + for (std::size_t i = 0; i < 6; ++i) + { + double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i)); + t[i] += val * val; + } + } + + // Remap the sampled_indices to the input_ cloud + for (int &sampled_index : sampled_indices) + sampled_index = (*indices_)[candidate_indices[sampled_index]]; +} + + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::CovarianceSampling::applyFilter (Cloud &output) +{ + std::vector sampled_indices; + applyFilter (sampled_indices); + + output.resize (sampled_indices.size ()); + output.header = input_->header; + output.height = 1; + output.width = std::uint32_t (output.size ()); + output.is_dense = true; + for (std::size_t i = 0; i < sampled_indices.size (); ++i) + output[i] = (*input_)[sampled_indices[i]]; +} + + +#define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling; + +#endif /* PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/crop_box.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/crop_box.hpp new file mode 100755 index 0000000..97f5aac --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/crop_box.hpp @@ -0,0 +1,146 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2015, Google, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: extract_indices.hpp 1897 2011-07-26 20:35:49Z rusu $ + * + */ + +#ifndef PCL_FILTERS_IMPL_CROP_BOX_H_ +#define PCL_FILTERS_IMPL_CROP_BOX_H_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::CropBox::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilter (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (const auto ri : *removed_indices_) // ri = removed index + output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + output.is_dense = true; + applyFilter (indices); + pcl::copyPointCloud (*input_, indices, output); + } +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::CropBox::applyFilter (std::vector &indices) +{ + indices.resize (input_->points.size ()); + removed_indices_->resize (input_->points.size ()); + int indices_count = 0; + int removed_indices_count = 0; + + Eigen::Affine3f transform = Eigen::Affine3f::Identity (); + Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity (); + + if (rotation_ != Eigen::Vector3f::Zero ()) + { + pcl::getTransformation (0, 0, 0, + rotation_ (0), rotation_ (1), rotation_ (2), + transform); + inverse_transform = transform.inverse (); + } + + bool transform_matrix_is_identity = transform_.matrix ().isIdentity (); + bool translation_is_zero = (translation_ == Eigen::Vector3f::Zero ()); + bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity (); + + for (const auto index : *indices_) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!isFinite (input_->points[index])) + continue; + + // Get local point + PointT local_pt = input_->points[index]; + + // Transform point to world space + if (!transform_matrix_is_identity) + local_pt = pcl::transformPoint (local_pt, transform_); + + if (!translation_is_zero) + { + local_pt.x -= translation_ (0); + local_pt.y -= translation_ (1); + local_pt.z -= translation_ (2); + } + + // Transform point to local space of crop box + if (!inverse_transform_matrix_is_identity) + local_pt = pcl::transformPoint (local_pt, inverse_transform); + + // If outside the cropbox + if ( (local_pt.x < min_pt_[0] || local_pt.y < min_pt_[1] || local_pt.z < min_pt_[2]) || + (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2])) + { + if (negative_) + indices[indices_count++] = index; + else if (extract_removed_indices_) + (*removed_indices_)[removed_indices_count++] = static_cast (index); + } + // If inside the cropbox + else + { + if (negative_ && extract_removed_indices_) + (*removed_indices_)[removed_indices_count++] = static_cast (index); + else if (!negative_) + indices[indices_count++] = index; + } + } + indices.resize (indices_count); + removed_indices_->resize (removed_indices_count); +} + +#define PCL_INSTANTIATE_CropBox(T) template class PCL_EXPORTS pcl::CropBox; + +#endif // PCL_FILTERS_IMPL_CROP_BOX_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/crop_hull.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/crop_hull.hpp new file mode 100755 index 0000000..e0ce3fe --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/crop_hull.hpp @@ -0,0 +1,329 @@ + /* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FILTERS_IMPL_CROP_HULL_H_ +#define PCL_FILTERS_IMPL_CROP_HULL_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CropHull::applyFilter (PointCloud &output) +{ + if (dim_ == 2) + { + // in this case we are assuming all the points lie in the same plane as the + // 2D convex hull, so the choice of projection just changes the + // conditioning of the problem: choose to squash the XYZ component of the + // hull-points that has least variation - this will also give reasonable + // results if the points don't lie exactly in the same plane + const Eigen::Vector3f range = getHullCloudRange (); + if (range[0] <= range[1] && range[0] <= range[2]) + applyFilter2D<1,2> (output); + else if (range[1] <= range[2] && range[1] <= range[0]) + applyFilter2D<2,0> (output); + else + applyFilter2D<0,1> (output); + } + else + { + applyFilter3D (output); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CropHull::applyFilter (std::vector &indices) +{ + if (dim_ == 2) + { + // in this case we are assuming all the points lie in the same plane as the + // 2D convex hull, so the choice of projection just changes the + // conditioning of the problem: choose to squash the XYZ component of the + // hull-points that has least variation - this will also give reasonable + // results if the points don't lie exactly in the same plane + const Eigen::Vector3f range = getHullCloudRange (); + if (range[0] <= range[1] && range[0] <= range[2]) + applyFilter2D<1,2> (indices); + else if (range[1] <= range[2] && range[1] <= range[0]) + applyFilter2D<2,0> (indices); + else + applyFilter2D<0,1> (indices); + } + else + { + applyFilter3D (indices); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::Vector3f +pcl::CropHull::getHullCloudRange () +{ + Eigen::Vector3f cloud_min ( + std::numeric_limits::max (), + std::numeric_limits::max (), + std::numeric_limits::max () + ); + Eigen::Vector3f cloud_max ( + -std::numeric_limits::max (), + -std::numeric_limits::max (), + -std::numeric_limits::max () + ); + for (std::size_t index = 0; index < indices_->size (); index++) + { + Eigen::Vector3f pt = input_->points[(*indices_)[index]].getVector3fMap (); + for (int i = 0; i < 3; i++) + { + if (pt[i] < cloud_min[i]) cloud_min[i] = pt[i]; + if (pt[i] > cloud_max[i]) cloud_max[i] = pt[i]; + } + } + + return (cloud_max - cloud_min); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template template void +pcl::CropHull::applyFilter2D (PointCloud &output) +{ + for (std::size_t index = 0; index < indices_->size (); index++) + { + // iterate over polygons faster than points because we expect this data + // to be, in general, more cache-local - the point cloud might be huge + std::size_t poly; + for (poly = 0; poly < hull_polygons_.size (); poly++) + { + if (isPointIn2DPolyWithVertIndices ( + input_->points[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_ + )) + { + if (crop_outside_) + output.push_back (input_->points[(*indices_)[index]]); + // once a point has tested +ve for being inside one polygon, we can + // stop checking the others: + break; + } + } + // If we're removing points *inside* the hull, only remove points that + // haven't been found inside any polygons + if (poly == hull_polygons_.size () && !crop_outside_) + output.push_back (input_->points[(*indices_)[index]]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template template void +pcl::CropHull::applyFilter2D (std::vector &indices) +{ + // see comments in (PointCloud& output) overload + for (std::size_t index = 0; index < indices_->size (); index++) + { + std::size_t poly; + for (poly = 0; poly < hull_polygons_.size (); poly++) + { + if (isPointIn2DPolyWithVertIndices ( + input_->points[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_ + )) + { + if (crop_outside_) + indices.push_back ((*indices_)[index]); + break; + } + } + if (poly == hull_polygons_.size () && !crop_outside_) + indices.push_back ((*indices_)[index]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CropHull::applyFilter3D (PointCloud &output) +{ + // This algorithm could definitely be sped up using kdtree/octree + // information, if that is available! + + for (std::size_t index = 0; index < indices_->size (); index++) + { + // test ray-crossings for three random rays, and take vote of crossings + // counts to determine if each point is inside the hull: the vote avoids + // tricky edge and corner cases when rays might fluke through the edge + // between two polygons + // 'random' rays are arbitrary - basically anything that is less likely to + // hit the edge between polygons than coordinate-axis aligned rays would + // be. + std::size_t crossings[3] = {0,0,0}; + Eigen::Vector3f rays[3] = + { + Eigen::Vector3f (0.264882f, 0.688399f, 0.675237f), + Eigen::Vector3f (0.0145419f, 0.732901f, 0.68018f), + Eigen::Vector3f (0.856514f, 0.508771f, 0.0868081f) + }; + + for (std::size_t poly = 0; poly < hull_polygons_.size (); poly++) + for (std::size_t ray = 0; ray < 3; ray++) + crossings[ray] += rayTriangleIntersect + (input_->points[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_); + + if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1) + output.push_back (input_->points[(*indices_)[index]]); + else if (!crop_outside_) + output.push_back (input_->points[(*indices_)[index]]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CropHull::applyFilter3D (std::vector &indices) +{ + // see comments in applyFilter3D (PointCloud& output) + for (std::size_t index = 0; index < indices_->size (); index++) + { + std::size_t crossings[3] = {0,0,0}; + Eigen::Vector3f rays[3] = + { + Eigen::Vector3f(0.264882f, 0.688399f, 0.675237f), + Eigen::Vector3f(0.0145419f, 0.732901f, 0.68018f), + Eigen::Vector3f(0.856514f, 0.508771f, 0.0868081f) + }; + + for (std::size_t poly = 0; poly < hull_polygons_.size (); poly++) + for (std::size_t ray = 0; ray < 3; ray++) + crossings[ray] += rayTriangleIntersect + (input_->points[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_); + + if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1) + indices.push_back ((*indices_)[index]); + else if (!crop_outside_) + indices.push_back ((*indices_)[index]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template template bool +pcl::CropHull::isPointIn2DPolyWithVertIndices ( + const PointT& point, const Vertices& verts, const PointCloud& cloud) +{ + bool in_poly = false; + double x1, x2, y1, y2; + + const int nr_poly_points = static_cast(verts.vertices.size ()); + double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1]; + double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2]; + for (int i = 0; i < nr_poly_points; i++) + { + const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1]; + const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2]; + if (xnew > xold) + { + x1 = xold; + x2 = xnew; + y1 = yold; + y2 = ynew; + } + else + { + x1 = xnew; + x2 = xold; + y1 = ynew; + y2 = yold; + } + + if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) && + (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1)) + { + in_poly = !in_poly; + } + xold = xnew; + yold = ynew; + } + + return (in_poly); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::CropHull::rayTriangleIntersect (const PointT& point, + const Eigen::Vector3f& ray, + const Vertices& verts, + const PointCloud& cloud) +{ + // Algorithm here is adapted from: + // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle() + // + // Original copyright notice: + // Copyright 2001, softSurfer (www.softsurfer.com) + // This code may be freely used and modified for any purpose + // providing that this copyright notice is included with it. + // + assert (verts.vertices.size () == 3); + + const Eigen::Vector3f p = point.getVector3fMap (); + const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap (); + const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap (); + const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap (); + const Eigen::Vector3f u = b - a; + const Eigen::Vector3f v = c - a; + const Eigen::Vector3f n = u.cross (v); + const float n_dot_ray = n.dot (ray); + + if (std::fabs (n_dot_ray) < 1e-9) + return (false); + + const float r = n.dot (a - p) / n_dot_ray; + + if (r < 0) + return (false); + + const Eigen::Vector3f w = p + r * ray - a; + const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v); + const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u); + const float s = s_numerator / denominator; + if (s < 0 || s > 1) + return (false); + + const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v); + const float t = t_numerator / denominator; + if (t < 0 || s+t > 1) + return (false); + + return (true); +} + +#define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull; + +#endif // PCL_FILTERS_IMPL_CROP_HULL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/extract_indices.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/extract_indices.hpp new file mode 100755 index 0000000..7005fde --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/extract_indices.hpp @@ -0,0 +1,171 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_ +#define PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_ + +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ExtractIndices::filterDirectly (PointCloudPtr &cloud) +{ + std::vector indices; + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + this->setInputCloud (cloud); + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + std::vector fields; + pcl::for_each_type (pcl::detail::FieldAdder (fields)); + for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator + { + std::size_t pt_index = (std::size_t) (*removed_indices_)[rii]; + if (pt_index >= input_->points.size ()) + { + PCL_ERROR ("[pcl::%s::filterDirectly] The index exceeds the size of the input. Do nothing.\n", + getClassName ().c_str ()); + *cloud = *input_; + return; + } + std::uint8_t* pt_data = reinterpret_cast (&cloud->points[pt_index]); + for (const auto &field : fields) + memcpy (pt_data + field.offset, &user_filter_value_, sizeof (float)); + } + if (!std::isfinite (user_filter_value_)) + cloud->is_dense = false; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ExtractIndices::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + output = *input_; + std::vector fields; + pcl::for_each_type (pcl::detail::FieldAdder (fields)); + for (const auto ri : *removed_indices_) // ri = removed index + { + std::size_t pt_index = (std::size_t)ri; + if (pt_index >= input_->points.size ()) + { + PCL_ERROR ("[pcl::%s::applyFilter] The index exceeds the size of the input. Do nothing.\n", + getClassName ().c_str ()); + output = *input_; + return; + } + std::uint8_t* pt_data = reinterpret_cast (&output.points[pt_index]); + for (const auto &field : fields) + memcpy (pt_data + field.offset, &user_filter_value_, sizeof (float)); + } + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + applyFilterIndices (indices); + copyPointCloud (*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ExtractIndices::applyFilterIndices (std::vector &indices) +{ + if (indices_->size () > input_->points.size ()) + { + PCL_ERROR ("[pcl::%s::applyFilter] The indices size exceeds the size of the input.\n", getClassName ().c_str ()); + indices.clear (); + removed_indices_->clear (); + return; + } + + if (!negative_) // Normal functionality + { + indices = *indices_; + + if (extract_removed_indices_) + { + // Set up the full indices set + std::vector full_indices (input_->points.size ()); + for (int fii = 0; fii < static_cast (full_indices.size ()); ++fii) // fii = full indices iterator + full_indices[fii] = fii; + + // Set up the sorted input indices + std::vector sorted_input_indices = *indices_; + std::sort (sorted_input_indices.begin (), sorted_input_indices.end ()); + + // Store the difference in removed_indices + removed_indices_->clear (); + set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (*removed_indices_, removed_indices_->begin ())); + } + } + else // Inverted functionality + { + // Set up the full indices set + std::vector full_indices (input_->points.size ()); + for (int fii = 0; fii < static_cast (full_indices.size ()); ++fii) // fii = full indices iterator + full_indices[fii] = fii; + + // Set up the sorted input indices + std::vector sorted_input_indices = *indices_; + std::sort (sorted_input_indices.begin (), sorted_input_indices.end ()); + + // Store the difference in indices + indices.clear (); + set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (indices, indices.begin ())); + + if (extract_removed_indices_) + removed_indices_ = indices_; + } +} + +#define PCL_INSTANTIATE_ExtractIndices(T) template class PCL_EXPORTS pcl::ExtractIndices; + +#endif // PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/fast_bilateral.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/fast_bilateral.hpp new file mode 100755 index 0000000..bddfc65 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/fast_bilateral.hpp @@ -0,0 +1,215 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2004, Sylvain Paris and Francois Sillion + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ +#define PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FastBilateralFilter::applyFilter (PointCloud &output) +{ + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::FastBilateralFilter] Input cloud needs to be organized.\n"); + return; + } + + copyPointCloud (*input_, output); + float base_max = -std::numeric_limits::max (), + base_min = std::numeric_limits::max (); + bool found_finite = false; + for (std::size_t x = 0; x < output.width; ++x) + { + for (std::size_t y = 0; y < output.height; ++y) + { + if (std::isfinite (output (x, y).z)) + { + if (base_max < output (x, y).z) + base_max = output (x, y).z; + if (base_min > output (x, y).z) + base_min = output (x, y).z; + found_finite = true; + } + } + } + if (!found_finite) + { + PCL_WARN ("[pcl::FastBilateralFilter] Given an empty cloud. Doing nothing.\n"); + return; + } + + for (std::size_t x = 0; x < output.width; ++x) + for (std::size_t y = 0; y < output.height; ++y) + if (!std::isfinite (output (x, y).z)) + output (x, y).z = base_max; + + const float base_delta = base_max - base_min; + + const std::size_t padding_xy = 2; + const std::size_t padding_z = 2; + + const std::size_t small_width = static_cast (static_cast (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy; + const std::size_t small_height = static_cast (static_cast (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy; + const std::size_t small_depth = static_cast (base_delta / sigma_r_) + 1 + 2 * padding_z; + + + Array3D data (small_width, small_height, small_depth); + for (std::size_t x = 0; x < input_->width; ++x) + { + const std::size_t small_x = static_cast (static_cast (x) / sigma_s_ + 0.5f) + padding_xy; + for (std::size_t y = 0; y < input_->height; ++y) + { + const float z = output (x,y).z - base_min; + + const std::size_t small_y = static_cast (static_cast (y) / sigma_s_ + 0.5f) + padding_xy; + const std::size_t small_z = static_cast (static_cast (z) / sigma_r_ + 0.5f) + padding_z; + + Eigen::Vector2f& d = data (small_x, small_y, small_z); + d[0] += output (x,y).z; + d[1] += 1.0f; + } + } + + + std::vector offset (3); + offset[0] = &(data (1,0,0)) - &(data (0,0,0)); + offset[1] = &(data (0,1,0)) - &(data (0,0,0)); + offset[2] = &(data (0,0,1)) - &(data (0,0,0)); + + Array3D buffer (small_width, small_height, small_depth); + + for (std::size_t dim = 0; dim < 3; ++dim) + { + const long int off = offset[dim]; + for (std::size_t n_iter = 0; n_iter < 2; ++n_iter) + { + std::swap (buffer, data); + for(std::size_t x = 1; x < small_width - 1; ++x) + for(std::size_t y = 1; y < small_height - 1; ++y) + { + Eigen::Vector2f* d_ptr = &(data (x,y,1)); + Eigen::Vector2f* b_ptr = &(buffer (x,y,1)); + + for(std::size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr) + *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0; + } + } + } + + if (early_division_) + { + for (std::vector >::iterator d = data.begin (); d != data.end (); ++d) + *d /= ((*d)[0] != 0) ? (*d)[1] : 1; + + for (std::size_t x = 0; x < input_->width; x++) + for (std::size_t y = 0; y < input_->height; y++) + { + const float z = output (x,y).z - base_min; + const Eigen::Vector2f D = data.trilinear_interpolation (static_cast (x) / sigma_s_ + padding_xy, + static_cast (y) / sigma_s_ + padding_xy, + z / sigma_r_ + padding_z); + output(x,y).z = D[0]; + } + } + else + { + for (std::size_t x = 0; x < input_->width; ++x) + for (std::size_t y = 0; y < input_->height; ++y) + { + const float z = output (x,y).z - base_min; + const Eigen::Vector2f D = data.trilinear_interpolation (static_cast (x) / sigma_s_ + padding_xy, + static_cast (y) / sigma_s_ + padding_xy, + z / sigma_r_ + padding_z); + output (x,y).z = D[0] / D[1]; + } + } +} + + + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::FastBilateralFilter::Array3D::clamp (const std::size_t min_value, + const std::size_t max_value, + const std::size_t x) +{ + if (x >= min_value && x <= max_value) + { + return x; + } + if (x < min_value) + { + return (min_value); + } + return (max_value); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::Vector2f +pcl::FastBilateralFilter::Array3D::trilinear_interpolation (const float x, + const float y, + const float z) +{ + const std::size_t x_index = clamp (0, x_dim_ - 1, static_cast (x)); + const std::size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1); + + const std::size_t y_index = clamp (0, y_dim_ - 1, static_cast (y)); + const std::size_t yy_index = clamp (0, y_dim_ - 1, y_index + 1); + + const std::size_t z_index = clamp (0, z_dim_ - 1, static_cast (z)); + const std::size_t zz_index = clamp (0, z_dim_ - 1, z_index + 1); + + const float x_alpha = x - static_cast (x_index); + const float y_alpha = y - static_cast (y_index); + const float z_alpha = z - static_cast (z_index); + + return + (1.0f-x_alpha) * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(x_index, y_index, z_index) + + x_alpha * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(xx_index, y_index, z_index) + + (1.0f-x_alpha) * y_alpha * (1.0f-z_alpha) * (*this)(x_index, yy_index, z_index) + + x_alpha * y_alpha * (1.0f-z_alpha) * (*this)(xx_index, yy_index, z_index) + + (1.0f-x_alpha) * (1.0f-y_alpha) * z_alpha * (*this)(x_index, y_index, zz_index) + + x_alpha * (1.0f-y_alpha) * z_alpha * (*this)(xx_index, y_index, zz_index) + + (1.0f-x_alpha) * y_alpha * z_alpha * (*this)(x_index, yy_index, zz_index) + + x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index); +} + +#endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/fast_bilateral_omp.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/fast_bilateral_omp.hpp new file mode 100755 index 0000000..d56d465 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/fast_bilateral_omp.hpp @@ -0,0 +1,212 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2004, Sylvain Paris and Francois Sillion + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: fast_bilateral_omp.hpp 8381 2013-01-02 23:12:44Z sdmiller $ + * + */ +#ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ +#define PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FastBilateralFilterOMP::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::FastBilateralFilterOMP::applyFilter (PointCloud &output) +{ + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::FastBilateralFilterOMP] Input cloud needs to be organized.\n"); + return; + } + + copyPointCloud (*input_, output); + float base_max = -std::numeric_limits::max (), + base_min = std::numeric_limits::max (); + bool found_finite = false; + for (std::size_t x = 0; x < output.width; ++x) + { + for (std::size_t y = 0; y < output.height; ++y) + { + if (std::isfinite (output (x, y).z)) + { + if (base_max < output (x, y).z) + base_max = output (x, y).z; + if (base_min > output (x, y).z) + base_min = output (x, y).z; + found_finite = true; + } + } + } + if (!found_finite) + { + PCL_WARN ("[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n"); + return; + } +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for (long int i = 0; i < static_cast (output.size ()); ++i) + if (!std::isfinite (output.at(i).z)) + output.at(i).z = base_max; + + const float base_delta = base_max - base_min; + + const std::size_t padding_xy = 2; + const std::size_t padding_z = 2; + + const std::size_t small_width = static_cast (static_cast (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy; + const std::size_t small_height = static_cast (static_cast (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy; + const std::size_t small_depth = static_cast (base_delta / sigma_r_) + 1 + 2 * padding_z; + + Array3D data (small_width, small_height, small_depth); +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for (long int i = 0; i < static_cast (small_width * small_height); ++i) + { + std::size_t small_x = static_cast (i % small_width); + std::size_t small_y = static_cast (i / small_width); + std::size_t start_x = static_cast( + std::max ((static_cast (small_x) - static_cast (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f)); + std::size_t end_x = static_cast( + std::max ((static_cast (small_x) - static_cast (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f)); + std::size_t start_y = static_cast( + std::max ((static_cast (small_y) - static_cast (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f)); + std::size_t end_y = static_cast( + std::max ((static_cast (small_y) - static_cast (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f)); + for (std::size_t x = start_x; x < end_x && x < input_->width; ++x) + { + for (std::size_t y = start_y; y < end_y && y < input_->height; ++y) + { + const float z = output (x,y).z - base_min; + const std::size_t small_z = static_cast (static_cast (z) / sigma_r_ + 0.5f) + padding_z; + Eigen::Vector2f& d = data (small_x, small_y, small_z); + d[0] += output (x,y).z; + d[1] += 1.0f; + } + } + } + + std::vector offset (3); + offset[0] = &(data (1,0,0)) - &(data (0,0,0)); + offset[1] = &(data (0,1,0)) - &(data (0,0,0)); + offset[2] = &(data (0,0,1)) - &(data (0,0,0)); + + Array3D buffer (small_width, small_height, small_depth); + + for (std::size_t dim = 0; dim < 3; ++dim) + { + for (std::size_t n_iter = 0; n_iter < 2; ++n_iter) + { + Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data); + Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer); +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(long int i = 0; i < static_cast ((small_width - 2)*(small_height - 2)); ++i) + { + std::size_t x = static_cast (i % (small_width - 2) + 1); + std::size_t y = static_cast (i / (small_width - 2) + 1); + const long int off = offset[dim]; + Eigen::Vector2f* d_ptr = &(current_data->operator() (x,y,1)); + Eigen::Vector2f* b_ptr = &(current_buffer->operator() (x,y,1)); + + for(std::size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr) + *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0; + } + } + } + // Note: this works because there are an even number of iterations. + // If there were an odd number, we would need to end with a: + // std::swap (data, buffer); + + if (early_division_) + { + for (std::vector >::iterator d = data.begin (); d != data.end (); ++d) + *d /= ((*d)[0] != 0) ? (*d)[1] : 1; + +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for (long int i = 0; i < static_cast (input_->size ()); ++i) + { + std::size_t x = static_cast (i % input_->width); + std::size_t y = static_cast (i / input_->width); + const float z = output (x,y).z - base_min; + const Eigen::Vector2f D = data.trilinear_interpolation (static_cast (x) / sigma_s_ + padding_xy, + static_cast (y) / sigma_s_ + padding_xy, + z / sigma_r_ + padding_z); + output(x,y).z = D[0]; + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for (long i = 0; i < static_cast (input_->size ()); ++i) + { + std::size_t x = static_cast (i % input_->width); + std::size_t y = static_cast (i / input_->width); + const float z = output (x,y).z - base_min; + const Eigen::Vector2f D = data.trilinear_interpolation (static_cast (x) / sigma_s_ + padding_xy, + static_cast (y) / sigma_s_ + padding_xy, + z / sigma_r_ + padding_z); + output (x,y).z = D[0] / D[1]; + } + } +} + + + +#endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ */ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/filter.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/filter.hpp new file mode 100755 index 0000000..03d17c1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/filter.hpp @@ -0,0 +1,146 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_FILTER_H_ +#define PCL_FILTERS_IMPL_FILTER_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////// +template void +pcl::removeNaNFromPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + std::vector &index) +{ + // If the clouds are not the same, prepare the output + if (&cloud_in != &cloud_out) + { + cloud_out.header = cloud_in.header; + cloud_out.points.resize (cloud_in.points.size ()); + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + } + // Reserve enough space for the indices + index.resize (cloud_in.points.size ()); + + // If the data is dense, we don't need to check for NaN + if (cloud_in.is_dense) + { + // Simply copy the data + cloud_out = cloud_in; + for (std::size_t j = 0; j < cloud_out.points.size (); ++j) + index[j] = static_cast(j); + } + else + { + std::size_t j = 0; + for (std::size_t i = 0; i < cloud_in.points.size (); ++i) + { + if (!std::isfinite (cloud_in.points[i].x) || + !std::isfinite (cloud_in.points[i].y) || + !std::isfinite (cloud_in.points[i].z)) + continue; + cloud_out.points[j] = cloud_in.points[i]; + index[j] = static_cast(i); + j++; + } + if (j != cloud_in.points.size ()) + { + // Resize to the correct size + cloud_out.points.resize (j); + index.resize (j); + } + + cloud_out.height = 1; + cloud_out.width = static_cast(j); + + // Removing bad points => dense (note: 'dense' doesn't mean 'organized') + cloud_out.is_dense = true; + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::removeNaNNormalsFromPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + std::vector &index) +{ + // If the clouds are not the same, prepare the output + if (&cloud_in != &cloud_out) + { + cloud_out.header = cloud_in.header; + cloud_out.points.resize (cloud_in.points.size ()); + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + } + // Reserve enough space for the indices + index.resize (cloud_in.points.size ()); + std::size_t j = 0; + + // Assume cloud is dense + cloud_out.is_dense = true; + + for (std::size_t i = 0; i < cloud_in.points.size (); ++i) + { + if (!std::isfinite (cloud_in.points[i].normal_x) || + !std::isfinite (cloud_in.points[i].normal_y) || + !std::isfinite (cloud_in.points[i].normal_z)) + continue; + if (cloud_out.is_dense && !pcl::isFinite(cloud_in.points[i])) + cloud_out.is_dense = false; + cloud_out.points[j] = cloud_in.points[i]; + index[j] = static_cast(i); + j++; + } + if (j != cloud_in.points.size ()) + { + // Resize to the correct size + cloud_out.points.resize (j); + index.resize (j); + } + + cloud_out.height = 1; + cloud_out.width = static_cast(j); +} + + +#define PCL_INSTANTIATE_removeNaNFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud(const pcl::PointCloud&, pcl::PointCloud&, std::vector&); +#define PCL_INSTANTIATE_removeNaNNormalsFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNNormalsFromPointCloud(const pcl::PointCloud&, pcl::PointCloud&, std::vector&); + +#endif // PCL_FILTERS_IMPL_FILTER_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/filter_indices.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/filter_indices.hpp new file mode 100755 index 0000000..60ada05 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/filter_indices.hpp @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: filter.hpp 1800 2011-07-15 11:45:31Z marton $ + * + */ + +#ifndef PCL_FILTERS_IMPL_FILTER_INDICES_H_ +#define PCL_FILTERS_IMPL_FILTER_INDICES_H_ + +#include +#include + +template void +pcl::removeNaNFromPointCloud (const pcl::PointCloud &cloud_in, + std::vector &index) +{ + // Reserve enough space for the indices + index.resize (cloud_in.points.size ()); + + // If the data is dense, we don't need to check for NaN + if (cloud_in.is_dense) + { + for (int j = 0; j < static_cast (cloud_in.points.size ()); ++j) + index[j] = j; + } + else + { + int j = 0; + for (int i = 0; i < static_cast (cloud_in.points.size ()); ++i) + { + if (!std::isfinite (cloud_in.points[i].x) || + !std::isfinite (cloud_in.points[i].y) || + !std::isfinite (cloud_in.points[i].z)) + continue; + index[j] = i; + j++; + } + if (j != static_cast (cloud_in.points.size ())) + { + // Resize to the correct size + index.resize (j); + } + } +} + +#define PCL_INSTANTIATE_removeNanFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud(const pcl::PointCloud&, std::vector&); + +#endif // PCL_FILTERS_IMPL_FILTER_INDICES_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/frustum_culling.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/frustum_culling.hpp new file mode 100755 index 0000000..d4e4ce5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/frustum_culling.hpp @@ -0,0 +1,181 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_ +#define PCL_FILTERS_IMPL_FRUSTUM_CULLING_HPP_ + +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::FrustumCulling::applyFilter (PointCloud& output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilter (indices); + extract_removed_indices_ = temp; + copyPointCloud (*input_, output); + + for (std::size_t rii = 0; rii < removed_indices_->size (); ++rii) + { + PointT &pt_to_remove = output.at ((*removed_indices_)[rii]); + pt_to_remove.x = pt_to_remove.y = pt_to_remove.z = user_filter_value_; + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + } + else + { + output.is_dense = true; + applyFilter (indices); + copyPointCloud (*input_, indices, output); + } +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::FrustumCulling::applyFilter (std::vector &indices) +{ + Eigen::Vector4f pl_n; // near plane + Eigen::Vector4f pl_f; // far plane + Eigen::Vector4f pl_t; // top plane + Eigen::Vector4f pl_b; // bottom plane + Eigen::Vector4f pl_r; // right plane + Eigen::Vector4f pl_l; // left plane + + Eigen::Vector3f view = camera_pose_.block<3, 1> (0, 0); // view vector for the camera - first column of the rotation matrix + Eigen::Vector3f up = camera_pose_.block<3, 1> (0, 1); // up vector for the camera - second column of the rotation matrix + Eigen::Vector3f right = camera_pose_.block<3, 1> (0, 2); // right vector for the camera - third column of the rotation matrix + Eigen::Vector3f T = camera_pose_.block<3, 1> (0, 3); // The (X, Y, Z) position of the camera w.r.t origin + + + float vfov_rad = float (vfov_ * M_PI / 180); // degrees to radians + float hfov_rad = float (hfov_ * M_PI / 180); // degrees to radians + + float np_h = float (2 * tan (vfov_rad / 2) * np_dist_); // near plane height + float np_w = float (2 * tan (hfov_rad / 2) * np_dist_); // near plane width + + float fp_h = float (2 * tan (vfov_rad / 2) * fp_dist_); // far plane height + float fp_w = float (2 * tan (hfov_rad / 2) * fp_dist_); // far plane width + + Eigen::Vector3f fp_c (T + view * fp_dist_); // far plane center + Eigen::Vector3f fp_tl (fp_c + (up * fp_h / 2) - (right * fp_w / 2)); // Top left corner of the far plane + Eigen::Vector3f fp_tr (fp_c + (up * fp_h / 2) + (right * fp_w / 2)); // Top right corner of the far plane + Eigen::Vector3f fp_bl (fp_c - (up * fp_h / 2) - (right * fp_w / 2)); // Bottom left corner of the far plane + Eigen::Vector3f fp_br (fp_c - (up * fp_h / 2) + (right * fp_w / 2)); // Bottom right corner of the far plane + + Eigen::Vector3f np_c (T + view * np_dist_); // near plane center + //Eigen::Vector3f np_tl = np_c + (up * np_h/2) - (right * np_w/2); // Top left corner of the near plane + Eigen::Vector3f np_tr (np_c + (up * np_h / 2) + (right * np_w / 2)); // Top right corner of the near plane + Eigen::Vector3f np_bl (np_c - (up * np_h / 2) - (right * np_w / 2)); // Bottom left corner of the near plane + Eigen::Vector3f np_br (np_c - (up * np_h / 2) + (right * np_w / 2)); // Bottom right corner of the near plane + + pl_f.head<3> () = (fp_bl - fp_br).cross (fp_tr - fp_br); // Far plane equation - cross product of the + pl_f (3) = -fp_c.dot (pl_f.head<3> ()); // perpendicular edges of the far plane + + pl_n.head<3> () = (np_tr - np_br).cross (np_bl - np_br); // Near plane equation - cross product of the + pl_n (3) = -np_c.dot (pl_n.head<3> ()); // perpendicular edges of the far plane + + Eigen::Vector3f a (fp_bl - T); // Vector connecting the camera and far plane bottom left + Eigen::Vector3f b (fp_br - T); // Vector connecting the camera and far plane bottom right + Eigen::Vector3f c (fp_tr - T); // Vector connecting the camera and far plane top right + Eigen::Vector3f d (fp_tl - T); // Vector connecting the camera and far plane top left + + // Frustum and the vectors a, b, c and d. T is the position of the camera + // _________ + // /| . | + // d / | c . | + // / | __._____| + // / / . . + // a <---/-/ . . + // / / . . b + // / . + // . + // T + // + + pl_r.head<3> () = b.cross (c); + pl_l.head<3> () = d.cross (a); + pl_t.head<3> () = c.cross (d); + pl_b.head<3> () = a.cross (b); + + pl_r (3) = -T.dot (pl_r.head<3> ()); + pl_l (3) = -T.dot (pl_l.head<3> ()); + pl_t (3) = -T.dot (pl_t.head<3> ()); + pl_b (3) = -T.dot (pl_b.head<3> ()); + + if (extract_removed_indices_) + { + removed_indices_->resize (indices_->size ()); + } + indices.resize (indices_->size ()); + std::size_t indices_ctr = 0; + std::size_t removed_ctr = 0; + for (std::size_t i = 0; i < indices_->size (); i++) + { + int idx = indices_->at (i); + Eigen::Vector4f pt (input_->points[idx].x, + input_->points[idx].y, + input_->points[idx].z, + 1.0f); + bool is_in_fov = (pt.dot (pl_l) <= 0) && + (pt.dot (pl_r) <= 0) && + (pt.dot (pl_t) <= 0) && + (pt.dot (pl_b) <= 0) && + (pt.dot (pl_f) <= 0) && + (pt.dot (pl_n) <= 0); + if (is_in_fov ^ negative_) + { + indices[indices_ctr++] = idx; + } + else if (extract_removed_indices_) + { + (*removed_indices_)[removed_ctr++] = idx; + } + } + indices.resize (indices_ctr); + removed_indices_->resize (removed_ctr); +} + +#define PCL_INSTANTIATE_FrustumCulling(T) template class PCL_EXPORTS pcl::FrustumCulling; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/grid_minimum.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/grid_minimum.hpp new file mode 100755 index 0000000..c30da59 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/grid_minimum.hpp @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ +#define PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ + +#include +#include +#include + +struct point_index_idx +{ + unsigned int idx; + unsigned int cloud_point_index; + + point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} + bool operator < (const point_index_idx &p) const { return (idx < p.idx); } +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridMinimum::applyFilter (PointCloud &output) +{ + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + std::vector indices; + + output.is_dense = true; + applyFilterIndices (indices); + pcl::copyPointCloud (*input_, indices, output); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridMinimum::applyFilterIndices (std::vector &indices) +{ + indices.resize (indices_->size ()); + int oii = 0; + + // Get the minimum and maximum dimensions + Eigen::Vector4f min_p, max_p; + getMinMax3D (*input_, *indices_, min_p, max_p); + + // Check that the resolution is not too small, given the size of the data + std::int64_t dx = static_cast ((max_p[0] - min_p[0]) * inverse_resolution_)+1; + std::int64_t dy = static_cast ((max_p[1] - min_p[1]) * inverse_resolution_)+1; + + if ((dx*dy) > static_cast (std::numeric_limits::max ())) + { + PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName ().c_str ()); + return; + } + + Eigen::Vector4i min_b, max_b, div_b, divb_mul; + + // Compute the minimum and maximum bounding box values + min_b[0] = static_cast (std::floor (min_p[0] * inverse_resolution_)); + max_b[0] = static_cast (std::floor (max_p[0] * inverse_resolution_)); + min_b[1] = static_cast (std::floor (min_p[1] * inverse_resolution_)); + max_b[1] = static_cast (std::floor (max_p[1] * inverse_resolution_)); + + // Compute the number of divisions needed along all axis + div_b = max_b - min_b + Eigen::Vector4i::Ones (); + div_b[3] = 0; + + // Set up the division multiplier + divb_mul = Eigen::Vector4i (1, div_b[0], 0, 0); + + std::vector index_vector; + index_vector.reserve (indices_->size ()); + + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!std::isfinite (input_->points[*it].x) || + !std::isfinite (input_->points[*it].y) || + !std::isfinite (input_->points[*it].z)) + continue; + + int ijk0 = static_cast (std::floor (input_->points[*it].x * inverse_resolution_) - static_cast (min_b[0])); + int ijk1 = static_cast (std::floor (input_->points[*it].y * inverse_resolution_) - static_cast (min_b[1])); + + // Compute the grid cell index + int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1]; + index_vector.emplace_back(static_cast (idx), *it); + } + + // Second pass: sort the index_vector vector using value representing target cell as index + // in effect all points belonging to the same output cell will be next to each other + std::sort (index_vector.begin (), index_vector.end (), std::less ()); + + // Third pass: count output cells + // we need to skip all the same, adjacenent idx values + unsigned int total = 0; + unsigned int index = 0; + + // first_and_last_indices_vector[i] represents the index in index_vector of the first point in + // index_vector belonging to the voxel which corresponds to the i-th output point, + // and of the first point not belonging to. + std::vector > first_and_last_indices_vector; + + // Worst case size + first_and_last_indices_vector.reserve (index_vector.size ()); + while (index < index_vector.size ()) + { + unsigned int i = index + 1; + while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx) + ++i; + ++total; + first_and_last_indices_vector.emplace_back(index, i); + index = i; + } + + // Fourth pass: locate grid minimums + indices.resize (total); + + index = 0; + + for (const auto &cp : first_and_last_indices_vector) + { + unsigned int first_index = cp.first; + unsigned int last_index = cp.second; + unsigned int min_index = index_vector[first_index].cloud_point_index; + float min_z = input_->points[index_vector[first_index].cloud_point_index].z; + + for (unsigned int i = first_index + 1; i < last_index; ++i) + { + if (input_->points[index_vector[i].cloud_point_index].z < min_z) + { + min_z = input_->points[index_vector[i].cloud_point_index].z; + min_index = index_vector[i].cloud_point_index; + } + } + + indices[index] = min_index; + + ++index; + } + + oii = indices.size (); + + // Resize the output arrays + indices.resize (oii); +} + +#define PCL_INSTANTIATE_GridMinimum(T) template class PCL_EXPORTS pcl::GridMinimum; + +#endif // PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/local_maximum.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/local_maximum.hpp new file mode 100755 index 0000000..4ba3d9e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/local_maximum.hpp @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ +#define PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LocalMaximum::applyFilter (PointCloud &output) +{ + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + std::vector indices; + + output.is_dense = true; + applyFilterIndices (indices); + pcl::copyPointCloud (*input_, indices, output); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LocalMaximum::applyFilterIndices (std::vector &indices) +{ + typename PointCloud::Ptr cloud_projected (new PointCloud); + + // Create a set of planar coefficients with X=Y=0,Z=1 + pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); + coefficients->values.resize (4); + coefficients->values[0] = coefficients->values[1] = 0; + coefficients->values[2] = 1.0; + coefficients->values[3] = 0; + + // Create the filtering object and project input into xy plane + pcl::ProjectInliers proj; + proj.setModelType (pcl::SACMODEL_PLANE); + proj.setInputCloud (input_); + proj.setModelCoefficients (coefficients); + proj.filter (*cloud_projected); + + // Initialize the search class + if (!searcher_) + { + if (input_->isOrganized ()) + searcher_.reset (new pcl::search::OrganizedNeighbor ()); + else + searcher_.reset (new pcl::search::KdTree (false)); + } + searcher_->setInputCloud (cloud_projected); + + // The arrays to be used + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + + std::vector point_is_max (indices_->size (), false); + std::vector point_is_visited (indices_->size (), false); + + // Find all points within xy radius (i.e., a vertical cylinder) of the query + // point, removing those that are locally maximal (i.e., highest z within the + // cylinder) + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) + { + if (!isFinite (input_->points[(*indices_)[iii]])) + { + continue; + } + + // Points in the neighborhood of a previously identified local max, will + // not be maximal in their own neighborhood + if (point_is_visited[(*indices_)[iii]] && !point_is_max[(*indices_)[iii]]) + { + continue; + } + + // Assume the current query point is the maximum, mark as visited + point_is_max[(*indices_)[iii]] = true; + point_is_visited[(*indices_)[iii]] = true; + + // Perform the radius search in the projected cloud + std::vector radius_indices; + std::vector radius_dists; + PointT p = cloud_projected->points[(*indices_)[iii]]; + if (searcher_->radiusSearch (p, radius_, radius_indices, radius_dists) == 0) + { + PCL_WARN ("[pcl::%s::applyFilter] Searching for neighbors within radius %f failed.\n", getClassName ().c_str (), radius_); + continue; + } + + // If query point is alone, we retain it regardless + if (radius_indices.size () == 1) + { + point_is_max[(*indices_)[iii]] = false; + } + + // Check to see if a neighbor is higher than the query point + float query_z = input_->points[(*indices_)[iii]].z; + for (std::size_t k = 1; k < radius_indices.size (); ++k) // k = 1 is the first neighbor + { + if (input_->points[radius_indices[k]].z > query_z) + { + // Query point is not the local max, no need to check others + point_is_max[(*indices_)[iii]] = false; + break; + } + } + + // If the query point was a local max, all neighbors can be marked as + // visited, excluding them from future consideration as local maxima + if (point_is_max[(*indices_)[iii]]) + { + for (std::size_t k = 1; k < radius_indices.size (); ++k) // k = 1 is the first neighbor + { + point_is_visited[radius_indices[k]] = true; + } + } + + // Points that are local maxima are passed to removed indices + // Unless negative was set, then it's the opposite condition + if ((!negative_ && point_is_max[(*indices_)[iii]]) || (negative_ && !point_is_max[(*indices_)[iii]])) + { + if (extract_removed_indices_) + { + (*removed_indices_)[rii++] = (*indices_)[iii]; + } + + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[iii]; + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); +} + +#define PCL_INSTANTIATE_LocalMaximum(T) template class PCL_EXPORTS pcl::LocalMaximum; + +#endif // PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/median_filter.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/median_filter.hpp new file mode 100755 index 0000000..ff510af --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/median_filter.hpp @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ +#define PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ + +#include +#include + +template void +pcl::MedianFilter::applyFilter (PointCloud &output) +{ + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::MedianFilter] Input cloud needs to be organized\n"); + return; + } + + // Copy everything from the input cloud to the output cloud (takes care of all the fields) + copyPointCloud (*input_, output); + + int height = static_cast (output.height); + int width = static_cast (output.width); + for (int y = 0; y < height; ++y) + for (int x = 0; x < width; ++x) + if (pcl::isFinite ((*input_)(x, y))) + { + std::vector vals; + vals.reserve (window_size_ * window_size_); + // Fill in the vector of values with the depths around the interest point + for (int y_dev = -window_size_/2; y_dev <= window_size_/2; ++y_dev) + for (int x_dev = -window_size_/2; x_dev <= window_size_/2; ++x_dev) + { + if (x + x_dev >= 0 && x + x_dev < width && + y + y_dev >= 0 && y + y_dev < height && + pcl::isFinite ((*input_)(x+x_dev, y+y_dev))) + vals.push_back ((*input_)(x+x_dev, y+y_dev).z); + } + + if (vals.empty ()) + continue; + + // The output depth will be the median of all the depths in the window + partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ()); + float new_depth = vals[vals.size () / 2]; + // Do not allow points to move more than the set max_allowed_movement_ + if (std::abs (new_depth - (*input_)(x, y).z) < max_allowed_movement_) + output (x, y).z = new_depth; + else + output (x, y).z = (*input_)(x, y).z + + max_allowed_movement_ * (new_depth - (*input_)(x, y).z) / std::abs (new_depth - (*input_)(x, y).z); + } +} + + +#endif /* PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/model_outlier_removal.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/model_outlier_removal.hpp new file mode 100755 index 0000000..a09c42b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/model_outlier_removal.hpp @@ -0,0 +1,267 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ +#define PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ModelOutlierRemoval::initSACModel (pcl::SacModel model_type) +{ + // Build the model + switch (model_type) + { + case SACMODEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelPlane (input_)); + break; + } + case SACMODEL_LINE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelLine (input_)); + break; + } + case SACMODEL_CIRCLE2D: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCircle2D (input_)); + break; + } + case SACMODEL_SPHERE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelSphere (input_)); + break; + } + case SACMODEL_PARALLEL_LINE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelLine (input_)); + break; + } + case SACMODEL_PERPENDICULAR_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelPerpendicularPlane (input_)); + break; + } + case SACMODEL_CYLINDER: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCylinder (input_)); + break; + } + case SACMODEL_NORMAL_PLANE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalPlane (input_)); + break; + } + case SACMODEL_CONE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCone (input_)); + break; + } + case SACMODEL_NORMAL_SPHERE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalSphere (input_)); + break; + } + case SACMODEL_NORMAL_PARALLEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalParallelPlane (input_)); + break; + } + case SACMODEL_PARALLEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelPlane (input_)); + break; + } + default: + { + PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); + return (false); + } + } + return (true); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ModelOutlierRemoval::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator + output.points[ (*removed_indices_)[rii]].x = output.points[ (*removed_indices_)[rii]].y = output.points[ (*removed_indices_)[rii]].z = user_filter_value_; + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + applyFilterIndices (indices); + copyPointCloud (*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) +{ + //The arrays to be used + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + //is the filtersetup correct? + bool valid_setup = true; + + valid_setup &= initSACModel (model_type_); + + using SACModelFromNormals = SampleConsensusModelFromNormals; + // Returns NULL if cast isn't possible + SACModelFromNormals *model_from_normals = dynamic_cast (& (*model_)); + + if (model_from_normals) + { + if (!cloud_normals_) + { + valid_setup = false; + PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n"); + } + else + { + model_from_normals->setNormalDistanceWeight (normals_distance_weight_); + model_from_normals->setInputNormals (cloud_normals_); + } + } + + //if the filter setup is invalid filter for nan and return; + if (!valid_setup) + { + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if (!isFinite (input_->points[ (*indices_)[iii]])) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + indices[oii++] = (*indices_)[iii]; + } + return; + } + // check distance of pointcloud to model + std::vector distances; + //TODO: get signed distances ! + model_->setIndices(indices_); // added to reduce computation and arrange distances with indices + model_->getDistancesToModel (model_coefficients_, distances); + + bool thresh_result; + + // Filter for non-finite entries and the specified field limits + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if (!isFinite (input_->points[ (*indices_)[iii]])) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // use threshold function to separate outliers from inliers: + thresh_result = threshold_function_ (distances[iii]); + + // in normal mode: define outliers as false thresh_result + if (!negative_ && !thresh_result) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // in negative_ mode: define outliers as true thresh_result + if (negative_ && thresh_result) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[iii]; + + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); + +} + +#define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval; + +#endif // PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/morphological_filter.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/morphological_filter.hpp new file mode 100755 index 0000000..2ed31e3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/morphological_filter.hpp @@ -0,0 +1,208 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ +#define PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, + float resolution, const int morphological_operator, + pcl::PointCloud &cloud_out) +{ + if (cloud_in->empty ()) + return; + + pcl::copyPointCloud (*cloud_in, cloud_out); + + pcl::octree::OctreePointCloudSearch tree (resolution); + + tree.setInputCloud (cloud_in); + tree.addPointsFromInputCloud (); + + float half_res = resolution / 2.0f; + + switch (morphological_operator) + { + case MORPH_DILATE: + case MORPH_ERODE: + { + for (std::size_t p_idx = 0; p_idx < cloud_in->points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_in->points[p_idx].x - half_res; + float miny = cloud_in->points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_in->points[p_idx].x + half_res; + float maxy = cloud_in->points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (!pt_indices.empty ()) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (*cloud_in, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_DILATE: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + case MORPH_ERODE: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + } + } + } + break; + } + case MORPH_OPEN: + case MORPH_CLOSE: + { + pcl::PointCloud cloud_temp; + + pcl::copyPointCloud (*cloud_in, cloud_temp); + + for (std::size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_temp.points[p_idx].x - half_res; + float miny = cloud_temp.points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_temp.points[p_idx].x + half_res; + float maxy = cloud_temp.points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (!pt_indices.empty ()) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (cloud_temp, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_OPEN: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + case MORPH_CLOSE: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + } + } + } + + cloud_temp.swap (cloud_out); + + for (std::size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_temp.points[p_idx].x - half_res; + float miny = cloud_temp.points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_temp.points[p_idx].x + half_res; + float maxy = cloud_temp.points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (!pt_indices.empty ()) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (cloud_temp, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_OPEN: + default: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + case MORPH_CLOSE: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + } + } + } + break; + } + default: + { + PCL_ERROR ("Morphological operator is not supported!\n"); + break; + } + } + + return; +} + +#define PCL_INSTANTIATE_applyMorphologicalOperator(T) template PCL_EXPORTS void pcl::applyMorphologicalOperator (const pcl::PointCloud::ConstPtr &, float, const int, pcl::PointCloud &); + +#endif //#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/normal_refinement.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/normal_refinement.hpp new file mode 100755 index 0000000..6a744ab --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/normal_refinement.hpp @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_NORMAL_REFINEMENT_H_ +#define PCL_FILTERS_IMPL_NORMAL_REFINEMENT_H_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalRefinement::applyFilter (PointCloud &output) +{ + // Check input + if (input_->empty ()) + { + PCL_ERROR ("[pcl::%s::applyFilter] No source was input!\n", + getClassName ().c_str ()); + } + + // Copy to output + output = *input_; + + // Check that correspondences are non-empty + if (k_indices_.empty () || k_sqr_distances_.empty ()) + { + PCL_ERROR ("[pcl::%s::applyFilter] No point correspondences given! Returning original input.\n", + getClassName ().c_str ()); + return; + } + + // Check that correspondences are OK + const unsigned int size = k_indices_.size (); + if (k_sqr_distances_.size () != size || input_->size () != size) + { + PCL_ERROR ("[pcl::%s::applyFilter] Inconsistency between size of correspondence indices/distances or input! Returning original input.\n", + getClassName ().c_str ()); + return; + } + + // Run refinement while monitoring convergence + for (unsigned int i = 0; i < max_iterations_; ++i) + { + // Output of the current iteration + PointCloud tmp = output; + + // Mean change in direction, measured by dot products + float ddot = 0.0f; + + // Loop over all points in current output and write refined normal to tmp + int num_valids = 0; + for(unsigned int j = 0; j < size; ++j) + { + // Point to write to + NormalT& tmpj = tmp[j]; + + // Refine + if (refineNormal (output, j, k_indices_[j], k_sqr_distances_[j], tmpj)) + { + // Accumulate using similarity in direction between previous iteration and current + const NormalT& outputj = output[j]; + ddot += tmpj.normal_x * outputj.normal_x + tmpj.normal_y * outputj.normal_y + tmpj.normal_z * outputj.normal_z; + ++num_valids; + } + } + + // Take mean of similarities + ddot /= static_cast (num_valids); + + // Negate to since we want an error measure to approach zero + ddot = 1.0f - ddot; + + // Update output + output = tmp; + + // Break if converged + if (ddot < convergence_threshold_) + { + PCL_DEBUG("[pcl::%s::applyFilter] Converged after %i iterations with mean error of %f.\n", + getClassName ().c_str (), i+1, ddot); + break; + } + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/normal_space.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/normal_space.hpp new file mode 100755 index 0000000..f4b8f48 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/normal_space.hpp @@ -0,0 +1,280 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_ +#define PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_ + +#include +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////// +template bool +pcl::NormalSpaceSampling::initCompute () +{ + if (!FilterIndices::initCompute ()) + return false; + + // If sample size is 0 or if the sample size is greater then input cloud size then return entire copy of cloud + if (sample_ >= input_->size ()) + { + PCL_ERROR ("[NormalSpaceSampling::initCompute] Requested more samples than the input cloud size: %d vs %lu\n", + sample_, input_->size ()); + return false; + } + + boost::mt19937 rng (static_cast (seed_)); + boost::uniform_int uniform_distrib (0, unsigned (input_->size ())); + delete rng_uniform_distribution_; + rng_uniform_distribution_ = new boost::variate_generator > (rng, uniform_distrib); + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalSpaceSampling::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilter (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator + output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_; + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + output.is_dense = true; + applyFilter (indices); + pcl::copyPointCloud (*input_, indices, output); + } +} + +/////////////////////////////////////////////////////////////////////////////// +template bool +pcl::NormalSpaceSampling::isEntireBinSampled (boost::dynamic_bitset<> &array, + unsigned int start_index, + unsigned int length) +{ + bool status = true; + for (unsigned int i = start_index; i < start_index + length; i++) + { + status &= array.test (i); + } + return status; +} + +/////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::NormalSpaceSampling::findBin (const float *normal, unsigned int) +{ + unsigned int bin_number = 0; + // Holds the bin numbers for direction cosines in x,y,z directions + unsigned int t[3] = {0,0,0}; + + // dcos is the direction cosine. + float dcos = 0.0; + float bin_size = 0.0; + // max_cos and min_cos are the maximum and minimum values of std::cos(theta) respectively + float max_cos = 1.0; + float min_cos = -1.0; + +// dcos = std::cos (normal[0]); + dcos = normal[0]; + bin_size = (max_cos - min_cos) / static_cast (binsx_); + + // Finding bin number for direction cosine in x direction + unsigned int k = 0; + for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) + { + if (dcos >= i && dcos <= (i+bin_size)) + { + break; + } + } + t[0] = k; + +// dcos = std::cos (normal[1]); + dcos = normal[1]; + bin_size = (max_cos - min_cos) / static_cast (binsy_); + + // Finding bin number for direction cosine in y direction + k = 0; + for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) + { + if (dcos >= i && dcos <= (i+bin_size)) + { + break; + } + } + t[1] = k; + +// dcos = std::cos (normal[2]); + dcos = normal[2]; + bin_size = (max_cos - min_cos) / static_cast (binsz_); + + // Finding bin number for direction cosine in z direction + k = 0; + for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) + { + if (dcos >= i && dcos <= (i+bin_size)) + { + break; + } + } + t[2] = k; + + bin_number = t[0] * (binsy_*binsz_) + t[1] * binsz_ + t[2]; + return bin_number; +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalSpaceSampling::applyFilter (std::vector &indices) +{ + if (!initCompute ()) + { + indices = *indices_; + return; + } + + unsigned int max_values = (std::min) (sample_, static_cast (input_normals_->size ())); + // Resize output indices to sample size + indices.resize (max_values); + removed_indices_->resize (max_values); + + // Allocate memory for the histogram of normals. Normals will then be sampled from each bin. + unsigned int n_bins = binsx_ * binsy_ * binsz_; + // list holds the indices of points in that bin. Using list to avoid repeated resizing of vectors. + // Helps when the point cloud is large. + std::vector > normals_hg; + normals_hg.reserve (n_bins); + for (unsigned int i = 0; i < n_bins; i++) + normals_hg.emplace_back(); + + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins); + normals_hg[bin_number].push_back (*it); + } + + + // Setting up random access for the list created above. Maintaining the iterators to individual elements of the list + // in a vector. Using vector now as the size of the list is known. + std::vector::iterator> > random_access (normals_hg.size ()); + for (std::size_t i = 0; i < normals_hg.size (); i++) + { + random_access.emplace_back(); + random_access[i].resize (normals_hg[i].size ()); + + std::size_t j = 0; + for (std::list::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); ++itr, ++j) + random_access[i][j] = itr; + } + std::vector start_index (normals_hg.size ()); + start_index[0] = 0; + std::size_t prev_index = 0; + for (std::size_t i = 1; i < normals_hg.size (); i++) + { + start_index[i] = prev_index + normals_hg[i-1].size (); + prev_index = start_index[i]; + } + + // Maintaining flags to check if a point is sampled + boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ()); + // Maintaining flags to check if all points in the bin are sampled + boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ()); + unsigned int i = 0; + while (i < sample_) + { + // Iterating through every bin and picking one point at random, until the required number of points are sampled. + for (std::size_t j = 0; j < normals_hg.size (); j++) + { + unsigned int M = static_cast (normals_hg[j].size ()); + if (M == 0 || bin_empty_flag.test (j)) // bin_empty_flag(i) is set if all points in that bin are sampled.. + continue; + + unsigned int pos = 0; + unsigned int random_index = 0; + + // Picking up a sample at random from jth bin + do + { + random_index = static_cast ((*rng_uniform_distribution_) () % M); + pos = start_index[j] + random_index; + } while (is_sampled_flag.test (pos)); + + is_sampled_flag.flip (start_index[j] + random_index); + + // Checking if all points in bin j are sampled. + if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast (normals_hg[j].size ()))) + bin_empty_flag.flip (j); + + unsigned int index = *(random_access[j][random_index]); + indices[i] = index; + i++; + if (i == sample_) + break; + } + } + + // If we need to return the indices that we haven't sampled + if (extract_removed_indices_) + { + std::vector indices_temp = indices; + std::sort (indices_temp.begin (), indices_temp.end ()); + + std::vector all_indices_temp = *indices_; + std::sort (all_indices_temp.begin (), all_indices_temp.end ()); + set_difference (all_indices_temp.begin (), all_indices_temp.end (), + indices_temp.begin (), indices_temp.end (), + inserter (*removed_indices_, removed_indices_->begin ())); + } +} + +#define PCL_INSTANTIATE_NormalSpaceSampling(T,NT) template class PCL_EXPORTS pcl::NormalSpaceSampling; + +#endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/passthrough.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/passthrough.hpp new file mode 100755 index 0000000..269a9d7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/passthrough.hpp @@ -0,0 +1,167 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_PASSTHROUGH_HPP_ +#define PCL_FILTERS_IMPL_PASSTHROUGH_HPP_ + +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PassThrough::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (const auto ri : *removed_indices_) // ri = removed index + output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + output.is_dense = true; + applyFilterIndices (indices); + copyPointCloud (*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PassThrough::applyFilterIndices (std::vector &indices) +{ + // The arrays to be used + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + + // Has a field name been specified? + if (filter_field_name_.empty ()) + { + // Only filter for non-finite entries then + for (const auto ii : *indices_) // ii = input index + { + // Non-finite entries are always passed to removed indices + if (!std::isfinite (input_->points[ii].x) || + !std::isfinite (input_->points[ii].y) || + !std::isfinite (input_->points[ii].z)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = ii; + continue; + } + indices[oii++] = ii; + } + } + else + { + // Attempt to get the field name's index + std::vector fields; + int distance_idx = pcl::getFieldIndex (filter_field_name_, fields); + if (distance_idx == -1) + { + PCL_WARN ("[pcl::%s::applyFilter] Unable to find field name in point type.\n", getClassName ().c_str ()); + indices.clear (); + removed_indices_->clear (); + return; + } + + // Filter for non-finite entries and the specified field limits + for (const auto ii : *indices_) // ii = input index + { + // Non-finite entries are always passed to removed indices + if (!std::isfinite (input_->points[ii].x) || + !std::isfinite (input_->points[ii].y) || + !std::isfinite (input_->points[ii].z)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = ii; + continue; + } + + // Get the field's value + const std::uint8_t* pt_data = reinterpret_cast (&input_->points[ii]); + float field_value = 0; + memcpy (&field_value, pt_data + fields[distance_idx].offset, sizeof (float)); + + // Remove NAN/INF/-INF values. We expect passthrough to output clean valid data. + if (!std::isfinite (field_value)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = ii; + continue; + } + + // Outside of the field limits are passed to removed indices + if (!negative_ && (field_value < filter_limit_min_ || field_value > filter_limit_max_)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = ii; + continue; + } + + // Inside of the field limits are passed to removed indices if negative was set + if (negative_ && field_value >= filter_limit_min_ && field_value <= filter_limit_max_) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = ii; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = ii; + } + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); +} + +#define PCL_INSTANTIATE_PassThrough(T) template class PCL_EXPORTS pcl::PassThrough; + +#endif // PCL_FILTERS_IMPL_PASSTHROUGH_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/plane_clipper3D.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/plane_clipper3D.hpp new file mode 100755 index 0000000..0adb681 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/plane_clipper3D.hpp @@ -0,0 +1,228 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP +#define PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP + +#include + +template +pcl::PlaneClipper3D::PlaneClipper3D (const Eigen::Vector4f& plane_params) +: plane_params_ (plane_params) +{ +} + +template +pcl::PlaneClipper3D::~PlaneClipper3D () noexcept +{ +} + +template void +pcl::PlaneClipper3D::setPlaneParameters (const Eigen::Vector4f& plane_params) +{ + plane_params_ = plane_params; +} + +template const Eigen::Vector4f& +pcl::PlaneClipper3D::getPlaneParameters () const +{ + return plane_params_; +} + +template pcl::Clipper3D* +pcl::PlaneClipper3D::clone () const +{ + return new PlaneClipper3D (plane_params_); +} + +template float +pcl::PlaneClipper3D::getDistance (const PointT& point) const +{ + return (plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z + plane_params_[3]); +} + +template bool +pcl::PlaneClipper3D::clipPoint3D (const PointT& point) const +{ + return ((plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z ) >= -plane_params_[3]); +} + +/** + * @attention untested code + */ +template bool +pcl::PlaneClipper3D::clipLineSegment3D (PointT& point1, PointT& point2) const +{ + float dist1 = getDistance (point1); + float dist2 = getDistance (point2); + + if (dist1 * dist2 > 0) // both on same side of the plane -> nothing to clip + return (dist1 > 0); // true if both are on positive side, thus visible + + float lambda = dist2 / (dist2 - dist1); + + // get the plane intersecion + PointT intersection; + intersection.x = (point1.x - point2.x) * lambda + point2.x; + intersection.y = (point1.y - point2.y) * lambda + point2.y; + intersection.z = (point1.z - point2.z) * lambda + point2.z; + + // point1 is visible, point2 not => point2 needs to be replaced by intersection + if (dist1 >= 0) + point2 = intersection; + else + point1 = intersection; + + return false; +} + +/** + * @attention untested code + */ +template void +pcl::PlaneClipper3D::clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const +{ + clipped_polygon.clear (); + clipped_polygon.reserve (polygon.size ()); + + // test for degenerated polygons + if (polygon.size () < 3) + { + if (polygon.size () == 1) + { + // point outside clipping area ? + if (clipPoint3D (polygon [0])) + clipped_polygon.push_back (polygon [0]); + } + else if (polygon.size () == 2) + { + clipped_polygon.push_back (polygon [0]); + clipped_polygon.push_back (polygon [1]); + if (!clipLineSegment3D (clipped_polygon [0], clipped_polygon [1])) + clipped_polygon.clear (); + } + return; + } + + float previous_distance = getDistance (polygon [0]); + + if (previous_distance > 0) + clipped_polygon.push_back (polygon [0]); + + typename std::vector >::const_iterator prev_it = polygon.begin (); + + for (typename std::vector >::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++) + { + // if we intersect plane + float distance = getDistance (*pIt); + if (distance * previous_distance < 0) + { + float lambda = distance / (distance - previous_distance); + + PointT intersection; + intersection.x = (prev_it->x - pIt->x) * lambda + pIt->x; + intersection.y = (prev_it->y - pIt->y) * lambda + pIt->y; + intersection.z = (prev_it->z - pIt->z) * lambda + pIt->z; + + clipped_polygon.push_back (intersection); + } + if (distance > 0) + clipped_polygon.push_back (*pIt); + + previous_distance = distance; + } +} + +/** + * @attention untested code + */ +template void +pcl::PlaneClipper3D::clipPlanarPolygon3D (std::vector > &polygon) const +{ + std::vector > clipped; + clipPlanarPolygon3D (polygon, clipped); + polygon = clipped; +} + +// /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations. +template void +pcl::PlaneClipper3D::clipPointCloud3D (const pcl::PointCloud& cloud_in, std::vector& clipped, const std::vector& indices) const +{ + if (indices.empty ()) + { + clipped.reserve (cloud_in.size ()); + /* +#if 0 + Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float)); + Eigen::VectorXf distances = plane_params_.transpose () * points; + for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) + { + if (distances (rIdx, 0) >= -plane_params_[3]) + clipped.push_back (rIdx); + } +#else + Eigen::Matrix4Xf points (4, cloud_in.size ()); + for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) + { + points (0, rIdx) = cloud_in[rIdx].x; + points (1, rIdx) = cloud_in[rIdx].y; + points (2, rIdx) = cloud_in[rIdx].z; + points (3, rIdx) = 1; + } + Eigen::VectorXf distances = plane_params_.transpose () * points; + for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) + { + if (distances (rIdx, 0) >= 0) + clipped.push_back (rIdx); + } + +#endif + + //std::cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << std::endl; + + //std::cout << "distances: " << distances.rows () << " x " << distances.cols () << std::endl; + /*/ + for (unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx) + if (clipPoint3D (cloud_in[pIdx])) + clipped.push_back (pIdx); + //*/ + } + else + { + for (std::vector::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) + if (clipPoint3D (cloud_in[*iIt])) + clipped.push_back (*iIt); + } +} +#endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/project_inliers.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/project_inliers.hpp new file mode 100755 index 0000000..8b4386c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/project_inliers.hpp @@ -0,0 +1,166 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_PROJECT_INLIERS_H_ +#define PCL_FILTERS_IMPL_PROJECT_INLIERS_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ProjectInliers::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 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 (std::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::applyFilter] 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); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ProjectInliers::initSACModel (int model_type) +{ + // 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 (input_)); + break; + } + case SACMODEL_LINE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ()); + sacmodel_.reset (new SampleConsensusModelLine (input_)); + break; + } + case SACMODEL_CIRCLE2D: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ()); + sacmodel_.reset (new SampleConsensusModelCircle2D (input_)); + break; + } + case SACMODEL_SPHERE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ()); + sacmodel_.reset (new SampleConsensusModelSphere (input_)); + 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 (input_)); + 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 (input_)); + break; + } + case SACMODEL_CYLINDER: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); + sacmodel_.reset (new SampleConsensusModelCylinder (input_)); + 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 (input_)); + break; + } + case SACMODEL_CONE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ()); + sacmodel_.reset (new SampleConsensusModelCone (input_)); + 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 (input_)); + 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 (input_)); + 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 (input_)); + break; + } + default: + { + PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); + return (false); + } + } + return (true); +} + +#define PCL_INSTANTIATE_ProjectInliers(T) template class PCL_EXPORTS pcl::ProjectInliers; + +#endif // PCL_FILTERS_IMPL_PROJECT_INLIERS_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/radius_outlier_removal.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/radius_outlier_removal.hpp new file mode 100755 index 0000000..c3f96d7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/radius_outlier_removal.hpp @@ -0,0 +1,186 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_ +#define PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_ + +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RadiusOutlierRemoval::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (const auto ri : *removed_indices_) // ri = removed index + output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + applyFilterIndices (indices); + copyPointCloud (*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RadiusOutlierRemoval::applyFilterIndices (std::vector &indices) +{ + if (search_radius_ == 0.0) + { + PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ()); + indices.clear (); + removed_indices_->clear (); + return; + } + + // Initialize the search class + if (!searcher_) + { + if (input_->isOrganized ()) + searcher_.reset (new pcl::search::OrganizedNeighbor ()); + else + searcher_.reset (new pcl::search::KdTree (false)); + } + searcher_->setInputCloud (input_); + + // The arrays to be used + std::vector nn_indices (indices_->size ()); + std::vector nn_dists (indices_->size ()); + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + + // If the data is dense => use nearest-k search + if (input_->is_dense) + { + // Note: k includes the query point, so is always at least 1 + int mean_k = min_pts_radius_ + 1; + double nn_dists_max = search_radius_ * search_radius_; + + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + // Perform the nearest-k search + int k = searcher_->nearestKSearch (*it, mean_k, nn_indices, nn_dists); + + // Check the number of neighbors + // Note: nn_dists is sorted, so check the last item + bool chk_neighbors = true; + if (k == mean_k) + { + if (negative_) + { + chk_neighbors = false; + if (nn_dists_max < nn_dists[k-1]) + { + chk_neighbors = true; + } + } + else + { + chk_neighbors = true; + if (nn_dists_max < nn_dists[k-1]) + { + chk_neighbors = false; + } + } + } + else + { + if (negative_) + chk_neighbors = true; + else + chk_neighbors = false; + } + + // Points having too few neighbors are outliers and are passed to removed indices + // Unless negative was set, then it's the opposite condition + if (!chk_neighbors) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = *it; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = *it; + } + } + // NaN or Inf values could exist => use radius search + else + { + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + // Perform the radius search + // Note: k includes the query point, so is always at least 1 + int k = searcher_->radiusSearch (*it, search_radius_, nn_indices, nn_dists); + + // Points having too few neighbors are outliers and are passed to removed indices + // Unless negative was set, then it's the opposite condition + if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = *it; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = *it; + } + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); +} + +#define PCL_INSTANTIATE_RadiusOutlierRemoval(T) template class PCL_EXPORTS pcl::RadiusOutlierRemoval; + +#endif // PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/random_sample.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/random_sample.hpp new file mode 100755 index 0000000..d7bebbb --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/random_sample.hpp @@ -0,0 +1,157 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: extract_indices.hpp 1897 2011-07-26 20:35:49Z rusu $ + * + */ + +#ifndef PCL_FILTERS_IMPL_RANDOM_SAMPLE_H_ +#define PCL_FILTERS_IMPL_RANDOM_SAMPLE_H_ + +#include +#include +#include + + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::RandomSample::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilter (indices); + extract_removed_indices_ = temp; + copyPointCloud (*input_, output); + // Get X, Y, Z fields + const auto fields = pcl::getFields (); + std::vector offsets; + for (const auto &field : fields) + { + if (field.name == "x" || + field.name == "y" || + field.name == "z") + offsets.push_back (field.offset); + } + // For every "removed" point, set the x,y,z fields to user_filter_value_ + const static float user_filter_value = user_filter_value_; + for (const auto ri : *removed_indices_) // ri = removed index + { + std::uint8_t* pt_data = reinterpret_cast (&output[ri]); + for (const unsigned long &offset : offsets) + { + memcpy (pt_data + offset, &user_filter_value, sizeof (float)); + } + } + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + output.is_dense = true; + applyFilter (indices); + copyPointCloud (*input_, indices, output); + } +} + +/////////////////////////////////////////////////////////////////////////////// +template +void +pcl::RandomSample::applyFilter (std::vector &indices) +{ + std::size_t N = indices_->size (); + std::size_t sample_size = negative_ ? N - sample_ : sample_; + // If sample size is 0 or if the sample size is greater then input cloud size + // then return all indices + if (sample_size >= N) + { + indices = *indices_; + removed_indices_->clear (); + } + else + { + // Resize output indices to sample size + indices.resize (sample_size); + if (extract_removed_indices_) + removed_indices_->resize (N - sample_size); + + // Set random seed so derived indices are the same each time the filter runs + std::srand (seed_); + + // Algorithm S + std::size_t i = 0; + std::size_t index = 0; + std::vector added; + if (extract_removed_indices_) + added.resize (indices_->size (), false); + std::size_t n = sample_size; + while (n > 0) + { + // Step 1: [Generate U.] Generate a random variate U that is uniformly distributed between 0 and 1. + const float U = unifRand (); + // Step 2: [Test.] If N * U > n, go to Step 4. + if ((N * U) <= n) + { + // Step 3: [Select.] Select the next record in the file for the sample, and set n : = n - 1. + if (extract_removed_indices_) + added[index] = true; + indices[i++] = (*indices_)[index]; + --n; + } + // Step 4: [Don't select.] Skip over the next record (do not include it in the sample). + // Set N : = N - 1. + --N; + ++index; + // If n > 0, then return to Step 1; otherwise, the sample is complete and the algorithm terminates. + } + + // Now populate removed_indices_ appropriately + if (extract_removed_indices_) + { + std::size_t ri = 0; + for (std::size_t i = 0; i < added.size (); i++) + { + if (!added[i]) + { + (*removed_indices_)[ri++] = (*indices_)[i]; + } + } + } + } +} + +#define PCL_INSTANTIATE_RandomSample(T) template class PCL_EXPORTS pcl::RandomSample; + +#endif // PCL_FILTERS_IMPL_RANDOM_SAMPLE_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/sampling_surface_normal.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/sampling_surface_normal.hpp new file mode 100755 index 0000000..38f2dd5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/sampling_surface_normal.hpp @@ -0,0 +1,313 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FILTERS_IMPL_SAMPLING_SURFACE_NORMAL_H_ +#define PCL_FILTERS_IMPL_SAMPLING_SURFACE_NORMAL_H_ + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::SamplingSurfaceNormal::applyFilter (PointCloud &output) +{ + std::vector indices; + std::size_t npts = input_->points.size (); + for (std::size_t i = 0; i < npts; i++) + indices.push_back (i); + + Vector max_vec (3, 1); + Vector min_vec (3, 1); + findXYZMaxMin (*input_, max_vec, min_vec); + PointCloud data = *input_; + partition (data, 0, npts, min_vec, max_vec, indices, output); + output.width = 1; + output.height = std::uint32_t (output.points.size ()); +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::SamplingSurfaceNormal::findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec) +{ + float maxval = cloud.points[0].x; + float minval = cloud.points[0].x; + + for (std::size_t i = 0; i < cloud.points.size (); i++) + { + if (cloud.points[i].x > maxval) + { + maxval = cloud.points[i].x; + } + if (cloud.points[i].x < minval) + { + minval = cloud.points[i].x; + } + } + max_vec (0) = maxval; + min_vec (0) = minval; + + maxval = cloud.points[0].y; + minval = cloud.points[0].y; + + for (std::size_t i = 0; i < cloud.points.size (); i++) + { + if (cloud.points[i].y > maxval) + { + maxval = cloud.points[i].y; + } + if (cloud.points[i].y < minval) + { + minval = cloud.points[i].y; + } + } + max_vec (1) = maxval; + min_vec (1) = minval; + + maxval = cloud.points[0].z; + minval = cloud.points[0].z; + + for (std::size_t i = 0; i < cloud.points.size (); i++) + { + if (cloud.points[i].z > maxval) + { + maxval = cloud.points[i].z; + } + if (cloud.points[i].z < minval) + { + minval = cloud.points[i].z; + } + } + max_vec (2) = maxval; + min_vec (2) = minval; +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::SamplingSurfaceNormal::partition ( + const PointCloud& cloud, const int first, const int last, + const Vector min_values, const Vector max_values, + std::vector& indices, PointCloud& output) +{ + const int count (last - first); + if (count <= static_cast (sample_)) + { + samplePartition (cloud, first, last, indices, output); + return; + } + int cutDim = 0; + (max_values - min_values).maxCoeff (&cutDim); + + const int rightCount (count / 2); + const int leftCount (count - rightCount); + assert (last - rightCount == first + leftCount); + + // sort, hack std::nth_element + std::nth_element (indices.begin () + first, indices.begin () + first + leftCount, + indices.begin () + last, CompareDim (cutDim, cloud)); + + const int cutIndex (indices[first+leftCount]); + const float cutVal = findCutVal (cloud, cutDim, cutIndex); + + // update bounds for left + Vector leftMaxValues (max_values); + leftMaxValues[cutDim] = cutVal; + // update bounds for right + Vector rightMinValues (min_values); + rightMinValues[cutDim] = cutVal; + + // recurse + partition (cloud, first, first + leftCount, min_values, leftMaxValues, indices, output); + partition (cloud, first + leftCount, last, rightMinValues, max_values, indices, output); +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::SamplingSurfaceNormal::samplePartition ( + const PointCloud& data, const int first, const int last, + std::vector & indices, PointCloud& output) +{ + pcl::PointCloud cloud; + + for (int i = first; i < last; i++) + { + PointT pt; + pt.x = data.points[indices[i]].x; + pt.y = data.points[indices[i]].y; + pt.z = data.points[indices[i]].z; + cloud.points.push_back (pt); + } + cloud.width = 1; + cloud.height = std::uint32_t (cloud.points.size ()); + + Eigen::Vector4f normal; + float curvature = 0; + //pcl::computePointNormal (cloud, normal, curvature); + + computeNormal (cloud, normal, curvature); + + for (std::size_t i = 0; i < cloud.points.size (); i++) + { + // TODO: change to Boost random number generators! + const float r = float (std::rand ()) / float (RAND_MAX); + + if (r < ratio_) + { + PointT pt = cloud.points[i]; + pt.normal[0] = normal (0); + pt.normal[1] = normal (1); + pt.normal[2] = normal (2); + pt.curvature = curvature; + + output.points.push_back (pt); + } + } +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::SamplingSurfaceNormal::computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature) +{ + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f xyz_centroid; + float nx = 0.0; + float ny = 0.0; + float nz = 0.0; + + if (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, xyz_centroid) == 0) + { + nx = ny = nz = curvature = std::numeric_limits::quiet_NaN (); + return; + } + + // Get the plane normal and surface curvature + solvePlaneParameters (covariance_matrix, nx, ny, nz, curvature); + normal (0) = nx; + normal (1) = ny; + normal (2) = nz; + + normal (3) = 0; + // Hessian form (D = nc . p_plane (centroid here) + p) + normal (3) = -1 * normal.dot (xyz_centroid); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline unsigned int +pcl::SamplingSurfaceNormal::computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix3f &covariance_matrix, + Eigen::Vector4f ¢roid) +{ + // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer + Eigen::Matrix accu = Eigen::Matrix::Zero (); + std::size_t point_count = 0; + for (std::size_t i = 0; i < cloud.points.size (); i++) + { + if (!isFinite (cloud[i])) + { + continue; + } + + ++point_count; + accu [0] += cloud[i].x * cloud[i].x; + accu [1] += cloud[i].x * cloud[i].y; + accu [2] += cloud[i].x * cloud[i].z; + accu [3] += cloud[i].y * cloud[i].y; // 4 + accu [4] += cloud[i].y * cloud[i].z; // 5 + accu [5] += cloud[i].z * cloud[i].z; // 8 + accu [6] += cloud[i].x; + accu [7] += cloud[i].y; + accu [8] += cloud[i].z; + } + + accu /= static_cast (point_count); + centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; + centroid[3] = 0; + covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; + covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; + covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; + covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; + covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; + covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; + covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); + covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); + covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); + + return (static_cast (point_count)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SamplingSurfaceNormal::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + float &nx, float &ny, float &nz, float &curvature) +{ + // Extract the smallest eigenvalue and its eigenvector + EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + + nx = eigen_vector [0]; + ny = eigen_vector [1]; + nz = eigen_vector [2]; + + // Compute the curvature surface change + float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8); + if (eig_sum != 0) + curvature = std::abs (eigen_value / eig_sum); + else + curvature = 0; +} + +/////////////////////////////////////////////////////////////////////////////// +template float +pcl::SamplingSurfaceNormal::findCutVal ( + const PointCloud& cloud, const int cut_dim, const int cut_index) +{ + if (cut_dim == 0) + return (cloud.points[cut_index].x); + if (cut_dim == 1) + return (cloud.points[cut_index].y); + if (cut_dim == 2) + return (cloud.points[cut_index].z); + + return (0.0f); +} + + +#define PCL_INSTANTIATE_SamplingSurfaceNormal(T) template class PCL_EXPORTS pcl::SamplingSurfaceNormal; + +#endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/shadowpoints.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/shadowpoints.hpp new file mode 100755 index 0000000..839789e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/shadowpoints.hpp @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FILTERS_IMPL_SHADOW_POINTS_FILTER_H_ +#define PCL_FILTERS_IMPL_SHADOW_POINTS_FILTER_H_ + +#include + +#include + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::ShadowPoints::applyFilter (PointCloud &output) +{ + assert (input_normals_ != NULL); + output.points.resize (input_->points.size ()); + if (extract_removed_indices_) + removed_indices_->resize (input_->points.size ()); + + std::size_t cp = 0; + std::size_t ri = 0; + for (std::size_t i = 0; i < input_->points.size (); i++) + { + const NormalT &normal = input_normals_->points[i]; + const PointT &pt = input_->points[i]; + const float val = std::abs (normal.normal_x * pt.x + normal.normal_y * pt.y + normal.normal_z * pt.z); + + if ( (val >= threshold_) ^ negative_) + output.points[cp++] = pt; + else + { + if (extract_removed_indices_) + (*removed_indices_)[ri++] = i; + if (keep_organized_) + { + PointT &pt_new = output.points[cp++] = pt; + pt_new.x = pt_new.y = pt_new.z = user_filter_value_; + } + + } + } + output.points.resize (cp); + removed_indices_->resize (ri); + output.width = 1; + output.height = static_cast (output.points.size ()); +} + +/////////////////////////////////////////////////////////////////////////////// +template void +pcl::ShadowPoints::applyFilter (std::vector &indices) +{ + assert (input_normals_ != NULL); + indices.resize (input_->points.size ()); + if (extract_removed_indices_) + removed_indices_->resize (indices_->size ()); + + unsigned int k = 0; + unsigned int z = 0; + for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) + { + const NormalT &normal = input_normals_->points[*idx]; + const PointT &pt = input_->points[*idx]; + + float val = std::abs (normal.normal_x * pt.x + normal.normal_y * pt.y + normal.normal_z * pt.z); + + if ( (val >= threshold_) ^ negative_) + indices[k++] = *idx; + else if (extract_removed_indices_) + (*removed_indices_)[z++] = *idx; + } + indices.resize (k); + removed_indices_->resize (z); +} + +#define PCL_INSTANTIATE_ShadowPoints(T,NT) template class PCL_EXPORTS pcl::ShadowPoints; + +#endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/statistical_outlier_removal.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/statistical_outlier_removal.hpp new file mode 100755 index 0000000..73110ef --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/statistical_outlier_removal.hpp @@ -0,0 +1,159 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ +#define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ + +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::StatisticalOutlierRemoval::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (const auto ri : *removed_indices_) // ri = removed index + output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + applyFilterIndices (indices); + copyPointCloud (*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::StatisticalOutlierRemoval::applyFilterIndices (std::vector &indices) +{ + // Initialize the search class + if (!searcher_) + { + if (input_->isOrganized ()) + searcher_.reset (new pcl::search::OrganizedNeighbor ()); + else + searcher_.reset (new pcl::search::KdTree (false)); + } + searcher_->setInputCloud (input_); + + // The arrays to be used + std::vector nn_indices (mean_k_); + std::vector nn_dists (mean_k_); + std::vector distances (indices_->size ()); + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + + // First pass: Compute the mean distances for all points with respect to their k nearest neighbors + int valid_distances = 0; + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + if (!std::isfinite (input_->points[(*indices_)[iii]].x) || + !std::isfinite (input_->points[(*indices_)[iii]].y) || + !std::isfinite (input_->points[(*indices_)[iii]].z)) + { + distances[iii] = 0.0; + continue; + } + + // Perform the nearest k search + if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0) + { + distances[iii] = 0.0; + PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_); + continue; + } + + // Calculate the mean distance to its neighbors + double dist_sum = 0.0; + for (int k = 1; k < mean_k_ + 1; ++k) // k = 0 is the query point + dist_sum += sqrt (nn_dists[k]); + distances[iii] = static_cast (dist_sum / mean_k_); + valid_distances++; + } + + // Estimate the mean and the standard deviation of the distance vector + double sum = 0, sq_sum = 0; + for (const float &distance : distances) + { + sum += distance; + sq_sum += distance * distance; + } + double mean = sum / static_cast(valid_distances); + double variance = (sq_sum - sum * sum / static_cast(valid_distances)) / (static_cast(valid_distances) - 1); + double stddev = sqrt (variance); + //getMeanStd (distances, mean, stddev); + + double distance_threshold = mean + std_mul_ * stddev; + + // Second pass: Classify the points on the computed distance threshold + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Points having a too high average distance are outliers and are passed to removed indices + // Unless negative was set, then it's the opposite condition + if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[iii]; + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); +} + +#define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval; + +#endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/uniform_sampling.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/uniform_sampling.hpp new file mode 100755 index 0000000..eb9ae9e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/uniform_sampling.hpp @@ -0,0 +1,144 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_ +#define PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UniformSampling::applyFilter (PointCloud &output) +{ + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + output.height = 1; // downsampling breaks the organized structure + output.is_dense = true; // we filter out invalid points + + Eigen::Vector4f min_p, max_p; + // Get the minimum and maximum dimensions + pcl::getMinMax3D(*input_, min_p, max_p); + + // Compute the minimum and maximum bounding box values + min_b_[0] = static_cast (std::floor (min_p[0] * inverse_leaf_size_[0])); + max_b_[0] = static_cast (std::floor (max_p[0] * inverse_leaf_size_[0])); + min_b_[1] = static_cast (std::floor (min_p[1] * inverse_leaf_size_[1])); + max_b_[1] = static_cast (std::floor (max_p[1] * inverse_leaf_size_[1])); + min_b_[2] = static_cast (std::floor (min_p[2] * inverse_leaf_size_[2])); + max_b_[2] = static_cast (std::floor (max_p[2] * inverse_leaf_size_[2])); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); + div_b_[3] = 0; + + // Clear the leaves + leaves_.clear (); + + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); + + removed_indices_->clear(); + // First pass: build a set of leaves with the point index closest to the leaf center + for (std::size_t cp = 0; cp < indices_->size (); ++cp) + { + if (!input_->is_dense) + { + // Check if the point is invalid + if (!std::isfinite (input_->points[(*indices_)[cp]].x) || + !std::isfinite (input_->points[(*indices_)[cp]].y) || + !std::isfinite (input_->points[(*indices_)[cp]].z)) + { + if (extract_removed_indices_) + removed_indices_->push_back ((*indices_)[cp]); + continue; + } + } + + Eigen::Vector4i ijk = Eigen::Vector4i::Zero (); + ijk[0] = static_cast (std::floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0])); + ijk[1] = static_cast (std::floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1])); + ijk[2] = static_cast (std::floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2])); + + // Compute the leaf index + int idx = (ijk - min_b_).dot (divb_mul_); + Leaf& leaf = leaves_[idx]; + // First time we initialize the index + if (leaf.idx == -1) + { + leaf.idx = (*indices_)[cp]; + continue; + } + + // Check to see if this point is closer to the leaf center than the previous one we saved + float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast ()).squaredNorm (); + float diff_prev = (input_->points[leaf.idx].getVector4fMap () - ijk.cast ()).squaredNorm (); + + // If current point is closer, copy its index instead + if (diff_cur < diff_prev) + { + if (extract_removed_indices_) + removed_indices_->push_back (leaf.idx); + leaf.idx = (*indices_)[cp]; + } + else + { + if (extract_removed_indices_) + removed_indices_->push_back ((*indices_)[cp]); + } + } + + // Second pass: go over all leaves and copy data + output.points.resize (leaves_.size ()); + int cp = 0; + + for (const auto& leaf : leaves_) + output.points[cp++] = input_->points[leaf.second.idx]; + output.width = static_cast (output.points.size ()); +} + +#define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling; + +#endif // PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid.hpp new file mode 100755 index 0000000..6ba0bc6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid.hpp @@ -0,0 +1,437 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_ +#define PCL_FILTERS_IMPL_VOXEL_GRID_H_ + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getMinMax3D (const typename pcl::PointCloud::ConstPtr &cloud, + const std::string &distance_field_name, float min_distance, float max_distance, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative) +{ + Eigen::Array4f min_p, max_p; + min_p.setConstant (FLT_MAX); + max_p.setConstant (-FLT_MAX); + + // Get the fields list and the distance field index + std::vector fields; + int distance_idx = pcl::getFieldIndex (distance_field_name, fields); + + float distance_value; + // If dense, no need to check for NaNs + if (cloud->is_dense) + { + for (std::size_t i = 0; i < cloud->points.size (); ++i) + { + // Get the distance value + const std::uint8_t* pt_data = reinterpret_cast (&cloud->points[i]); + memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); + + if (limit_negative) + { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < max_distance) && (distance_value > min_distance)) + continue; + } + else + { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > max_distance) || (distance_value < min_distance)) + continue; + } + // Create the point structure and get the min/max + pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + else + { + for (std::size_t i = 0; i < cloud->points.size (); ++i) + { + // Get the distance value + const std::uint8_t* pt_data = reinterpret_cast (&cloud->points[i]); + memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); + + if (limit_negative) + { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < max_distance) && (distance_value > min_distance)) + continue; + } + else + { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > max_distance) || (distance_value < min_distance)) + continue; + } + + // Check if the point is invalid + if (!std::isfinite (cloud->points[i].x) || + !std::isfinite (cloud->points[i].y) || + !std::isfinite (cloud->points[i].z)) + continue; + // Create the point structure and get the min/max + pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + min_pt = min_p; + max_pt = max_p; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getMinMax3D (const typename pcl::PointCloud::ConstPtr &cloud, + const std::vector &indices, + const std::string &distance_field_name, float min_distance, float max_distance, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative) +{ + Eigen::Array4f min_p, max_p; + min_p.setConstant (FLT_MAX); + max_p.setConstant (-FLT_MAX); + + // Get the fields list and the distance field index + std::vector fields; + int distance_idx = pcl::getFieldIndex (distance_field_name, fields); + + float distance_value; + // If dense, no need to check for NaNs + if (cloud->is_dense) + { + for (const int &index : indices) + { + // Get the distance value + const std::uint8_t* pt_data = reinterpret_cast (&cloud->points[index]); + memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); + + if (limit_negative) + { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < max_distance) && (distance_value > min_distance)) + continue; + } + else + { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > max_distance) || (distance_value < min_distance)) + continue; + } + // Create the point structure and get the min/max + pcl::Array4fMapConst pt = cloud->points[index].getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + else + { + for (const int &index : indices) + { + // Get the distance value + const std::uint8_t* pt_data = reinterpret_cast (&cloud->points[index]); + memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); + + if (limit_negative) + { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < max_distance) && (distance_value > min_distance)) + continue; + } + else + { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > max_distance) || (distance_value < min_distance)) + continue; + } + + // Check if the point is invalid + if (!std::isfinite (cloud->points[index].x) || + !std::isfinite (cloud->points[index].y) || + !std::isfinite (cloud->points[index].z)) + continue; + // Create the point structure and get the min/max + pcl::Array4fMapConst pt = cloud->points[index].getArray4fMap (); + min_p = min_p.min (pt); + max_p = max_p.max (pt); + } + } + min_pt = min_p; + max_pt = max_p; +} + +struct cloud_point_index_idx +{ + unsigned int idx; + unsigned int cloud_point_index; + + cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} + bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); } +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::VoxelGrid::applyFilter (PointCloud &output) +{ + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Copy the header (and thus the frame_id) + allocate enough space for points + output.height = 1; // downsampling breaks the organized structure + output.is_dense = true; // we filter out invalid points + + Eigen::Vector4f min_p, max_p; + // Get the minimum and maximum dimensions + if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud... + getMinMax3D (input_, *indices_, filter_field_name_, static_cast (filter_limit_min_), static_cast (filter_limit_max_), min_p, max_p, filter_limit_negative_); + else + getMinMax3D (*input_, *indices_, min_p, max_p); + + // Check that the leaf size is not too small, given the size of the data + std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; + std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; + std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; + + if ((dx*dy*dz) > static_cast(std::numeric_limits::max())) + { + PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); + output = *input_; + return; + } + + // Compute the minimum and maximum bounding box values + min_b_[0] = static_cast (std::floor (min_p[0] * inverse_leaf_size_[0])); + max_b_[0] = static_cast (std::floor (max_p[0] * inverse_leaf_size_[0])); + min_b_[1] = static_cast (std::floor (min_p[1] * inverse_leaf_size_[1])); + max_b_[1] = static_cast (std::floor (max_p[1] * inverse_leaf_size_[1])); + min_b_[2] = static_cast (std::floor (min_p[2] * inverse_leaf_size_[2])); + max_b_[2] = static_cast (std::floor (max_p[2] * inverse_leaf_size_[2])); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); + div_b_[3] = 0; + + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); + + // Storage for mapping leaf and pointcloud indexes + std::vector index_vector; + index_vector.reserve (indices_->size ()); + + // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... + if (!filter_field_name_.empty ()) + { + // Get the distance field index + std::vector fields; + int distance_idx = pcl::getFieldIndex (filter_field_name_, fields); + if (distance_idx == -1) + PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); + + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!std::isfinite (input_->points[*it].x) || + !std::isfinite (input_->points[*it].y) || + !std::isfinite (input_->points[*it].z)) + continue; + + // Get the distance value + const std::uint8_t* pt_data = reinterpret_cast (&input_->points[*it]); + float distance_value = 0; + memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); + + if (filter_limit_negative_) + { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) + continue; + } + else + { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) + continue; + } + + int ijk0 = static_cast (std::floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); + int ijk1 = static_cast (std::floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); + int ijk2 = static_cast (std::floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + index_vector.emplace_back(static_cast (idx), *it); + } + } + // No distance filtering, process all data + else + { + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!std::isfinite (input_->points[*it].x) || + !std::isfinite (input_->points[*it].y) || + !std::isfinite (input_->points[*it].z)) + continue; + + int ijk0 = static_cast (std::floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); + int ijk1 = static_cast (std::floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); + int ijk2 = static_cast (std::floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + index_vector.emplace_back(static_cast (idx), *it); + } + } + + // Second pass: sort the index_vector vector using value representing target cell as index + // in effect all points belonging to the same output cell will be next to each other + std::sort (index_vector.begin (), index_vector.end (), std::less ()); + + // Third pass: count output cells + // we need to skip all the same, adjacent idx values + unsigned int total = 0; + unsigned int index = 0; + // first_and_last_indices_vector[i] represents the index in index_vector of the first point in + // index_vector belonging to the voxel which corresponds to the i-th output point, + // and of the first point not belonging to. + std::vector > first_and_last_indices_vector; + // Worst case size + first_and_last_indices_vector.reserve (index_vector.size ()); + while (index < index_vector.size ()) + { + unsigned int i = index + 1; + while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx) + ++i; + if (i - index >= min_points_per_voxel_) + { + ++total; + first_and_last_indices_vector.emplace_back(index, i); + } + index = i; + } + + // Fourth pass: compute centroids, insert them into their final position + output.points.resize (total); + if (save_leaf_layout_) + { + try + { + // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1 + std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2]; + //This is the number of elements that need to be re-initialized to -1 + std::uint32_t reinit_size = std::min (static_cast (new_layout_size), static_cast (leaf_layout_.size())); + for (std::uint32_t i = 0; i < reinit_size; i++) + { + leaf_layout_[i] = -1; + } + leaf_layout_.resize (new_layout_size, -1); + } + catch (std::bad_alloc&) + { + throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout", + "voxel_grid.hpp", "applyFilter"); + } + catch (std::length_error&) + { + throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout", + "voxel_grid.hpp", "applyFilter"); + } + } + + index = 0; + for (const auto &cp : first_and_last_indices_vector) + { + // calculate centroid - sum values from all input points, that have the same idx value in index_vector array + unsigned int first_index = cp.first; + unsigned int last_index = cp.second; + + // index is centroid final position in resulting PointCloud + if (save_leaf_layout_) + leaf_layout_[index_vector[first_index].idx] = index; + + //Limit downsampling to coords + if (!downsample_all_data_) + { + Eigen::Vector4f centroid (Eigen::Vector4f::Zero ()); + + for (unsigned int li = first_index; li < last_index; ++li) + centroid += input_->points[index_vector[li].cloud_point_index].getVector4fMap (); + + centroid /= static_cast (last_index - first_index); + output.points[index].getVector4fMap () = centroid; + } + else + { + CentroidPoint centroid; + + // fill in the accumulator with leaf points + for (unsigned int li = first_index; li < last_index; ++li) + centroid.add (input_->points[index_vector[li].cloud_point_index]); + + centroid.get (output.points[index]); + } + + ++index; + } + output.width = static_cast (output.points.size ()); +} + +#define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid; +#define PCL_INSTANTIATE_getMinMax3D(T) template PCL_EXPORTS void pcl::getMinMax3D (const pcl::PointCloud::ConstPtr &, const std::string &, float, float, Eigen::Vector4f &, Eigen::Vector4f &, bool); + +#endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid_covariance.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid_covariance.hpp new file mode 100755 index 0000000..413d05c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid_covariance.hpp @@ -0,0 +1,448 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ +#define PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::VoxelGridCovariance::applyFilter (PointCloud &output) +{ + voxel_centroids_leaf_indices_.clear (); + + // Has the input dataset been set already? + if (!input_) + { + PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Copy the header (and thus the frame_id) + allocate enough space for points + output.height = 1; // downsampling breaks the organized structure + output.is_dense = true; // we filter out invalid points + output.points.clear (); + + Eigen::Vector4f min_p, max_p; + // Get the minimum and maximum dimensions + if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud... + getMinMax3D (input_, filter_field_name_, static_cast (filter_limit_min_), static_cast (filter_limit_max_), min_p, max_p, filter_limit_negative_); + else + getMinMax3D (*input_, min_p, max_p); + + // Check that the leaf size is not too small, given the size of the data + std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; + std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; + std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; + + if((dx*dy*dz) > std::numeric_limits::max()) + { + PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); + output.clear(); + return; + } + + // Compute the minimum and maximum bounding box values + min_b_[0] = static_cast (std::floor (min_p[0] * inverse_leaf_size_[0])); + max_b_[0] = static_cast (std::floor (max_p[0] * inverse_leaf_size_[0])); + min_b_[1] = static_cast (std::floor (min_p[1] * inverse_leaf_size_[1])); + max_b_[1] = static_cast (std::floor (max_p[1] * inverse_leaf_size_[1])); + min_b_[2] = static_cast (std::floor (min_p[2] * inverse_leaf_size_[2])); + max_b_[2] = static_cast (std::floor (max_p[2] * inverse_leaf_size_[2])); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); + div_b_[3] = 0; + + // Clear the leaves + leaves_.clear (); + + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); + + int centroid_size = 4; + + if (downsample_all_data_) + centroid_size = boost::mpl::size::value; + + // ---[ RGB special case + std::vector fields; + int rgba_index = -1; + rgba_index = pcl::getFieldIndex ("rgb", fields); + if (rgba_index == -1) + rgba_index = pcl::getFieldIndex ("rgba", fields); + if (rgba_index >= 0) + { + rgba_index = fields[rgba_index].offset; + centroid_size += 4; + } + + // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... + if (!filter_field_name_.empty ()) + { + // Get the distance field index + std::vector fields; + int distance_idx = pcl::getFieldIndex (filter_field_name_, fields); + if (distance_idx == -1) + PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); + + // First pass: go over all points and insert them into the right leaf + for (std::size_t cp = 0; cp < input_->points.size (); ++cp) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!std::isfinite (input_->points[cp].x) || + !std::isfinite (input_->points[cp].y) || + !std::isfinite (input_->points[cp].z)) + continue; + + // Get the distance value + const std::uint8_t* pt_data = reinterpret_cast (&input_->points[cp]); + float distance_value = 0; + memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); + + if (filter_limit_negative_) + { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) + continue; + } + else + { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) + continue; + } + + int ijk0 = static_cast (std::floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); + int ijk1 = static_cast (std::floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); + int ijk2 = static_cast (std::floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + Leaf& leaf = leaves_[idx]; + if (leaf.nr_points == 0) + { + leaf.centroid.resize (centroid_size); + leaf.centroid.setZero (); + } + + Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); + // Accumulate point sum for centroid calculation + leaf.mean_ += pt3d; + // Accumulate x*xT for single pass covariance calculation + leaf.cov_ += pt3d * pt3d.transpose (); + + // Do we need to process all the fields? + if (!downsample_all_data_) + { + Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); + leaf.centroid.template head<4> () += pt; + } + else + { + // Copy all the fields + Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); + pcl::for_each_type (NdCopyPointEigenFunctor (input_->points[cp], centroid)); + // ---[ RGB special case + if (rgba_index >= 0) + { + // Fill r/g/b data, assuming that the order is BGRA + const pcl::RGB& rgb = *reinterpret_cast (reinterpret_cast (&input_->points[cp]) + rgba_index); + centroid[centroid_size - 4] = rgb.a; + centroid[centroid_size - 3] = rgb.r; + centroid[centroid_size - 2] = rgb.g; + centroid[centroid_size - 1] = rgb.b; + } + leaf.centroid += centroid; + } + ++leaf.nr_points; + } + } + // No distance filtering, process all data + else + { + // First pass: go over all points and insert them into the right leaf + for (std::size_t cp = 0; cp < input_->points.size (); ++cp) + { + if (!input_->is_dense) + // Check if the point is invalid + if (!std::isfinite (input_->points[cp].x) || + !std::isfinite (input_->points[cp].y) || + !std::isfinite (input_->points[cp].z)) + continue; + + int ijk0 = static_cast (std::floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast (min_b_[0])); + int ijk1 = static_cast (std::floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast (min_b_[1])); + int ijk2 = static_cast (std::floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast (min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast ()).matrix () - min_b_).dot (divb_mul_); + Leaf& leaf = leaves_[idx]; + if (leaf.nr_points == 0) + { + leaf.centroid.resize (centroid_size); + leaf.centroid.setZero (); + } + + Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); + // Accumulate point sum for centroid calculation + leaf.mean_ += pt3d; + // Accumulate x*xT for single pass covariance calculation + leaf.cov_ += pt3d * pt3d.transpose (); + + // Do we need to process all the fields? + if (!downsample_all_data_) + { + Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); + leaf.centroid.template head<4> () += pt; + } + else + { + // Copy all the fields + Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); + pcl::for_each_type (NdCopyPointEigenFunctor (input_->points[cp], centroid)); + // ---[ RGB special case + if (rgba_index >= 0) + { + // Fill r/g/b data, assuming that the order is BGRA + const pcl::RGB& rgb = *reinterpret_cast (reinterpret_cast (&input_->points[cp]) + rgba_index); + centroid[centroid_size - 4] = rgb.a; + centroid[centroid_size - 3] = rgb.r; + centroid[centroid_size - 2] = rgb.g; + centroid[centroid_size - 1] = rgb.b; + } + leaf.centroid += centroid; + } + ++leaf.nr_points; + } + } + + // Second pass: go over all leaves and compute centroids and covariance matrices + output.points.reserve (leaves_.size ()); + if (searchable_) + voxel_centroids_leaf_indices_.reserve (leaves_.size ()); + int cp = 0; + if (save_leaf_layout_) + leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1); + + // Eigen values and vectors calculated to prevent near singluar matrices + Eigen::SelfAdjointEigenSolver eigensolver; + Eigen::Matrix3d eigen_val; + Eigen::Vector3d pt_sum; + + // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value. + double min_covar_eigvalue; + + for (typename std::map::iterator it = leaves_.begin (); it != leaves_.end (); ++it) + { + + // Normalize the centroid + Leaf& leaf = it->second; + + // Normalize the centroid + leaf.centroid /= static_cast (leaf.nr_points); + // Point sum used for single pass covariance calculation + pt_sum = leaf.mean_; + // Normalize mean + leaf.mean_ /= leaf.nr_points; + + // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds. + // Points with less than the minimum points will have a can not be accuratly approximated using a normal distribution. + if (leaf.nr_points >= min_points_per_voxel_) + { + if (save_leaf_layout_) + leaf_layout_[it->first] = cp++; + + output.push_back (PointT ()); + + // Do we need to process all the fields? + if (!downsample_all_data_) + { + output.points.back ().x = leaf.centroid[0]; + output.points.back ().y = leaf.centroid[1]; + output.points.back ().z = leaf.centroid[2]; + } + else + { + pcl::for_each_type (pcl::NdCopyEigenPointFunctor (leaf.centroid, output.back ())); + // ---[ RGB special case + if (rgba_index >= 0) + { + pcl::RGB& rgb = *reinterpret_cast (reinterpret_cast (&output.points.back ()) + rgba_index); + rgb.a = leaf.centroid[centroid_size - 4]; + rgb.r = leaf.centroid[centroid_size - 3]; + rgb.g = leaf.centroid[centroid_size - 2]; + rgb.b = leaf.centroid[centroid_size - 1]; + } + } + + // Stores the voxel indice for fast access searching + if (searchable_) + voxel_centroids_leaf_indices_.push_back (static_cast (it->first)); + + // Single pass covariance calculation + leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose (); + leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points; + + //Normalize Eigen Val such that max no more than 100x min. + eigensolver.compute (leaf.cov_); + eigen_val = eigensolver.eigenvalues ().asDiagonal (); + leaf.evecs_ = eigensolver.eigenvectors (); + + if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0) + { + leaf.nr_points = -1; + continue; + } + + // Avoids matrices near singularities (eq 6.11)[Magnusson 2009] + + min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2); + if (eigen_val (0, 0) < min_covar_eigvalue) + { + eigen_val (0, 0) = min_covar_eigvalue; + + if (eigen_val (1, 1) < min_covar_eigvalue) + { + eigen_val (1, 1) = min_covar_eigvalue; + } + + leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse (); + } + leaf.evals_ = eigen_val.diagonal (); + + leaf.icov_ = leaf.cov_.inverse (); + if (leaf.icov_.maxCoeff () == std::numeric_limits::infinity ( ) + || leaf.icov_.minCoeff () == -std::numeric_limits::infinity ( ) ) + { + leaf.nr_points = -1; + } + + } + } + + output.width = static_cast (output.points.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::VoxelGridCovariance::getNeighborhoodAtPoint (const PointT& reference_point, std::vector &neighbors) +{ + neighbors.clear (); + + // Find displacement coordinates + Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices (); + Eigen::Vector4i ijk (static_cast (std::floor (reference_point.x / leaf_size_[0])), + static_cast (std::floor (reference_point.y / leaf_size_[1])), + static_cast (std::floor (reference_point.z / leaf_size_[2])), 0); + Eigen::Array4i diff2min = min_b_ - ijk; + Eigen::Array4i diff2max = max_b_ - ijk; + neighbors.reserve (relative_coordinates.cols ()); + + // Check each neighbor to see if it is occupied and contains sufficient points + // Slower than radius search because needs to check 26 indices + for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++) + { + Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished (); + // Checking if the specified cell is in the grid + if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ()) + { + typename std::map::iterator leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_))); + if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_) + { + LeafConstPtr leaf = &(leaf_iter->second); + neighbors.push_back (leaf); + } + } + } + + return (static_cast (neighbors.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::VoxelGridCovariance::getDisplayCloud (pcl::PointCloud& cell_cloud) +{ + cell_cloud.clear (); + + int pnt_per_cell = 1000; + boost::mt19937 rng; + boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ()); + boost::variate_generator > var_nor (rng, nd); + + Eigen::LLT llt_of_cov; + Eigen::Matrix3d cholesky_decomp; + Eigen::Vector3d cell_mean; + Eigen::Vector3d rand_point; + Eigen::Vector3d dist_point; + + // Generate points for each occupied voxel with sufficient points. + for (typename std::map::iterator it = leaves_.begin (); it != leaves_.end (); ++it) + { + Leaf& leaf = it->second; + + if (leaf.nr_points >= min_points_per_voxel_) + { + cell_mean = leaf.mean_; + llt_of_cov.compute (leaf.cov_); + cholesky_decomp = llt_of_cov.matrixL (); + + // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix + for (int i = 0; i < pnt_per_cell; i++) + { + rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ()); + dist_point = cell_mean + cholesky_decomp * rand_point; + cell_cloud.push_back (PointXYZ (static_cast (dist_point (0)), static_cast (dist_point (1)), static_cast (dist_point (2)))); + } + } + } +} + +#define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance; + +#endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp b/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp new file mode 100755 index 0000000..689f592 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp @@ -0,0 +1,439 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ +#define PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::VoxelGridOcclusionEstimation::initializeVoxelGrid () +{ + // initialization set to true + initialized_ = true; + + // create the voxel grid and store the output cloud + this->filter (filtered_cloud_); + + // Get the minimum and maximum bounding box dimensions + b_min_[0] = (static_cast ( min_b_[0]) * leaf_size_[0]); + b_min_[1] = (static_cast ( min_b_[1]) * leaf_size_[1]); + b_min_[2] = (static_cast ( min_b_[2]) * leaf_size_[2]); + b_max_[0] = (static_cast ( (max_b_[0]) + 1) * leaf_size_[0]); + b_max_[1] = (static_cast ( (max_b_[1]) + 1) * leaf_size_[1]); + b_max_[2] = (static_cast ( (max_b_[2]) + 1) * leaf_size_[2]); + + // set the sensor origin and sensor orientation + sensor_origin_ = filtered_cloud_.sensor_origin_; + sensor_orientation_ = filtered_cloud_.sensor_orientation_; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::VoxelGridOcclusionEstimation::occlusionEstimation (int& out_state, + const Eigen::Vector3i& in_target_voxel) +{ + if (!initialized_) + { + PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n"); + return -1; + } + + // estimate direction to target voxel + Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel); + Eigen::Vector4f direction = p - sensor_origin_; + direction.normalize (); + + // estimate entry point into the voxel grid + float tmin = rayBoxIntersection (sensor_origin_, direction); + + if (tmin == -1) + { + PCL_ERROR ("The ray does not intersect with the bounding box \n"); + return -1; + } + + // ray traversal + out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin); + + return 0; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::VoxelGridOcclusionEstimation::occlusionEstimation (int& out_state, + std::vector >& out_ray, + const Eigen::Vector3i& in_target_voxel) +{ + if (!initialized_) + { + PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n"); + return -1; + } + + // estimate direction to target voxel + Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel); + Eigen::Vector4f direction = p - sensor_origin_; + direction.normalize (); + + // estimate entry point into the voxel grid + float tmin = rayBoxIntersection (sensor_origin_, direction); + + if (tmin == -1) + { + PCL_ERROR ("The ray does not intersect with the bounding box \n"); + return -1; + } + + // ray traversal + out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin); + + return 0; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::VoxelGridOcclusionEstimation::occlusionEstimationAll (std::vector >& occluded_voxels) +{ + if (!initialized_) + { + PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n"); + return -1; + } + + // reserve space for the ray vector + int reserve_size = div_b_[0] * div_b_[1] * div_b_[2]; + occluded_voxels.reserve (reserve_size); + + // iterate over the entire voxel grid + for (int kk = min_b_.z (); kk <= max_b_.z (); ++kk) + for (int jj = min_b_.y (); jj <= max_b_.y (); ++jj) + for (int ii = min_b_.x (); ii <= max_b_.x (); ++ii) + { + Eigen::Vector3i ijk (ii, jj, kk); + // process all free voxels + int index = this->getCentroidIndexAt (ijk); + if (index == -1) + { + // estimate direction to target voxel + Eigen::Vector4f p = getCentroidCoordinate (ijk); + Eigen::Vector4f direction = p - sensor_origin_; + direction.normalize (); + + // estimate entry point into the voxel grid + float tmin = rayBoxIntersection (sensor_origin_, direction); + + // ray traversal + int state = rayTraversal (ijk, sensor_origin_, direction, tmin); + + // if voxel is occluded + if (state == 1) + occluded_voxels.push_back (ijk); + } + } + return 0; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::VoxelGridOcclusionEstimation::rayBoxIntersection (const Eigen::Vector4f& origin, + const Eigen::Vector4f& direction) +{ + float tmin, tmax, tymin, tymax, tzmin, tzmax; + + if (direction[0] >= 0) + { + tmin = (b_min_[0] - origin[0]) / direction[0]; + tmax = (b_max_[0] - origin[0]) / direction[0]; + } + else + { + tmin = (b_max_[0] - origin[0]) / direction[0]; + tmax = (b_min_[0] - origin[0]) / direction[0]; + } + + if (direction[1] >= 0) + { + tymin = (b_min_[1] - origin[1]) / direction[1]; + tymax = (b_max_[1] - origin[1]) / direction[1]; + } + else + { + tymin = (b_max_[1] - origin[1]) / direction[1]; + tymax = (b_min_[1] - origin[1]) / direction[1]; + } + + if ((tmin > tymax) || (tymin > tmax)) + { + PCL_ERROR ("no intersection with the bounding box \n"); + tmin = -1.0f; + return tmin; + } + + if (tymin > tmin) + tmin = tymin; + if (tymax < tmax) + tmax = tymax; + + if (direction[2] >= 0) + { + tzmin = (b_min_[2] - origin[2]) / direction[2]; + tzmax = (b_max_[2] - origin[2]) / direction[2]; + } + else + { + tzmin = (b_max_[2] - origin[2]) / direction[2]; + tzmax = (b_min_[2] - origin[2]) / direction[2]; + } + + if ((tmin > tzmax) || (tzmin > tmax)) + { + PCL_ERROR ("no intersection with the bounding box \n"); + tmin = -1.0f; + return tmin; + } + + if (tzmin > tmin) + tmin = tzmin; + if (tzmax < tmax) + tmax = tzmax; + + return tmin; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::VoxelGridOcclusionEstimation::rayTraversal (const Eigen::Vector3i& target_voxel, + const Eigen::Vector4f& origin, + const Eigen::Vector4f& direction, + const float t_min) +{ + // coordinate of the boundary of the voxel grid + Eigen::Vector4f start = origin + t_min * direction; + + // i,j,k coordinate of the voxel were the ray enters the voxel grid + Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]); + + // steps in which direction we have to travel in the voxel grid + int step_x, step_y, step_z; + + // centroid coordinate of the entry voxel + Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk); + + if (direction[0] >= 0) + { + voxel_max[0] += leaf_size_[0] * 0.5f; + step_x = 1; + } + else + { + voxel_max[0] -= leaf_size_[0] * 0.5f; + step_x = -1; + } + if (direction[1] >= 0) + { + voxel_max[1] += leaf_size_[1] * 0.5f; + step_y = 1; + } + else + { + voxel_max[1] -= leaf_size_[1] * 0.5f; + step_y = -1; + } + if (direction[2] >= 0) + { + voxel_max[2] += leaf_size_[2] * 0.5f; + step_z = 1; + } + else + { + voxel_max[2] -= leaf_size_[2] * 0.5f; + step_z = -1; + } + + float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0]; + float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1]; + float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2]; + + float t_delta_x = leaf_size_[0] / static_cast (std::abs (direction[0])); + float t_delta_y = leaf_size_[1] / static_cast (std::abs (direction[1])); + float t_delta_z = leaf_size_[2] / static_cast (std::abs (direction[2])); + + while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && + (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) && + (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) ) + { + // check if we reached target voxel + if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2]) + return 0; + + // index of the point in the point cloud + int index = this->getCentroidIndexAt (ijk); + // check if voxel is occupied, if yes return 1 for occluded + if (index != -1) + return 1; + + // estimate next voxel + if(t_max_x <= t_max_y && t_max_x <= t_max_z) + { + t_max_x += t_delta_x; + ijk[0] += step_x; + } + else if(t_max_y <= t_max_z && t_max_y <= t_max_x) + { + t_max_y += t_delta_y; + ijk[1] += step_y; + } + else + { + t_max_z += t_delta_z; + ijk[2] += step_z; + } + } + return 0; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::VoxelGridOcclusionEstimation::rayTraversal (std::vector >& out_ray, + const Eigen::Vector3i& target_voxel, + const Eigen::Vector4f& origin, + const Eigen::Vector4f& direction, + const float t_min) +{ + // reserve space for the ray vector + int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff (); + out_ray.reserve (reserve_size); + + // coordinate of the boundary of the voxel grid + Eigen::Vector4f start = origin + t_min * direction; + + // i,j,k coordinate of the voxel were the ray enters the voxel grid + Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]); + //Eigen::Vector3i ijk = this->getGridCoordinates (start_x, start_y, start_z); + + // steps in which direction we have to travel in the voxel grid + int step_x, step_y, step_z; + + // centroid coordinate of the entry voxel + Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk); + + if (direction[0] >= 0) + { + voxel_max[0] += leaf_size_[0] * 0.5f; + step_x = 1; + } + else + { + voxel_max[0] -= leaf_size_[0] * 0.5f; + step_x = -1; + } + if (direction[1] >= 0) + { + voxel_max[1] += leaf_size_[1] * 0.5f; + step_y = 1; + } + else + { + voxel_max[1] -= leaf_size_[1] * 0.5f; + step_y = -1; + } + if (direction[2] >= 0) + { + voxel_max[2] += leaf_size_[2] * 0.5f; + step_z = 1; + } + else + { + voxel_max[2] -= leaf_size_[2] * 0.5f; + step_z = -1; + } + + float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0]; + float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1]; + float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2]; + + float t_delta_x = leaf_size_[0] / static_cast (std::abs (direction[0])); + float t_delta_y = leaf_size_[1] / static_cast (std::abs (direction[1])); + float t_delta_z = leaf_size_[2] / static_cast (std::abs (direction[2])); + + // the index of the cloud (-1 if empty) + int result = 0; + + while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && + (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) && + (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) ) + { + // add voxel to ray + out_ray.push_back (ijk); + + // check if we reached target voxel + if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2]) + break; + + // check if voxel is occupied + int index = this->getCentroidIndexAt (ijk); + if (index != -1) + result = 1; + + // estimate next voxel + if(t_max_x <= t_max_y && t_max_x <= t_max_z) + { + t_max_x += t_delta_x; + ijk[0] += step_x; + } + else if(t_max_y <= t_max_z && t_max_y <= t_max_x) + { + t_max_y += t_delta_y; + ijk[1] += step_y; + } + else + { + t_max_z += t_delta_z; + ijk[2] += step_z; + } + } + return result; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +#define PCL_INSTANTIATE_VoxelGridOcclusionEstimation(T) template class PCL_EXPORTS pcl::VoxelGridOcclusionEstimation; + +#endif // PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ diff --git a/deps_install/include/pcl-1.10/pcl/filters/local_maximum.h b/deps_install/include/pcl-1.10/pcl/filters/local_maximum.h new file mode 100755 index 0000000..75e8730 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/local_maximum.h @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief LocalMaximum downsamples the cloud, by eliminating points that are locally maximal. + * + * The LocalMaximum class analyzes each point and removes those that are + * found to be locally maximal with respect to their neighbors (found via + * radius search). The comparison is made in the z dimension only at this + * time. + * + * \author Bradley J Chambers + * \ingroup filters + */ + template + class LocalMaximum: public FilterIndices + { + protected: + using PointCloud = typename FilterIndices::PointCloud; + using SearcherPtr = typename pcl::search::Search::Ptr; + + public: + /** \brief Empty constructor. */ + LocalMaximum (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + searcher_ (), + radius_ (1) + { + filter_name_ = "LocalMaximum"; + } + + /** \brief Set the radius to use to determine if a point is the local max. + * \param[in] radius The radius to use to determine if a point is the local max. + */ + inline void + setRadius (float radius) { radius_ = radius; } + + /** \brief Get the radius to use to determine if a point is the local max. + * \return The radius to use to determine if a point is the local max. + */ + inline float + getRadius () const { return (radius_); } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Downsample a Point Cloud by eliminating points that are locally maximal in z + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) override + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + private: + /** \brief A pointer to the spatial search object. */ + SearcherPtr searcher_; + + /** \brief The radius to use to determine if a point is the local max. */ + float radius_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/median_filter.h b/deps_install/include/pcl-1.10/pcl/filters/median_filter.h new file mode 100755 index 0000000..8e08e50 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/median_filter.h @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + + * All rights reserved. + + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Implementation of the median filter. + * The median filter is one of the simplest and wide-spread image processing filters. It is known to perform well + * with "shot"/impulse noise (some individual pixels having extreme values), it does not reduce contrast across steps + * in the function (as compared to filters based on averaging), and it is robust to outliers. Furthermore, it is + * simple to implement and efficient, as it requires a single pass over the image. It consists of a moving window of + * fixed size that replaces the pixel in the center with the median inside the window. + * + * \note This algorithm filters only the depth (z-component) of _organized_ and untransformed (i.e., in camera coordinates) + * point clouds. An error will be outputted if an unorganized cloud is given to the class instance. + * + * \author Alexandru E. Ichim + * \ingroup filters + */ + template + class MedianFilter : public pcl::Filter + { + using pcl::Filter::input_; + using PointCloud = typename pcl::Filter::PointCloud; + + public: + /** \brief Empty constructor. */ + MedianFilter () + : window_size_ (5) + , max_allowed_movement_ (std::numeric_limits::max ()) + { } + + /** \brief Set the window size of the filter. + * \param[in] window_size the new window size + */ + inline void + setWindowSize (int window_size) + { window_size_ = window_size; } + + /** \brief Get the window size of the filter. + * \returns the window size of the filter + */ + inline int + getWindowSize () const + { return window_size_; } + + /** \brief Set the largest value one dexel is allowed to move + * \param[in] max_allowed_movement maximum value a dexel is allowed to move during filtering + */ + inline void + setMaxAllowedMovement (float max_allowed_movement) + { max_allowed_movement_ = max_allowed_movement; } + + /** \brief Get the maximum distance one point is allowed to move along the z-axis. + * \returns the maximum distance a dexel is allowed to move + */ + inline float + getMaxAllowedMovement () const + { return max_allowed_movement_; } + + /** \brief Filter the input data and store the results into output. + * \param[out] output the result point cloud + */ + void + applyFilter (PointCloud &output) override; + + protected: + int window_size_; + float max_allowed_movement_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#else +#define PCL_INSTANTIATE_MedianFilter(T) template class PCL_EXPORTS pcl::MedianFilter; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/model_outlier_removal.h b/deps_install/include/pcl-1.10/pcl/filters/model_outlier_removal.h new file mode 100755 index 0000000..31dcbd1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/model_outlier_removal.h @@ -0,0 +1,254 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +// Sample Consensus models +#include +#include + +namespace pcl +{ + /** \brief @b ModelOutlierRemoval filters points in a cloud based on the distance between model and point. + * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside + * the model specified by setSampleConsensusModelPointer() and the threshold specified by setThreholdFunctionPointer(). + *

+ * Usage example: + * \code + * pcl::ModelCoefficients model_coeff; + * model_coeff.values.resize(4); + * model_coeff.values[0] = 0; model_coeff.values[1] = 0; model_coeff.values[2] = 1.5; model_coeff.values[3] = 0.5; + * pcl::ModelOutlierRemoval filter; + * filter.setModelCoefficients (model_coeff); + * filter.setThreshold (0.1); + * filter.setModelType (pcl::SACMODEL_PLANE); + * filter.setInputCloud (*cloud_in); + * filter.setFilterLimitsNegative (false); + * filter.filter (*cloud_out); + * \endcode + */ + template + class ModelOutlierRemoval : public FilterIndices + { + protected: + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + public: + using PointCloudNPtr = pcl::PointCloud::Ptr; + using PointCloudNConstPtr = pcl::PointCloud::ConstPtr; + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + inline + ModelOutlierRemoval (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices) + { + thresh_ = 0; + normals_distance_weight_ = 0; + filter_name_ = "ModelOutlierRemoval"; + setThresholdFunction (&pcl::ModelOutlierRemoval::checkSingleThreshold, *this); + } + + /** \brief sets the models coefficients */ + inline void + setModelCoefficients (const pcl::ModelCoefficients model_coefficients) + { + model_coefficients_.resize (model_coefficients.values.size ()); + for (std::size_t i = 0; i < model_coefficients.values.size (); i++) + { + model_coefficients_[i] = model_coefficients.values[i]; + } + } + + /** \brief returns the models coefficients + */ + inline pcl::ModelCoefficients + getModelCoefficients () const + { + pcl::ModelCoefficients mc; + mc.values.resize (model_coefficients_.size ()); + for (std::size_t i = 0; i < mc.values.size (); i++) + mc.values[i] = model_coefficients_[i]; + return (mc); + } + + /** \brief Set the type of SAC model used. */ + inline void + setModelType (pcl::SacModel model) + { + model_type_ = model; + } + + /** \brief Get the type of SAC model used. */ + inline pcl::SacModel + getModelType () const + { + return (model_type_); + } + + /** \brief Set the thresholdfunction*/ + inline void + setThreshold (float thresh) + { + thresh_ = thresh; + } + + /** \brief Get the thresholdfunction*/ + inline float + getThreshold () const + { + return (thresh_); + } + + /** \brief Set the normals cloud*/ + inline void + setInputNormals (const PointCloudNConstPtr normals_ptr) + { + cloud_normals_ = normals_ptr; + } + + /** \brief Get the normals cloud*/ + inline PointCloudNConstPtr + getInputNormals () const + { + return (cloud_normals_); + } + + /** \brief Set the normals distance weight*/ + inline void + setNormalDistanceWeight (const double weight) + { + normals_distance_weight_ = weight; + } + + /** \brief get the normal distance weight*/ + inline double + getNormalDistanceWeight () const + { + return (normals_distance_weight_); + } + + /** \brief Register a different threshold function + * \param[in] thresh pointer to a threshold function + */ + void + setThresholdFunction (std::function thresh) + { + threshold_function_ = thresh; + } + + /** \brief Register a different threshold function + * \param[in] thresh_function pointer to a threshold function + * \param[in] instance + */ + template void + setThresholdFunction (bool (T::*thresh_function) (double), T& instance) + { + setThresholdFunction ([=, &instance] (double threshold) { return (instance.*thresh_function) (threshold); }); + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) override + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + protected: + double normals_distance_weight_; + PointCloudNConstPtr cloud_normals_; + + /** \brief The model used to calculate distances */ + SampleConsensusModelPtr model_; + + /** \brief The threshold used to separate outliers (removed_indices) from inliers (indices) */ + float thresh_; + + /** \brief The model coefficients */ + Eigen::VectorXf model_coefficients_; + + /** \brief The type of model to use (user given parameter). */ + pcl::SacModel model_type_; + std::function threshold_function_; + + inline bool + checkSingleThreshold (double value) + { + return (value < thresh_); + } + + private: + virtual bool + initSACModel (pcl::SacModel model_type); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/morphological_filter.h b/deps_install/include/pcl-1.10/pcl/filters/morphological_filter.h new file mode 100755 index 0000000..eb18f28 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/morphological_filter.h @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + enum MorphologicalOperators + { + MORPH_OPEN, + MORPH_CLOSE, + MORPH_DILATE, + MORPH_ERODE + }; +} + +namespace pcl +{ + /** \brief Apply morphological operator to the z dimension of the input point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] resolution the window size to be used for the morphological operation + * \param[in] morphological_operator the morphological operator to apply (open, close, dilate, erode) + * \param[out] cloud_out the resultant output point cloud dataset + * \ingroup filters + */ + template PCL_EXPORTS void + applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, + float resolution, const int morphological_operator, + pcl::PointCloud &cloud_out); +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/normal_refinement.h b/deps_install/include/pcl-1.10/pcl/filters/normal_refinement.h new file mode 100755 index 0000000..5d88ef3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/normal_refinement.h @@ -0,0 +1,305 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Assign weights of nearby normals used for refinement + * \todo Currently, this function equalizes all weights to 1 + * @param cloud the point cloud data + * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point + * @param k_indices indices of neighboring points + * @param k_sqr_distances squared distances to the neighboring points + * @return weights + * \ingroup filters + */ + template inline std::vector + assignNormalWeights (const PointCloud&, + int, + const std::vector& k_indices, + const std::vector& k_sqr_distances) + { + // Check inputs + if (k_indices.size () != k_sqr_distances.size ()) + PCL_ERROR("[pcl::assignNormalWeights] inequal size of neighbor indices and distances!\n"); + + // TODO: For now we use uniform weights + return (std::vector (k_indices.size (), 1.0f)); + } + + /** \brief Refine an indexed point based on its neighbors, this function only writes to the normal_* fields + * + * \note If the indexed point has only NaNs in its neighborhood, the resulting normal will be zero. + * + * @param cloud the point cloud data + * @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point + * @param k_indices indices of neighboring points + * @param k_sqr_distances squared distances to the neighboring points + * @param point the output point, only normal_* fields are written + * @return false if an error occurred (norm of summed normals zero or all neighbors NaN) + * \ingroup filters + */ + template inline bool + refineNormal (const PointCloud& cloud, + int index, + const std::vector& k_indices, + const std::vector& k_sqr_distances, + NormalT& point) + { + // Start by zeroing result + point.normal_x = 0.0f; + point.normal_y = 0.0f; + point.normal_z = 0.0f; + + // Check inputs + if (k_indices.size () != k_sqr_distances.size ()) + { + PCL_ERROR("[pcl::refineNormal] inequal size of neighbor indices and distances!\n"); + return (false); + } + + // Get weights + const std::vector weights = assignNormalWeights (cloud, index, k_indices, k_sqr_distances); + + // Loop over all neighbors and accumulate sum of normal components + float nx = 0.0f; + float ny = 0.0f; + float nz = 0.0f; + for (std::size_t i = 0; i < k_indices.size (); ++i) { + // Neighbor + const NormalT& pointi = cloud[k_indices[i]]; + + // Accumulate if not NaN + if (std::isfinite (pointi.normal_x) && std::isfinite (pointi.normal_y) && std::isfinite (pointi.normal_z)) + { + const float& weighti = weights[i]; + nx += weighti * pointi.normal_x; + ny += weighti * pointi.normal_y; + nz += weighti * pointi.normal_z; + } + } + + // Normalize if norm valid and non-zero + const float norm = std::sqrt (nx * nx + ny * ny + nz * nz); + if (std::isfinite (norm) && norm > std::numeric_limits::epsilon ()) + { + point.normal_x = nx / norm; + point.normal_y = ny / norm; + point.normal_z = nz / norm; + + return (true); + } + + return (false); + } + + /** \brief %Normal vector refinement class + * + * This class refines a set of already estimated normals by iteratively updating each normal to the (weighted) + * mean of all normals in its neighborhood. The intention is that you reuse the same point correspondences + * as used when estimating the original normals in order to avoid repeating a nearest neighbor search. + * + * \note This class avoids points for which a NaN is encountered in the neighborhood. In the special case + * where a point has only NaNs in its neighborhood, the resultant refined normal will be set to zero, + * i.e. this class only produces finite normals. + * + * \details Usage example: + * + * \code + * // Input point cloud + * pcl::PointCloud cloud; + * + * // Fill cloud... + * + * // Estimated and refined normals + * pcl::PointCloud normals; + * pcl::PointCloud normals_refined; + * + * // Search parameters + * const int k = 5; + * std::vector > k_indices; + * std::vector > k_sqr_distances; + * + * // Run search + * pcl::search::KdTree search; + * search.setInputCloud (cloud.makeShared ()); + * search.nearestKSearch (cloud, std::vector (), k, k_indices, k_sqr_distances); + * + * // Use search results for normal estimation + * pcl::NormalEstimation ne; + * for (unsigned int i = 0; i < cloud.size (); ++i) + * { + * NormalT normal; + * ne.computePointNormal (cloud, k_indices[i] + * normal.normal_x, normal.normal_y, normal.normal_z, normal.curvature); + * pcl::flipNormalTowardsViewpoint (cloud[i], cloud.sensor_origin_[0], cloud.sensor_origin_[1], cloud.sensor_origin_[2], + * normal.normal_x, normal.normal_y, normal.normal_z); + * normals.push_back (normal); + * } + * + * // Run refinement using search results + * pcl::NormalRefinement nr (k_indices, k_sqr_distances); + * nr.setInputCloud (normals.makeShared ()); + * nr.filter (normals_refined); + * \endcode + * + * \author Anders Glent Buch + * \ingroup filters + */ + template + class NormalRefinement : public Filter + { + using Filter::input_; + using Filter::filter_name_; + using Filter::getClassName; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + /** \brief Empty constructor, sets default convergence parameters + */ + NormalRefinement () : + Filter::Filter () + { + filter_name_ = "NormalRefinement"; + setMaxIterations (15); + setConvergenceThreshold (0.00001f); + } + + /** \brief Constructor for setting correspondences, sets default convergence parameters + * @param k_indices indices of neighboring points + * @param k_sqr_distances squared distances to the neighboring points + */ + NormalRefinement (const std::vector< std::vector >& k_indices, const std::vector< std::vector >& k_sqr_distances) : + Filter::Filter () + { + filter_name_ = "NormalRefinement"; + setCorrespondences (k_indices, k_sqr_distances); + setMaxIterations (15); + setConvergenceThreshold (0.00001f); + } + + /** \brief Set correspondences calculated from nearest neighbor search + * @param k_indices indices of neighboring points + * @param k_sqr_distances squared distances to the neighboring points + */ + inline void + setCorrespondences (const std::vector< std::vector >& k_indices, const std::vector< std::vector >& k_sqr_distances) + { + k_indices_ = k_indices; + k_sqr_distances_ = k_sqr_distances; + } + + /** \brief Get correspondences (copy) + * @param k_indices indices of neighboring points + * @param k_sqr_distances squared distances to the neighboring points + */ + inline void + getCorrespondences (std::vector< std::vector >& k_indices, std::vector< std::vector >& k_sqr_distances) + { + k_indices.assign (k_indices_.begin (), k_indices_.end ()); + k_sqr_distances.assign (k_sqr_distances_.begin (), k_sqr_distances_.end ()); + } + + /** \brief Set maximum iterations + * @param max_iterations maximum iterations + */ + inline void + setMaxIterations (unsigned int max_iterations) + { + max_iterations_ = max_iterations; + } + + /** \brief Get maximum iterations + * @return maximum iterations + */ + inline unsigned int + getMaxIterations () + { + return max_iterations_; + } + + /** \brief Set convergence threshold + * @param convergence_threshold convergence threshold + */ + inline void + setConvergenceThreshold (float convergence_threshold) + { + convergence_threshold_ = convergence_threshold; + } + + /** \brief Get convergence threshold + * @return convergence threshold + */ + inline float + getConvergenceThreshold () + { + return convergence_threshold_; + } + + protected: + /** \brief Filter a Point Cloud. + * \param output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + + private: + /** \brief indices of neighboring points */ + std::vector< std::vector > k_indices_; + + /** \brief squared distances to the neighboring points */ + std::vector< std::vector > k_sqr_distances_; + + /** \brief Maximum allowable iterations over the whole point cloud for refinement */ + unsigned int max_iterations_; + + /** \brief Convergence threshold in the interval [0,1] on the mean of 1 - dot products between previous iteration and the current */ + float convergence_threshold_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#else +#define PCL_INSTANTIATE_NormalRefinement(T) template class PCL_EXPORTS pcl::NormalRefinement; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/normal_space.h b/deps_install/include/pcl-1.10/pcl/filters/normal_space.h new file mode 100755 index 0000000..3a3f65e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/normal_space.h @@ -0,0 +1,205 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief @b NormalSpaceSampling samples the input point cloud in the space of normal directions computed at every point. + * \ingroup filters + */ + template + class NormalSpaceSampling : public FilterIndices + { + using FilterIndices::filter_name_; + using FilterIndices::getClassName; + using FilterIndices::indices_; + using FilterIndices::input_; + using FilterIndices::keep_organized_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + using FilterIndices::user_filter_value_; + + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using NormalsConstPtr = typename pcl::PointCloud::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + NormalSpaceSampling () + : sample_ (std::numeric_limits::max ()) + , seed_ (static_cast (time (nullptr))) + , binsx_ () + , binsy_ () + , binsz_ () + , input_normals_ () + , rng_uniform_distribution_ (nullptr) + { + filter_name_ = "NormalSpaceSampling"; + } + + /** \brief Destructor. */ + ~NormalSpaceSampling () + { + delete rng_uniform_distribution_; + } + + /** \brief Set number of indices to be sampled. + * \param[in] sample the number of sample indices + */ + inline void + setSample (unsigned int sample) + { sample_ = sample; } + + /** \brief Get the value of the internal \a sample parameter. */ + inline unsigned int + getSample () const + { return (sample_); } + + /** \brief Set seed of random function. + * \param[in] seed the input seed + */ + inline void + setSeed (unsigned int seed) + { seed_ = seed; } + + /** \brief Get the value of the internal \a seed parameter. */ + inline unsigned int + getSeed () const + { return (seed_); } + + /** \brief Set the number of bins in x, y and z direction + * \param[in] binsx number of bins in x direction + * \param[in] binsy number of bins in y direction + * \param[in] binsz number of bins in z direction + */ + inline void + setBins (unsigned int binsx, unsigned int binsy, unsigned int binsz) + { + binsx_ = binsx; + binsy_ = binsy; + binsz_ = binsz; + } + + /** \brief Get the number of bins in x, y and z direction + * \param[out] binsx number of bins in x direction + * \param[out] binsy number of bins in y direction + * \param[out] binsz number of bins in z direction + */ + inline void + getBins (unsigned int& binsx, unsigned int& binsy, unsigned int& binsz) const + { + binsx = binsx_; + binsy = binsy_; + binsz = binsz_; + } + + /** \brief Set the normals computed on the input point cloud + * \param[in] normals the normals computed for the input cloud + */ + inline void + setNormals (const NormalsConstPtr &normals) { input_normals_ = normals; } + + /** \brief Get the normals computed on the input point cloud */ + inline NormalsConstPtr + getNormals () const { return (input_normals_); } + + protected: + /** \brief Number of indices that will be returned. */ + unsigned int sample_; + /** \brief Random number seed. */ + unsigned int seed_; + + /** \brief Number of bins in x direction. */ + unsigned int binsx_; + /** \brief Number of bins in y direction. */ + unsigned int binsy_; + /** \brief Number of bins in z direction. */ + unsigned int binsz_; + + /** \brief The normals computed at each point in the input cloud */ + NormalsConstPtr input_normals_; + + /** \brief Sample of point indices into a separate PointCloud + * \param[out] output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Sample of point indices + * \param[out] indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + + bool + initCompute (); + + private: + /** \brief Finds the bin number of the input normal, returns the bin number + * \param[in] normal the input normal + * \param[in] nbins total number of bins + */ + unsigned int + findBin (const float *normal, unsigned int nbins); + + /** \brief Checks of the entire bin is sampled, returns true or false + * \param[out] array flag which says whether a point is sampled or not + * \param[in] start_index the index to the first point of the bin in array. + * \param[in] length number of points in the bin + */ + bool + isEntireBinSampled (boost::dynamic_bitset<> &array, unsigned int start_index, unsigned int length); + + /** \brief Uniform random distribution. */ + boost::variate_generator > *rng_uniform_distribution_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/passthrough.h b/deps_install/include/pcl-1.10/pcl/filters/passthrough.h new file mode 100755 index 0000000..3f5e2b6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/passthrough.h @@ -0,0 +1,339 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b PassThrough passes points in a cloud based on constraints for one particular field of the point type. + * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside + * the interval specified by setFilterLimits(), which applies only to the field specified by setFilterFieldName(). + *

+ * Usage example: + * \code + * pcl::PassThrough ptfilter (true); // Initializing with true will allow us to extract the removed indices + * ptfilter.setInputCloud (cloud_in); + * ptfilter.setFilterFieldName ("x"); + * ptfilter.setFilterLimits (0.0, 1000.0); + * ptfilter.filter (*indices_x); + * // The indices_x array indexes all points of cloud_in that have x between 0.0 and 1000.0 + * indices_rem = ptfilter.getRemovedIndices (); + * // The indices_rem array indexes all points of cloud_in that have x smaller than 0.0 or larger than 1000.0 + * // and also indexes all non-finite points of cloud_in + * ptfilter.setIndices (indices_x); + * ptfilter.setFilterFieldName ("z"); + * ptfilter.setFilterLimits (-10.0, 10.0); + * ptfilter.setNegative (true); + * ptfilter.filter (*indices_xz); + * // The indices_xz array indexes all points of cloud_in that have x between 0.0 and 1000.0 and z larger than 10.0 or smaller than -10.0 + * ptfilter.setIndices (indices_xz); + * ptfilter.setFilterFieldName ("intensity"); + * ptfilter.setFilterLimits (FLT_MIN, 0.5); + * ptfilter.setNegative (false); + * ptfilter.filter (*cloud_out); + * // The resulting cloud_out contains all points of cloud_in that are finite and have: + * // x between 0.0 and 1000.0, z larger than 10.0 or smaller than -10.0 and intensity smaller than 0.5. + * \endcode + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template + class PassThrough : public FilterIndices + { + protected: + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using FieldList = typename pcl::traits::fieldList::type; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + PassThrough (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + filter_field_name_ (""), + filter_limit_min_ (FLT_MIN), + filter_limit_max_ (FLT_MAX) + { + filter_name_ = "PassThrough"; + } + + /** \brief Provide the name of the field to be used for filtering data. + * \details In conjunction with setFilterLimits(), points having values outside this interval for this field will be discarded. + * \param[in] field_name The name of the field that will be used for filtering. + */ + inline void + setFilterFieldName (const std::string &field_name) + { + filter_field_name_ = field_name; + } + + /** \brief Retrieve the name of the field to be used for filtering data. + * \return The name of the field that will be used for filtering. + */ + inline std::string const + getFilterFieldName () const + { + return (filter_field_name_); + } + + /** \brief Set the numerical limits for the field for filtering data. + * \details In conjunction with setFilterFieldName(), points having values outside this interval for this field will be discarded. + * \param[in] limit_min The minimum allowed field value (default = FLT_MIN). + * \param[in] limit_max The maximum allowed field value (default = FLT_MAX). + */ + inline void + setFilterLimits (const float &limit_min, const float &limit_max) + { + filter_limit_min_ = limit_min; + filter_limit_max_ = limit_max; + } + + /** \brief Get the numerical limits for the field for filtering data. + * \param[out] limit_min The minimum allowed field value (default = FLT_MIN). + * \param[out] limit_max The maximum allowed field value (default = FLT_MAX). + */ + inline void + getFilterLimits (float &limit_min, float &limit_max) const + { + limit_min = filter_limit_min_; + limit_max = filter_limit_max_; + } + + /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max) + * Default: false. + * \warning This method will be removed in the future. Use setNegative() instead. + * \param[in] limit_negative return data inside the interval (false) or outside (true) + */ + inline void + setFilterLimitsNegative (const bool limit_negative) + { + negative_ = limit_negative; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \warning This method will be removed in the future. Use getNegative() instead. + * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline void + getFilterLimitsNegative (bool &limit_negative) const + { + limit_negative = negative_; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \warning This method will be removed in the future. Use getNegative() instead. + * \return true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline bool + getFilterLimitsNegative () const + { + return (negative_); + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) override + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + private: + /** \brief The name of the field that will be used for filtering. */ + std::string filter_field_name_; + + /** \brief The minimum allowed field value (default = FLT_MIN). */ + float filter_limit_min_; + + /** \brief The maximum allowed field value (default = FLT_MIN). */ + float filter_limit_max_; + }; + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief PassThrough uses the base Filter class methods to pass through all data that satisfies the user given + * constraints. + * \author Radu B. Rusu + * \ingroup filters + */ + template<> + class PCL_EXPORTS PassThrough : public FilterIndices + { + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + using Filter::removed_indices_; + using Filter::extract_removed_indices_; + + public: + /** \brief Constructor. */ + PassThrough (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + filter_field_name_ (""), filter_limit_min_ (-FLT_MAX), filter_limit_max_ (FLT_MAX) + { + filter_name_ = "PassThrough"; + } + + /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits, + * points having values outside this interval will be discarded. + * \param[in] field_name the name of the field that contains values used for filtering + */ + inline void + setFilterFieldName (const std::string &field_name) + { + filter_field_name_ = field_name; + } + + /** \brief Get the name of the field used for filtering. */ + inline std::string const + getFilterFieldName () const + { + return (filter_field_name_); + } + + /** \brief Set the field filter limits. All points having field values outside this interval will be discarded. + * \param[in] limit_min the minimum allowed field value + * \param[in] limit_max the maximum allowed field value + */ + inline void + setFilterLimits (const double &limit_min, const double &limit_max) + { + filter_limit_min_ = limit_min; + filter_limit_max_ = limit_max; + } + + /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX. + * \param[out] limit_min the minimum allowed field value + * \param[out] limit_max the maximum allowed field value + */ + inline void + getFilterLimits (double &limit_min, double &limit_max) const + { + limit_min = filter_limit_min_; + limit_max = filter_limit_max_; + } + + /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max). + * Default: false. + * \param[in] limit_negative return data inside the interval (false) or outside (true) + */ + PCL_DEPRECATED("use inherited FilterIndices::setNegative() instead") + inline void + setFilterLimitsNegative (const bool limit_negative) + { + negative_ = limit_negative; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead") + inline void + getFilterLimitsNegative (bool &limit_negative) const + { + limit_negative = negative_; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \return true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead") + inline bool + getFilterLimitsNegative () const + { + return (negative_); + } + + protected: + void + applyFilter (PCLPointCloud2 &output) override; + + void + applyFilter (std::vector &indices) override; + + private: + /** \brief The desired user filter field name. */ + std::string filter_field_name_; + + /** \brief The minimum allowed filter value a point will be considered from. */ + double filter_limit_min_; + + /** \brief The maximum allowed filter value a point will be considered from. */ + double filter_limit_max_; + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/plane_clipper3D.h b/deps_install/include/pcl-1.10/pcl/filters/plane_clipper3D.h new file mode 100755 index 0000000..eb73227 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/plane_clipper3D.h @@ -0,0 +1,105 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "clipper3D.h" + +namespace pcl +{ + /** + * @author Suat Gedikli + * @brief Implementation of a plane clipper in 3D + * \ingroup filters + */ + template + class PlaneClipper3D : public Clipper3D + { + public: + + using Ptr = shared_ptr< PlaneClipper3D >; + using ConstPtr = shared_ptr< const PlaneClipper3D >; + + /** + * @author Suat Gedikli + * @brief Constructor taking the homogeneous representation of the plane as a Eigen::Vector4f + * @param[in] plane_params plane parameters, need not necessarily be normalized + */ + PlaneClipper3D (const Eigen::Vector4f& plane_params); + + virtual ~PlaneClipper3D () noexcept; + + /** + * \brief Set new plane parameters + * \param plane_params + */ + void setPlaneParameters (const Eigen::Vector4f& plane_params); + + /** + * \brief return the current plane parameters + * \return the current plane parameters + */ + const Eigen::Vector4f& getPlaneParameters () const; + + virtual bool + clipPoint3D (const PointT& point) const; + + virtual bool + clipLineSegment3D (PointT& from, PointT& to) const; + + virtual void + clipPlanarPolygon3D (std::vector >& polygon) const; + + virtual void + clipPlanarPolygon3D (const std::vector >& polygon, std::vector >& clipped_polygon) const; + + virtual void + clipPointCloud3D (const pcl::PointCloud &cloud_in, std::vector& clipped, const std::vector& indices = std::vector ()) const; + + virtual Clipper3D* + clone () const; + + protected: + float + getDistance (const PointT& point) const; + + private: + Eigen::Vector4f plane_params_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/filters/project_inliers.h b/deps_install/include/pcl-1.10/pcl/filters/project_inliers.h new file mode 100755 index 0000000..ef02660 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/project_inliers.h @@ -0,0 +1,289 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +// Sample Consensus models +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a + * separate PointCloud. + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template + class ProjectInliers : public Filter + { + using Filter::input_; + using Filter::indices_; + using Filter::filter_name_; + using Filter::getClassName; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Empty constructor. */ + ProjectInliers () : sacmodel_ (), model_type_ (), copy_all_data_ (false) + { + filter_name_ = "ProjectInliers"; + } + + /** \brief Empty destructor */ + ~ProjectInliers () {} + + /** \brief The type of model to use (user given parameter). + * \param model the model type (check \a model_types.h) + */ + inline void + setModelType (int model) + { + model_type_ = model; + } + + /** \brief Get the type of SAC model used. */ + inline int + getModelType () + { + return (model_type_); + } + + /** \brief Provide a pointer to the model coefficients. + * \param model a pointer to the model coefficients + */ + inline void + setModelCoefficients (const ModelCoefficientsConstPtr &model) + { + model_ = model; + } + + /** \brief Get a pointer to the model coefficients. */ + inline ModelCoefficientsConstPtr + getModelCoefficients () + { + return (model_); + } + + /** \brief Set whether all data will be returned, or only the projected inliers. + * \param val true if all data should be returned, false if only the projected inliers + */ + inline void + setCopyAllData (bool val) + { + copy_all_data_ = val; + } + + /** \brief Get whether all data is being copied (true), or only the projected inliers (false). */ + inline bool + getCopyAllData () + { + return (copy_all_data_); + } + protected: + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Project point indices into a separate PointCloud + * \param output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + + private: + /** \brief A pointer to the vector of model coefficients. */ + ModelCoefficientsConstPtr model_; + + /** \brief The model that needs to be segmented. */ + SampleConsensusModelPtr sacmodel_; + + /** \brief The type of model to use (user given parameter). */ + int model_type_; + + /** \brief True if all data will be returned, false if only the projected inliers. Default: false. */ + bool copy_all_data_; + + /** \brief Initialize the Sample Consensus model and set its parameters. + * \param model_type the type of SAC model that is to be used + */ + virtual bool + initSACModel (int model_type); + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a + * separate PointCloud. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template<> + class PCL_EXPORTS ProjectInliers : public Filter + { + using Filter::filter_name_; + using Filter::getClassName; + + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + using SampleConsensusModelPtr = SampleConsensusModel::Ptr; + + public: + /** \brief Empty constructor. */ + ProjectInliers () : model_type_ (), copy_all_data_ (false), copy_all_fields_ (true) + { + filter_name_ = "ProjectInliers"; + } + + /** \brief Empty destructor */ + ~ProjectInliers () {} + + /** \brief The type of model to use (user given parameter). + * \param[in] model the model type (check \a model_types.h) + */ + inline void + setModelType (int model) + { + model_type_ = model; + } + + /** \brief Get the type of SAC model used. */ + inline int + getModelType () const + { + return (model_type_); + } + + /** \brief Provide a pointer to the model coefficients. + * \param[in] model a pointer to the model coefficients + */ + inline void + setModelCoefficients (const ModelCoefficientsConstPtr &model) + { + model_ = model; + } + + /** \brief Get a pointer to the model coefficients. */ + inline ModelCoefficientsConstPtr + getModelCoefficients () const + { + return (model_); + } + + /** \brief Set whether all fields should be copied, or only the XYZ. + * \param[in] val true if all fields will be returned, false if only XYZ + */ + inline void + setCopyAllFields (bool val) + { + copy_all_fields_ = val; + } + + /** \brief Get whether all fields are being copied (true), or only XYZ (false). */ + inline bool + getCopyAllFields () const + { + return (copy_all_fields_); + } + + /** \brief Set whether all data will be returned, or only the projected inliers. + * \param[in] val true if all data should be returned, false if only the projected inliers + */ + inline void + setCopyAllData (bool val) + { + copy_all_data_ = val; + } + + /** \brief Get whether all data is being copied (true), or only the projected inliers (false). */ + inline bool + getCopyAllData () const + { + return (copy_all_data_); + } + protected: + /** \brief The type of model to use (user given parameter). */ + int model_type_; + + /** \brief True if all data will be returned, false if only the projected inliers. Default: false. */ + bool copy_all_data_; + + /** \brief True if all fields will be returned, false if only XYZ. Default: true. */ + bool copy_all_fields_; + + /** \brief A pointer to the vector of model coefficients. */ + ModelCoefficientsConstPtr model_; + + void + applyFilter (PCLPointCloud2 &output) override; + + private: + /** \brief The model that needs to be segmented. */ + SampleConsensusModelPtr sacmodel_; + + virtual bool + initSACModel (int model_type); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/radius_outlier_removal.h b/deps_install/include/pcl-1.10/pcl/filters/radius_outlier_removal.h new file mode 100755 index 0000000..d87d74b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/radius_outlier_removal.h @@ -0,0 +1,273 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b RadiusOutlierRemoval filters points in a cloud based on the number of neighbors they have. + * \details Iterates through the entire input once, and for each point, retrieves the number of neighbors within a certain radius. + * The point will be considered an outlier if it has too few neighbors, as determined by setMinNeighborsInRadius(). + * The radius can be changed using setRadiusSearch(). + *
+ * The neighbors found for each query point will be found amongst ALL points of setInputCloud(), not just those indexed by setIndices(). + * The setIndices() method only indexes the points that will be iterated through as search query points. + *

+ * Usage example: + * \code + * pcl::RadiusOutlierRemoval rorfilter (true); // Initializing with true will allow us to extract the removed indices + * rorfilter.setInputCloud (cloud_in); + * rorfilter.setRadiusSearch (0.1); + * rorfilter.setMinNeighborsInRadius (5); + * rorfilter.setNegative (true); + * rorfilter.filter (*cloud_out); + * // The resulting cloud_out contains all points of cloud_in that have 4 or less neighbors within the 0.1 search radius + * indices_rem = rorfilter.getRemovedIndices (); + * // The indices_rem array indexes all points of cloud_in that have 5 or more neighbors within the 0.1 search radius + * \endcode + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template + class RadiusOutlierRemoval : public FilterIndices + { + protected: + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using SearcherPtr = typename pcl::search::Search::Ptr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + RadiusOutlierRemoval (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + searcher_ (), + search_radius_ (0.0), + min_pts_radius_ (1) + { + filter_name_ = "RadiusOutlierRemoval"; + } + + /** \brief Set the radius of the sphere that will determine which points are neighbors. + * \details The number of points within this distance from the query point will need to be equal or greater + * than setMinNeighborsInRadius() in order to be classified as an inlier point (i.e. will not be filtered). + * \param[in] radius The radius of the sphere for nearest neighbor searching. + */ + inline void + setRadiusSearch (double radius) + { + search_radius_ = radius; + } + + /** \brief Get the radius of the sphere that will determine which points are neighbors. + * \details The number of points within this distance from the query point will need to be equal or greater + * than setMinNeighborsInRadius() in order to be classified as an inlier point (i.e. will not be filtered). + * \return The radius of the sphere for nearest neighbor searching. + */ + inline double + getRadiusSearch () + { + return (search_radius_); + } + + /** \brief Set the number of neighbors that need to be present in order to be classified as an inlier. + * \details The number of points within setRadiusSearch() from the query point will need to be equal or greater + * than this number in order to be classified as an inlier point (i.e. will not be filtered). + * \param min_pts The minimum number of neighbors (default = 1). + */ + inline void + setMinNeighborsInRadius (int min_pts) + { + min_pts_radius_ = min_pts; + } + + /** \brief Get the number of neighbors that need to be present in order to be classified as an inlier. + * \details The number of points within setRadiusSearch() from the query point will need to be equal or greater + * than this number in order to be classified as an inlier point (i.e. will not be filtered). + * \return The minimum number of neighbors (default = 1). + */ + inline int + getMinNeighborsInRadius () + { + return (min_pts_radius_); + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) override + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + private: + /** \brief A pointer to the spatial search object. */ + SearcherPtr searcher_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered an inlier. */ + int min_pts_radius_; + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain + * search radius is smaller than a given K. + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template<> + class PCL_EXPORTS RadiusOutlierRemoval : public FilterIndices + { + using Filter::filter_name_; + using Filter::getClassName; + + using Filter::removed_indices_; + using Filter::extract_removed_indices_; + + using KdTree = pcl::search::Search; + using KdTreePtr = pcl::search::Search::Ptr; + + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + public: + /** \brief Empty constructor. */ + RadiusOutlierRemoval (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + search_radius_ (0.0), min_pts_radius_ (1) + { + filter_name_ = "RadiusOutlierRemoval"; + } + + /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors for filtering. + * \param radius the sphere radius that is to contain all k-nearest neighbors + */ + inline void + setRadiusSearch (double radius) + { + search_radius_ = radius; + } + + /** \brief Get the sphere radius used for determining the k-nearest neighbors. */ + inline double + getRadiusSearch () + { + return (search_radius_); + } + + /** \brief Set the minimum number of neighbors that a point needs to have in the given search radius in order to + * be considered an inlier (i.e., valid). + * \param min_pts the minimum number of neighbors + */ + inline void + setMinNeighborsInRadius (int min_pts) + { + min_pts_radius_ = min_pts; + } + + /** \brief Get the minimum number of neighbors that a point needs to have in the given search radius to be + * considered an inlier and avoid being filtered. + */ + inline double + getMinNeighborsInRadius () + { + return (min_pts_radius_); + } + + protected: + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered + * an inlier. + */ + int min_pts_radius_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr searcher_; + + void + applyFilter (PCLPointCloud2 &output) override; + + void + applyFilter (std::vector &indices) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/random_sample.h b/deps_install/include/pcl-1.10/pcl/filters/random_sample.h new file mode 100755 index 0000000..c023955 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/random_sample.h @@ -0,0 +1,240 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: extract_indices.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b RandomSample applies a random sampling with uniform probability. + * Based off Algorithm A from the paper "Faster Methods for Random Sampling" + * by Jeffrey Scott Vitter. The algorithm runs in O(N) and results in sorted + * indices + * http://www.ittc.ku.edu/~jsv/Papers/Vit84.sampling.pdf + * \author Justin Rosen + * \ingroup filters + */ + template + class RandomSample : public FilterIndices + { + using FilterIndices::filter_name_; + using FilterIndices::getClassName; + using FilterIndices::indices_; + using FilterIndices::input_; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + RandomSample (bool extract_removed_indices = false) : + FilterIndices (extract_removed_indices), + sample_ (UINT_MAX), + seed_ (static_cast (time (nullptr))) + { + filter_name_ = "RandomSample"; + } + + /** \brief Set number of indices to be sampled. + * \param sample + */ + inline void + setSample (unsigned int sample) + { + sample_ = sample; + } + + /** \brief Get the value of the internal \a sample parameter. + */ + inline unsigned int + getSample () + { + return (sample_); + } + + /** \brief Set seed of random function. + * \param seed + */ + inline void + setSeed (unsigned int seed) + { + seed_ = seed; + } + + /** \brief Get the value of the internal \a seed parameter. + */ + inline unsigned int + getSeed () + { + return (seed_); + } + + protected: + + /** \brief Number of indices that will be returned. */ + unsigned int sample_; + /** \brief Random number seed. */ + unsigned int seed_; + + /** \brief Sample of point indices into a separate PointCloud + * \param output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Sample of point indices + * \param indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + + /** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm. + * See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information. + */ + inline float + unifRand () + { + return (static_cast(rand () / double (RAND_MAX))); + //return (((214013 * seed_ + 2531011) >> 16) & 0x7FFF); + } + }; + + /** \brief @b RandomSample applies a random sampling with uniform probability. + * \author Justin Rosen + * \ingroup filters + */ + template<> + class PCL_EXPORTS RandomSample : public FilterIndices + { + using FilterIndices::filter_name_; + using FilterIndices::getClassName; + + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + RandomSample () : sample_ (UINT_MAX), seed_ (static_cast (time (nullptr))) + { + filter_name_ = "RandomSample"; + } + + /** \brief Set number of indices to be sampled. + * \param sample + */ + inline void + setSample (unsigned int sample) + { + sample_ = sample; + } + + /** \brief Get the value of the internal \a sample parameter. + */ + inline unsigned int + getSample () + { + return (sample_); + } + + /** \brief Set seed of random function. + * \param seed + */ + inline void + setSeed (unsigned int seed) + { + seed_ = seed; + } + + /** \brief Get the value of the internal \a seed parameter. + */ + inline unsigned int + getSeed () + { + return (seed_); + } + + protected: + + /** \brief Number of indices that will be returned. */ + unsigned int sample_; + /** \brief Random number seed. */ + unsigned int seed_; + + /** \brief Sample of point indices into a separate PointCloud + * \param output the resultant point cloud + */ + void + applyFilter (PCLPointCloud2 &output) override; + + /** \brief Sample of point indices + * \param indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + + /** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm. + * See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information. + */ + inline float + unifRand () + { + return (static_cast (rand () / double (RAND_MAX))); + } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/sampling_surface_normal.h b/deps_install/include/pcl-1.10/pcl/filters/sampling_surface_normal.h new file mode 100755 index 0000000..9afeaac --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/sampling_surface_normal.h @@ -0,0 +1,250 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b SamplingSurfaceNormal divides the input space into grids until each grid contains a maximum of N points, + * and samples points randomly within each grid. Normal is computed using the N points of each grid. All points + * sampled within a grid are assigned the same normal. + * + * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher) + * \ingroup filters + */ + template + class SamplingSurfaceNormal: public Filter + { + using Filter::filter_name_; + using Filter::getClassName; + using Filter::indices_; + using Filter::input_; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using Vector = Eigen::Matrix; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + SamplingSurfaceNormal () : + sample_ (10), seed_ (static_cast (time (nullptr))), ratio_ () + { + filter_name_ = "SamplingSurfaceNormal"; + srand (seed_); + } + + /** \brief Set maximum number of samples in each grid + * \param[in] sample maximum number of samples in each grid + */ + inline void + setSample (unsigned int sample) + { + sample_ = sample; + } + + /** \brief Get the value of the internal \a sample parameter. */ + inline unsigned int + getSample () const + { + return (sample_); + } + + /** \brief Set seed of random function. + * \param[in] seed the input seed + */ + inline void + setSeed (unsigned int seed) + { + seed_ = seed; + srand (seed_); + } + + /** \brief Get the value of the internal \a seed parameter. */ + inline unsigned int + getSeed () const + { + return (seed_); + } + + /** \brief Set ratio of points to be sampled in each grid + * \param[in] ratio sample the ratio of points to be sampled in each grid + */ + inline void + setRatio (float ratio) + { + ratio_ = ratio; + } + + /** \brief Get the value of the internal \a ratio parameter. */ + inline float + getRatio () const + { + return ratio_; + } + + protected: + + /** \brief Maximum number of samples in each grid. */ + unsigned int sample_; + /** \brief Random number seed. */ + unsigned int seed_; + /** \brief Ratio of points to be sampled in each grid */ + float ratio_; + + /** \brief Sample of point indices into a separate PointCloud + * \param[out] output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + private: + + /** \brief @b CompareDim is a comparator object for sorting across a specific dimension (i,.e X, Y or Z) + */ + struct CompareDim + { + /** \brief The dimension to sort */ + const int dim; + /** \brief The input point cloud to sort */ + const pcl::PointCloud & cloud; + + /** \brief Constructor. */ + CompareDim (const int dim, const pcl::PointCloud & cloud) : dim (dim), cloud (cloud) + { + } + + /** \brief The operator function for sorting. */ + bool + operator () (const int& p0, const int& p1) + { + if (dim == 0) + return (cloud.points[p0].x < cloud.points[p1].x); + if (dim == 1) + return (cloud.points[p0].y < cloud.points[p1].y); + if (dim == 2) + return (cloud.points[p0].z < cloud.points[p1].z); + return (false); + } + }; + + /** \brief Finds the max and min values in each dimension + * \param[in] cloud the input cloud + * \param[out] max_vec the max value vector + * \param[out] min_vec the min value vector + */ + void + findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec); + + /** \brief Recursively partition the point cloud, stopping when each grid contains less than sample_ points + * Points are randomly sampled when a grid is found + * \param cloud + * \param first + * \param last + * \param min_values + * \param max_values + * \param indices + * \param[out] outcloud output the resultant point cloud + */ + void + partition (const PointCloud& cloud, const int first, const int last, + const Vector min_values, const Vector max_values, + std::vector& indices, PointCloud& outcloud); + + /** \brief Randomly sample the points in each grid. + * \param[in] data + * \param[in] first + * \param[in] last + * \param[out] indices + * \param[out] output the resultant point cloud + */ + void + samplePartition (const PointCloud& data, const int first, const int last, + std::vector& indices, PointCloud& outcloud); + + /** \brief Returns the threshold for splitting in a given dimension. + * \param[in] cloud the input cloud + * \param[in] cut_dim the input dimension (0=x, 1=y, 2=z) + * \param[in] cut_index the input index in the cloud + */ + float + findCutVal (const PointCloud& cloud, const int cut_dim, const int cut_index); + + /** \brief Computes the normal for points in a grid. This is a port from features to avoid features dependency for + * filters + * \param[in] cloud The input cloud + * \param[out] normal the computed normal + * \param[out] curvature the computed curvature + */ + void + computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature); + + /** \brief Computes the covariance matrix for points in the cloud. This is a port from features to avoid features dependency for + * filters + * \param[in] cloud The input cloud + * \param[out] covariance_matrix the covariance matrix + * \param[out] centroid the centroid + */ + unsigned int + computeMeanAndCovarianceMatrix (const pcl::PointCloud &cloud, + Eigen::Matrix3f &covariance_matrix, + Eigen::Vector4f ¢roid); + + /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares + * plane normal and surface curvature. + * \param[in] covariance_matrix the 3x3 covariance matrix + * \param[out] (nx ny nz) plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0) + * \param[out] curvature the estimated surface curvature as a measure of + */ + void + solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + float &nx, float &ny, float &nz, float &curvature); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/shadowpoints.h b/deps_install/include/pcl-1.10/pcl/filters/shadowpoints.h new file mode 100755 index 0000000..f3cce2e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/shadowpoints.h @@ -0,0 +1,130 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b ShadowPoints removes the ghost points appearing on edge discontinuties + * + * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher) + * \ingroup filters + */ + template + class ShadowPoints : public FilterIndices + { + using FilterIndices::filter_name_; + using FilterIndices::getClassName; + using FilterIndices::indices_; + using FilterIndices::input_; + using FilterIndices::removed_indices_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::negative_; + using FilterIndices::user_filter_value_; + using FilterIndices::keep_organized_; + + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using NormalsPtr = typename pcl::PointCloud::Ptr; + + public: + + using Ptr = shared_ptr< ShadowPoints >; + using ConstPtr = shared_ptr< const ShadowPoints >; + + /** \brief Empty constructor. */ + ShadowPoints (bool extract_removed_indices = false) : + FilterIndices (extract_removed_indices), + input_normals_ (), + threshold_ (0.1f) + { + filter_name_ = "ShadowPoints"; + } + + /** \brief Set the normals computed on the input point cloud + * \param[in] normals the normals computed for the input cloud + */ + inline void + setNormals (const NormalsPtr &normals) { input_normals_ = normals; } + + /** \brief Get the normals computed on the input point cloud */ + inline NormalsPtr + getNormals () const { return (input_normals_); } + + /** \brief Set the threshold for shadow points rejection + * \param[in] threshold the threshold + */ + inline void + setThreshold (float threshold) { threshold_ = threshold; } + + /** \brief Get the threshold for shadow points rejection */ + inline float + getThreshold () const { return threshold_; } + + protected: + + /** \brief The normals computed at each point in the input cloud */ + NormalsPtr input_normals_; + + /** \brief Sample of point indices into a separate PointCloud + * \param[out] output the resultant point cloud + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Sample of point indices + * \param[out] indices the resultant point cloud indices + */ + void + applyFilter (std::vector &indices) override; + + private: + + /** \brief Threshold for shadow point rejection + */ + float threshold_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/statistical_outlier_removal.h b/deps_install/include/pcl-1.10/pcl/filters/statistical_outlier_removal.h new file mode 100755 index 0000000..08451b6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/statistical_outlier_removal.h @@ -0,0 +1,289 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. + * \details The algorithm iterates through the entire input twice: + * During the first iteration it will compute the average distance that each point has to its nearest k neighbors. + * The value of k can be set using setMeanK(). + * Next, the mean and standard deviation of all these distances are computed in order to determine a distance threshold. + * The distance threshold will be equal to: mean + stddev_mult * stddev. + * The multiplier for the standard deviation can be set using setStddevMulThresh(). + * During the next iteration the points will be classified as inlier or outlier if their average neighbor distance is below or above this threshold respectively. + *
+ * The neighbors found for each query point will be found amongst ALL points of setInputCloud(), not just those indexed by setIndices(). + * The setIndices() method only indexes the points that will be iterated through as search query points. + *

+ * For more information: + * - R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. + * Towards 3D Point Cloud Based Object Maps for Household Environments + * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. + *

+ * Usage example: + * \code + * pcl::StatisticalOutlierRemoval sorfilter (true); // Initializing with true will allow us to extract the removed indices + * sorfilter.setInputCloud (cloud_in); + * sorfilter.setMeanK (8); + * sorfilter.setStddevMulThresh (1.0); + * sorfilter.filter (*cloud_out); + * // The resulting cloud_out contains all points of cloud_in that have an average distance to their 8 nearest neighbors that is below the computed threshold + * // Using a standard deviation multiplier of 1.0 and assuming the average distances are normally distributed there is a 84.1% chance that a point will be an inlier + * indices_rem = sorfilter.getRemovedIndices (); + * // The indices_rem array indexes all points of cloud_in that are outliers + * \endcode + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template + class StatisticalOutlierRemoval : public FilterIndices + { + protected: + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using SearcherPtr = typename pcl::search::Search::Ptr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + StatisticalOutlierRemoval (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), + searcher_ (), + mean_k_ (1), + std_mul_ (0.0) + { + filter_name_ = "StatisticalOutlierRemoval"; + } + + /** \brief Set the number of nearest neighbors to use for mean distance estimation. + * \param[in] nr_k The number of points to use for mean distance estimation. + */ + inline void + setMeanK (int nr_k) + { + mean_k_ = nr_k; + } + + /** \brief Get the number of nearest neighbors to use for mean distance estimation. + * \return The number of points to use for mean distance estimation. + */ + inline int + getMeanK () + { + return (mean_k_); + } + + /** \brief Set the standard deviation multiplier for the distance threshold calculation. + * \details The distance threshold will be equal to: mean + stddev_mult * stddev. + * Points will be classified as inlier or outlier if their average neighbor distance is below or above this threshold respectively. + * \param[in] stddev_mult The standard deviation multiplier. + */ + inline void + setStddevMulThresh (double stddev_mult) + { + std_mul_ = stddev_mult; + } + + /** \brief Get the standard deviation multiplier for the distance threshold calculation. + * \details The distance threshold will be equal to: mean + stddev_mult * stddev. + * Points will be classified as inlier or outlier if their average neighbor distance is below or above this threshold respectively. + */ + inline double + getStddevMulThresh () + { + return (std_mul_); + } + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void + applyFilter (PointCloud &output) override; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) override + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + private: + /** \brief A pointer to the spatial search object. */ + SearcherPtr searcher_; + + /** \brief The number of points to use for mean distance estimation. */ + int mean_k_; + + /** \brief Standard deviations threshold (i.e., points outside of + * \f$ \mu \pm \sigma \cdot std\_mul \f$ will be marked as outliers). */ + double std_mul_; + }; + + /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more + * information check: + * - R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz. + * Towards 3D Point Cloud Based Object Maps for Household Environments + * Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge), 2008. + * + * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template<> + class PCL_EXPORTS StatisticalOutlierRemoval : public FilterIndices + { + using FilterIndices::filter_name_; + using FilterIndices::getClassName; + + using FilterIndices::removed_indices_; + using FilterIndices::extract_removed_indices_; + + using KdTree = pcl::search::Search; + using KdTreePtr = pcl::search::Search::Ptr; + + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + public: + /** \brief Empty constructor. */ + StatisticalOutlierRemoval (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices), mean_k_ (2), + std_mul_ (0.0) + { + filter_name_ = "StatisticalOutlierRemoval"; + } + + /** \brief Set the number of points (k) to use for mean distance estimation + * \param nr_k the number of points to use for mean distance estimation + */ + inline void + setMeanK (int nr_k) + { + mean_k_ = nr_k; + } + + /** \brief Get the number of points to use for mean distance estimation. */ + inline int + getMeanK () + { + return (mean_k_); + } + + /** \brief Set the standard deviation multiplier threshold. All points outside the + * \f[ \mu \pm \sigma \cdot std\_mul \f] + * will be considered outliers, where \f$ \mu \f$ is the estimated mean, + * and \f$ \sigma \f$ is the standard deviation. + * \param std_mul the standard deviation multiplier threshold + */ + inline void + setStddevMulThresh (double std_mul) + { + std_mul_ = std_mul; + } + + /** \brief Get the standard deviation multiplier threshold as set by the user. */ + inline double + getStddevMulThresh () + { + return (std_mul_); + } + + protected: + /** \brief The number of points to use for mean distance estimation. */ + int mean_k_; + + /** \brief Standard deviations threshold (i.e., points outside of + * \f$ \mu \pm \sigma \cdot std\_mul \f$ will be marked as outliers). + */ + double std_mul_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + void + applyFilter (std::vector &indices) override; + + void + applyFilter (PCLPointCloud2 &output) override; + + /** + * \brief Compute the statistical values used in both applyFilter methods. + * + * This method tries to avoid duplicate code. + */ + virtual void + generateStatistics (double& mean, double& variance, double& stddev, std::vector& distances); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/uniform_sampling.h b/deps_install/include/pcl-1.10/pcl/filters/uniform_sampling.h new file mode 100755 index 0000000..a32f669 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/uniform_sampling.h @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + /** \brief @b UniformSampling assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. + * + * The @b UniformSampling class creates a *3D voxel grid* (think about a voxel + * grid as a set of tiny 3D boxes in space) over the input point cloud data. + * Then, in each *voxel* (i.e., 3D box), all the points present will be + * approximated (i.e., *downsampled*) with their centroid. This approach is + * a bit slower than approximating them with the center of the voxel, but it + * represents the underlying surface more accurately. + * + * \author Radu Bogdan Rusu + * \ingroup filters + */ + template + class UniformSampling: public Filter + { + using PointCloud = typename Filter::PointCloud; + + using Filter::filter_name_; + using Filter::input_; + using Filter::indices_; + using Filter::removed_indices_; + using Filter::extract_removed_indices_; + using Filter::getClassName; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + UniformSampling (bool extract_removed_indices = false) : + Filter(extract_removed_indices), + leaves_ (), + leaf_size_ (Eigen::Vector4f::Zero ()), + inverse_leaf_size_ (Eigen::Vector4f::Zero ()), + min_b_ (Eigen::Vector4i::Zero ()), + max_b_ (Eigen::Vector4i::Zero ()), + div_b_ (Eigen::Vector4i::Zero ()), + divb_mul_ (Eigen::Vector4i::Zero ()), + search_radius_ (0) + { + filter_name_ = "UniformSampling"; + } + + /** \brief Destructor. */ + ~UniformSampling () + { + leaves_.clear(); + } + + /** \brief Set the 3D grid leaf size. + * \param radius the 3D grid leaf size + */ + virtual inline void + setRadiusSearch (double radius) + { + leaf_size_[0] = leaf_size_[1] = leaf_size_[2] = static_cast (radius); + // Avoid division errors + if (leaf_size_[3] == 0) + leaf_size_[3] = 1; + // Use multiplications instead of divisions + inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); + search_radius_ = radius; + } + + protected: + /** \brief Simple structure to hold an nD centroid and the number of points in a leaf. */ + struct Leaf + { + Leaf () : idx (-1) { } + int idx; + }; + + /** \brief The 3D grid leaves. */ + std::unordered_map leaves_; + + /** \brief The size of a leaf. */ + Eigen::Vector4f leaf_size_; + + /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */ + Eigen::Array4f inverse_leaf_size_; + + /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */ + Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief Downsample a Point Cloud using a voxelized grid approach + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/voxel_grid.h b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid.h new file mode 100755 index 0000000..0356bdb --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid.h @@ -0,0 +1,852 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Obtain the maximum and minimum points in 3D from a given point cloud. + * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset + * \param[in] x_idx the index of the X channel + * \param[in] y_idx the index of the Y channel + * \param[in] z_idx the index of the Z channel + * \param[out] min_pt the minimum data point + * \param[out] max_pt the maximum data point + */ + PCL_EXPORTS void + getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); + + /** \brief Obtain the maximum and minimum points in 3D from a given point cloud. + * \note Performs internal data filtering as well. + * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset + * \param[in] x_idx the index of the X channel + * \param[in] y_idx the index of the Y channel + * \param[in] z_idx the index of the Z channel + * \param[in] distance_field_name the name of the dimension to filter data along to + * \param[in] min_distance the minimum acceptable value in \a distance_field_name data + * \param[in] max_distance the maximum acceptable value in \a distance_field_name data + * \param[out] min_pt the minimum data point + * \param[out] max_pt the maximum data point + * \param[in] limit_negative \b false if data \b inside of the [min_distance; max_distance] interval should be + * considered, \b true otherwise. + */ + PCL_EXPORTS void + getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, + const std::string &distance_field_name, float min_distance, float max_distance, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); + + /** \brief Get the relative cell indices of the "upper half" 13 neighbors. + * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid + * \ingroup filters + */ + inline Eigen::MatrixXi + getHalfNeighborCellIndices () + { + Eigen::MatrixXi relative_coordinates (3, 13); + int idx = 0; + + // 0 - 8 + for (int i = -1; i < 2; i++) + { + for (int j = -1; j < 2; j++) + { + relative_coordinates (0, idx) = i; + relative_coordinates (1, idx) = j; + relative_coordinates (2, idx) = -1; + idx++; + } + } + // 9 - 11 + for (int i = -1; i < 2; i++) + { + relative_coordinates (0, idx) = i; + relative_coordinates (1, idx) = -1; + relative_coordinates (2, idx) = 0; + idx++; + } + // 12 + relative_coordinates (0, idx) = -1; + relative_coordinates (1, idx) = 0; + relative_coordinates (2, idx) = 0; + + return (relative_coordinates); + } + + /** \brief Get the relative cell indices of all the 26 neighbors. + * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid + * \ingroup filters + */ + inline Eigen::MatrixXi + getAllNeighborCellIndices () + { + Eigen::MatrixXi relative_coordinates = getHalfNeighborCellIndices (); + Eigen::MatrixXi relative_coordinates_all( 3, 26); + relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates; + relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates; + return (relative_coordinates_all); + } + + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions + * in a given pointcloud, without considering points outside of a distance threshold from the laser origin + * \param[in] cloud the point cloud data message + * \param[in] distance_field_name the field name that contains the distance values + * \param[in] min_distance the minimum distance a point will be considered from + * \param[in] max_distance the maximum distance a point will be considered to + * \param[out] min_pt the resultant minimum bounds + * \param[out] max_pt the resultant maximum bounds + * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered + * \ingroup filters + */ + template void + getMinMax3D (const typename pcl::PointCloud::ConstPtr &cloud, + const std::string &distance_field_name, float min_distance, float max_distance, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); + + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions + * in a given pointcloud, without considering points outside of a distance threshold from the laser origin + * \param[in] cloud the point cloud data message + * \param[in] indices the vector of indices to use + * \param[in] distance_field_name the field name that contains the distance values + * \param[in] min_distance the minimum distance a point will be considered from + * \param[in] max_distance the maximum distance a point will be considered to + * \param[out] min_pt the resultant minimum bounds + * \param[out] max_pt the resultant maximum bounds + * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered + * \ingroup filters + */ + template void + getMinMax3D (const typename pcl::PointCloud::ConstPtr &cloud, + const std::vector &indices, + const std::string &distance_field_name, float min_distance, float max_distance, + Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); + + /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. + * + * The VoxelGrid class creates a *3D voxel grid* (think about a voxel + * grid as a set of tiny 3D boxes in space) over the input point cloud data. + * Then, in each *voxel* (i.e., 3D box), all the points present will be + * approximated (i.e., *downsampled*) with their centroid. This approach is + * a bit slower than approximating them with the center of the voxel, but it + * represents the underlying surface more accurately. + * + * \author Radu B. Rusu, Bastian Steder + * \ingroup filters + */ + template + class VoxelGrid: public Filter + { + protected: + using Filter::filter_name_; + using Filter::getClassName; + using Filter::input_; + using Filter::indices_; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + VoxelGrid () : + leaf_size_ (Eigen::Vector4f::Zero ()), + inverse_leaf_size_ (Eigen::Array4f::Zero ()), + downsample_all_data_ (true), + save_leaf_layout_ (false), + min_b_ (Eigen::Vector4i::Zero ()), + max_b_ (Eigen::Vector4i::Zero ()), + div_b_ (Eigen::Vector4i::Zero ()), + divb_mul_ (Eigen::Vector4i::Zero ()), + filter_field_name_ (""), + filter_limit_min_ (-FLT_MAX), + filter_limit_max_ (FLT_MAX), + filter_limit_negative_ (false), + min_points_per_voxel_ (0) + { + filter_name_ = "VoxelGrid"; + } + + /** \brief Destructor. */ + ~VoxelGrid () + { + } + + /** \brief Set the voxel grid leaf size. + * \param[in] leaf_size the voxel grid leaf size + */ + inline void + setLeafSize (const Eigen::Vector4f &leaf_size) + { + leaf_size_ = leaf_size; + // Avoid division errors + if (leaf_size_[3] == 0) + leaf_size_[3] = 1; + // Use multiplications instead of divisions + inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); + } + + /** \brief Set the voxel grid leaf size. + * \param[in] lx the leaf size for X + * \param[in] ly the leaf size for Y + * \param[in] lz the leaf size for Z + */ + inline void + setLeafSize (float lx, float ly, float lz) + { + leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz; + // Avoid division errors + if (leaf_size_[3] == 0) + leaf_size_[3] = 1; + // Use multiplications instead of divisions + inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); + } + + /** \brief Get the voxel grid leaf size. */ + inline Eigen::Vector3f + getLeafSize () const { return (leaf_size_.head<3> ()); } + + /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. + * \param[in] downsample the new value (true/false) + */ + inline void + setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } + + /** \brief Get the state of the internal downsampling parameter (true if + * all fields need to be downsampled, false if just XYZ). + */ + inline bool + getDownsampleAllData () const { return (downsample_all_data_); } + + /** \brief Set the minimum number of points required for a voxel to be used. + * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used + */ + inline void + setMinimumPointsNumberPerVoxel (unsigned int min_points_per_voxel) { min_points_per_voxel_ = min_points_per_voxel; } + + /** \brief Return the minimum number of points required for a voxel to be used. + */ + inline unsigned int + getMinimumPointsNumberPerVoxel () const { return min_points_per_voxel_; } + + /** \brief Set to true if leaf layout information needs to be saved for later access. + * \param[in] save_leaf_layout the new value (true/false) + */ + inline void + setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; } + + /** \brief Returns true if leaf layout information will to be saved for later access. */ + inline bool + getSaveLeafLayout () const { return (save_leaf_layout_); } + + /** \brief Get the minimum coordinates of the bounding box (after + * filtering is performed). + */ + inline Eigen::Vector3i + getMinBoxCoordinates () const { return (min_b_.head<3> ()); } + + /** \brief Get the minimum coordinates of the bounding box (after + * filtering is performed). + */ + inline Eigen::Vector3i + getMaxBoxCoordinates () const { return (max_b_.head<3> ()); } + + /** \brief Get the number of divisions along all 3 axes (after filtering + * is performed). + */ + inline Eigen::Vector3i + getNrDivisions () const { return (div_b_.head<3> ()); } + + /** \brief Get the multipliers to be applied to the grid coordinates in + * order to find the centroid index (after filtering is performed). + */ + inline Eigen::Vector3i + getDivisionMultiplier () const { return (divb_mul_.head<3> ()); } + + /** \brief Returns the index in the resulting downsampled cloud of the specified point. + * + * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering + * performed, and that the point is inside the grid, to avoid invalid access (or use + * getGridCoordinates+getCentroidIndexAt) + * + * \param[in] p the point to get the index at + */ + inline int + getCentroidIndex (const PointT &p) const + { + return (leaf_layout_.at ((Eigen::Vector4i (static_cast (std::floor (p.x * inverse_leaf_size_[0])), + static_cast (std::floor (p.y * inverse_leaf_size_[1])), + static_cast (std::floor (p.z * inverse_leaf_size_[2])), 0) - min_b_).dot (divb_mul_))); + } + + /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates, + * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds). + * \param[in] reference_point the coordinates of the reference point (corresponding cell is allowed to be empty/out of bounds) + * \param[in] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell + * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed + */ + inline std::vector + getNeighborCentroidIndices (const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates) const + { + Eigen::Vector4i ijk (static_cast (std::floor (reference_point.x * inverse_leaf_size_[0])), + static_cast (std::floor (reference_point.y * inverse_leaf_size_[1])), + static_cast (std::floor (reference_point.z * inverse_leaf_size_[2])), 0); + Eigen::Array4i diff2min = min_b_ - ijk; + Eigen::Array4i diff2max = max_b_ - ijk; + std::vector neighbors (relative_coordinates.cols()); + for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++) + { + Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); + // checking if the specified cell is in the grid + if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) + neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted + else + neighbors[ni] = -1; // cell is out of bounds, consider it empty + } + return (neighbors); + } + + /** \brief Returns the layout of the leafs for fast access to cells relative to current position. + * \note position at (i-min_x) + (j-min_y)*div_x + (k-min_z)*div_x*div_y holds the index of the element at coordinates (i,j,k) in the grid (-1 if empty) + */ + inline std::vector + getLeafLayout () const { return (leaf_layout_); } + + /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). + * \param[in] x the X point coordinate to get the (i, j, k) index at + * \param[in] y the Y point coordinate to get the (i, j, k) index at + * \param[in] z the Z point coordinate to get the (i, j, k) index at + */ + inline Eigen::Vector3i + getGridCoordinates (float x, float y, float z) const + { + return (Eigen::Vector3i (static_cast (std::floor (x * inverse_leaf_size_[0])), + static_cast (std::floor (y * inverse_leaf_size_[1])), + static_cast (std::floor (z * inverse_leaf_size_[2])))); + } + + /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates. + * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty) + */ + inline int + getCentroidIndexAt (const Eigen::Vector3i &ijk) const + { + int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_); + if (idx < 0 || idx >= static_cast (leaf_layout_.size ())) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed + { + //if (verbose) + // PCL_ERROR ("[pcl::%s::getCentroidIndexAt] Specified coordinate is outside grid bounds, or leaf layout is not saved, make sure to call setSaveLeafLayout(true) and filter(output) first!\n", getClassName ().c_str ()); + return (-1); + } + return (leaf_layout_[idx]); + } + + /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits, + * points having values outside this interval will be discarded. + * \param[in] field_name the name of the field that contains values used for filtering + */ + inline void + setFilterFieldName (const std::string &field_name) + { + filter_field_name_ = field_name; + } + + /** \brief Get the name of the field used for filtering. */ + inline std::string const + getFilterFieldName () const + { + return (filter_field_name_); + } + + /** \brief Set the field filter limits. All points having field values outside this interval will be discarded. + * \param[in] limit_min the minimum allowed field value + * \param[in] limit_max the maximum allowed field value + */ + inline void + setFilterLimits (const double &limit_min, const double &limit_max) + { + filter_limit_min_ = limit_min; + filter_limit_max_ = limit_max; + } + + /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX. + * \param[out] limit_min the minimum allowed field value + * \param[out] limit_max the maximum allowed field value + */ + inline void + getFilterLimits (double &limit_min, double &limit_max) const + { + limit_min = filter_limit_min_; + limit_max = filter_limit_max_; + } + + /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max). + * Default: false. + * \param[in] limit_negative return data inside the interval (false) or outside (true) + */ + inline void + setFilterLimitsNegative (const bool limit_negative) + { + filter_limit_negative_ = limit_negative; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline void + getFilterLimitsNegative (bool &limit_negative) const + { + limit_negative = filter_limit_negative_; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \return true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline bool + getFilterLimitsNegative () const + { + return (filter_limit_negative_); + } + + protected: + /** \brief The size of a leaf. */ + Eigen::Vector4f leaf_size_; + + /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */ + Eigen::Array4f inverse_leaf_size_; + + /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */ + bool downsample_all_data_; + + /** \brief Set to true if leaf layout information needs to be saved in \a leaf_layout_. */ + bool save_leaf_layout_; + + /** \brief The leaf layout information for fast access to cells relative to current position **/ + std::vector leaf_layout_; + + /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */ + Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; + + /** \brief The desired user filter field name. */ + std::string filter_field_name_; + + /** \brief The minimum allowed filter value a point will be considered from. */ + double filter_limit_min_; + + /** \brief The maximum allowed filter value a point will be considered from. */ + double filter_limit_max_; + + /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ + bool filter_limit_negative_; + + /** \brief Minimum number of points per voxel for the centroid to be computed */ + unsigned int min_points_per_voxel_; + + using FieldList = typename pcl::traits::fieldList::type; + + /** \brief Downsample a Point Cloud using a voxelized grid approach + * \param[out] output the resultant point cloud message + */ + void + applyFilter (PointCloud &output) override; + }; + + /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. + * + * The VoxelGrid class creates a *3D voxel grid* (think about a voxel + * grid as a set of tiny 3D boxes in space) over the input point cloud data. + * Then, in each *voxel* (i.e., 3D box), all the points present will be + * approximated (i.e., *downsampled*) with their centroid. This approach is + * a bit slower than approximating them with the center of the voxel, but it + * represents the underlying surface more accurately. + * + * \author Radu B. Rusu, Bastian Steder, Radoslaw Cybulski + * \ingroup filters + */ + template <> + class PCL_EXPORTS VoxelGrid : public Filter + { + using Filter::filter_name_; + using Filter::getClassName; + + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + public: + /** \brief Empty constructor. */ + VoxelGrid () : + leaf_size_ (Eigen::Vector4f::Zero ()), + inverse_leaf_size_ (Eigen::Array4f::Zero ()), + downsample_all_data_ (true), + save_leaf_layout_ (false), + min_b_ (Eigen::Vector4i::Zero ()), + max_b_ (Eigen::Vector4i::Zero ()), + div_b_ (Eigen::Vector4i::Zero ()), + divb_mul_ (Eigen::Vector4i::Zero ()), + filter_field_name_ (""), + filter_limit_min_ (-FLT_MAX), + filter_limit_max_ (FLT_MAX), + filter_limit_negative_ (false), + min_points_per_voxel_ (0) + { + filter_name_ = "VoxelGrid"; + } + + /** \brief Destructor. */ + ~VoxelGrid () + { + } + + /** \brief Set the voxel grid leaf size. + * \param[in] leaf_size the voxel grid leaf size + */ + inline void + setLeafSize (const Eigen::Vector4f &leaf_size) + { + leaf_size_ = leaf_size; + // Avoid division errors + if (leaf_size_[3] == 0) + leaf_size_[3] = 1; + // Use multiplications instead of divisions + inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); + } + + /** \brief Set the voxel grid leaf size. + * \param[in] lx the leaf size for X + * \param[in] ly the leaf size for Y + * \param[in] lz the leaf size for Z + */ + inline void + setLeafSize (float lx, float ly, float lz) + { + leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz; + // Avoid division errors + if (leaf_size_[3] == 0) + leaf_size_[3] = 1; + // Use multiplications instead of divisions + inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); + } + + /** \brief Get the voxel grid leaf size. */ + inline Eigen::Vector3f + getLeafSize () const { return (leaf_size_.head<3> ()); } + + /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. + * \param[in] downsample the new value (true/false) + */ + inline void + setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } + + /** \brief Get the state of the internal downsampling parameter (true if + * all fields need to be downsampled, false if just XYZ). + */ + inline bool + getDownsampleAllData () const { return (downsample_all_data_); } + + /** \brief Set the minimum number of points required for a voxel to be used. + * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used + */ + inline void + setMinimumPointsNumberPerVoxel (unsigned int min_points_per_voxel) { min_points_per_voxel_ = min_points_per_voxel; } + + /** \brief Return the minimum number of points required for a voxel to be used. + */ + inline unsigned int + getMinimumPointsNumberPerVoxel () const { return min_points_per_voxel_; } + + /** \brief Set to true if leaf layout information needs to be saved for later access. + * \param[in] save_leaf_layout the new value (true/false) + */ + inline void + setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; } + + /** \brief Returns true if leaf layout information will to be saved for later access. */ + inline bool + getSaveLeafLayout () const { return (save_leaf_layout_); } + + /** \brief Get the minimum coordinates of the bounding box (after + * filtering is performed). + */ + inline Eigen::Vector3i + getMinBoxCoordinates () const { return (min_b_.head<3> ()); } + + /** \brief Get the minimum coordinates of the bounding box (after + * filtering is performed). + */ + inline Eigen::Vector3i + getMaxBoxCoordinates () const { return (max_b_.head<3> ()); } + + /** \brief Get the number of divisions along all 3 axes (after filtering + * is performed). + */ + inline Eigen::Vector3i + getNrDivisions () const { return (div_b_.head<3> ()); } + + /** \brief Get the multipliers to be applied to the grid coordinates in + * order to find the centroid index (after filtering is performed). + */ + inline Eigen::Vector3i + getDivisionMultiplier () const { return (divb_mul_.head<3> ()); } + + /** \brief Returns the index in the resulting downsampled cloud of the specified point. + * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed, + * and that the point is inside the grid, to avoid invalid access (or use getGridCoordinates+getCentroidIndexAt) + * \param[in] x the X point coordinate to get the index at + * \param[in] y the Y point coordinate to get the index at + * \param[in] z the Z point coordinate to get the index at + */ + inline int + getCentroidIndex (float x, float y, float z) const + { + return (leaf_layout_.at ((Eigen::Vector4i (static_cast (std::floor (x * inverse_leaf_size_[0])), + static_cast (std::floor (y * inverse_leaf_size_[1])), + static_cast (std::floor (z * inverse_leaf_size_[2])), + 0) + - min_b_).dot (divb_mul_))); + } + + /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates, + * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds). + * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) + * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) + * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) + * \param[out] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell + * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed + */ + inline std::vector + getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates) const + { + Eigen::Vector4i ijk (static_cast (std::floor (x * inverse_leaf_size_[0])), + static_cast (std::floor (y * inverse_leaf_size_[1])), + static_cast (std::floor (z * inverse_leaf_size_[2])), 0); + Eigen::Array4i diff2min = min_b_ - ijk; + Eigen::Array4i diff2max = max_b_ - ijk; + std::vector neighbors (relative_coordinates.cols()); + for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++) + { + Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); + // checking if the specified cell is in the grid + if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) + neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted + else + neighbors[ni] = -1; // cell is out of bounds, consider it empty + } + return (neighbors); + } + + /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates, + * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds). + * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) + * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) + * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) + * \param[out] relative_coordinates vector with the elements being the coordinates of the requested cells, relative to the reference point's cell + * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed + */ + inline std::vector + getNeighborCentroidIndices (float x, float y, float z, const std::vector > &relative_coordinates) const + { + Eigen::Vector4i ijk (static_cast (std::floor (x * inverse_leaf_size_[0])), static_cast (std::floor (y * inverse_leaf_size_[1])), static_cast (std::floor (z * inverse_leaf_size_[2])), 0); + std::vector neighbors; + neighbors.reserve (relative_coordinates.size ()); + for (const auto &relative_coordinate : relative_coordinates) + neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << relative_coordinate, 0).finished() - min_b_).dot (divb_mul_)]); + return (neighbors); + } + + /** \brief Returns the layout of the leafs for fast access to cells relative to current position. + * \note position at (i-min_x) + (j-min_y)*div_x + (k-min_z)*div_x*div_y holds the index of the element at coordinates (i,j,k) in the grid (-1 if empty) + */ + inline std::vector + getLeafLayout () const { return (leaf_layout_); } + + /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). + * \param[in] x the X point coordinate to get the (i, j, k) index at + * \param[in] y the Y point coordinate to get the (i, j, k) index at + * \param[in] z the Z point coordinate to get the (i, j, k) index at + */ + inline Eigen::Vector3i + getGridCoordinates (float x, float y, float z) const + { + return (Eigen::Vector3i (static_cast (std::floor (x * inverse_leaf_size_[0])), + static_cast (std::floor (y * inverse_leaf_size_[1])), + static_cast (std::floor (z * inverse_leaf_size_[2])))); + } + + /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates. + * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty) + */ + inline int + getCentroidIndexAt (const Eigen::Vector3i &ijk) const + { + int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_); + if (idx < 0 || idx >= static_cast (leaf_layout_.size ())) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed + { + //if (verbose) + // PCL_ERROR ("[pcl::%s::getCentroidIndexAt] Specified coordinate is outside grid bounds, or leaf layout is not saved, make sure to call setSaveLeafLayout(true) and filter(output) first!\n", getClassName ().c_str ()); + return (-1); + } + return (leaf_layout_[idx]); + } + + /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits, + * points having values outside this interval will be discarded. + * \param[in] field_name the name of the field that contains values used for filtering + */ + inline void + setFilterFieldName (const std::string &field_name) + { + filter_field_name_ = field_name; + } + + /** \brief Get the name of the field used for filtering. */ + inline std::string const + getFilterFieldName () const + { + return (filter_field_name_); + } + + /** \brief Set the field filter limits. All points having field values outside this interval will be discarded. + * \param[in] limit_min the minimum allowed field value + * \param[in] limit_max the maximum allowed field value + */ + inline void + setFilterLimits (const double &limit_min, const double &limit_max) + { + filter_limit_min_ = limit_min; + filter_limit_max_ = limit_max; + } + + /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX. + * \param[out] limit_min the minimum allowed field value + * \param[out] limit_max the maximum allowed field value + */ + inline void + getFilterLimits (double &limit_min, double &limit_max) const + { + limit_min = filter_limit_min_; + limit_max = filter_limit_max_; + } + + /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max). + * Default: false. + * \param[in] limit_negative return data inside the interval (false) or outside (true) + */ + inline void + setFilterLimitsNegative (const bool limit_negative) + { + filter_limit_negative_ = limit_negative; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline void + getFilterLimitsNegative (bool &limit_negative) const + { + limit_negative = filter_limit_negative_; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \return true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline bool + getFilterLimitsNegative () const + { + return (filter_limit_negative_); + } + + protected: + /** \brief The size of a leaf. */ + Eigen::Vector4f leaf_size_; + + /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */ + Eigen::Array4f inverse_leaf_size_; + + /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */ + bool downsample_all_data_; + + /** \brief Set to true if leaf layout information needs to be saved in \a + * leaf_layout. + */ + bool save_leaf_layout_; + + /** \brief The leaf layout information for fast access to cells relative + * to current position + */ + std::vector leaf_layout_; + + /** \brief The minimum and maximum bin coordinates, the number of + * divisions, and the division multiplier. + */ + Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; + + /** \brief The desired user filter field name. */ + std::string filter_field_name_; + + /** \brief The minimum allowed filter value a point will be considered from. */ + double filter_limit_min_; + + /** \brief The maximum allowed filter value a point will be considered from. */ + double filter_limit_max_; + + /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ + bool filter_limit_negative_; + + /** \brief Minimum number of points per voxel for the centroid to be computed */ + unsigned int min_points_per_voxel_; + + /** \brief Downsample a Point Cloud using a voxelized grid approach + * \param[out] output the resultant point cloud + */ + void + applyFilter (PCLPointCloud2 &output) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_covariance.h b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_covariance.h new file mode 100755 index 0000000..e751d4c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_covariance.h @@ -0,0 +1,539 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief A searchable voxel strucure containing the mean and covariance of the data. + * \note For more information please see + * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — + * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. + * PhD thesis, Orebro University. Orebro Studies in Technology 36 + * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) + */ + template + class VoxelGridCovariance : public VoxelGrid + { + protected: + using VoxelGrid::filter_name_; + using VoxelGrid::getClassName; + using VoxelGrid::input_; + using VoxelGrid::indices_; + using VoxelGrid::filter_limit_negative_; + using VoxelGrid::filter_limit_min_; + using VoxelGrid::filter_limit_max_; + using VoxelGrid::filter_field_name_; + + using VoxelGrid::downsample_all_data_; + using VoxelGrid::leaf_layout_; + using VoxelGrid::save_leaf_layout_; + using VoxelGrid::leaf_size_; + using VoxelGrid::min_b_; + using VoxelGrid::max_b_; + using VoxelGrid::inverse_leaf_size_; + using VoxelGrid::div_b_; + using VoxelGrid::divb_mul_; + + + using FieldList = typename pcl::traits::fieldList::type; + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf. + * Inverse covariance, eigen vectors and engen values are precomputed. */ + struct Leaf + { + /** \brief Constructor. + * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix + */ + Leaf () : + nr_points (0), + mean_ (Eigen::Vector3d::Zero ()), + cov_ (Eigen::Matrix3d::Identity ()), + icov_ (Eigen::Matrix3d::Zero ()), + evecs_ (Eigen::Matrix3d::Identity ()), + evals_ (Eigen::Vector3d::Zero ()) + { + } + + /** \brief Get the voxel covariance. + * \return covariance matrix + */ + Eigen::Matrix3d + getCov () const + { + return (cov_); + } + + /** \brief Get the inverse of the voxel covariance. + * \return inverse covariance matrix + */ + Eigen::Matrix3d + getInverseCov () const + { + return (icov_); + } + + /** \brief Get the voxel centroid. + * \return centroid + */ + Eigen::Vector3d + getMean () const + { + return (mean_); + } + + /** \brief Get the eigen vectors of the voxel covariance. + * \note Order corresponds with \ref getEvals + * \return matrix whose columns contain eigen vectors + */ + Eigen::Matrix3d + getEvecs () const + { + return (evecs_); + } + + /** \brief Get the eigen values of the voxel covariance. + * \note Order corresponds with \ref getEvecs + * \return vector of eigen values + */ + Eigen::Vector3d + getEvals () const + { + return (evals_); + } + + /** \brief Get the number of points contained by this voxel. + * \return number of points + */ + int + getPointCount () const + { + return (nr_points); + } + + /** \brief Number of points contained by voxel */ + int nr_points; + + /** \brief 3D voxel centroid */ + Eigen::Vector3d mean_; + + /** \brief Nd voxel centroid + * \note Differs from \ref mean_ when color data is used + */ + Eigen::VectorXf centroid; + + /** \brief Voxel covariance matrix */ + Eigen::Matrix3d cov_; + + /** \brief Inverse of voxel covariance matrix */ + Eigen::Matrix3d icov_; + + /** \brief Eigen vectors of voxel covariance matrix */ + Eigen::Matrix3d evecs_; + + /** \brief Eigen values of voxel covariance matrix */ + Eigen::Vector3d evals_; + + }; + + /** \brief Pointer to VoxelGridCovariance leaf structure */ + using LeafPtr = Leaf *; + + /** \brief Const pointer to VoxelGridCovariance leaf structure */ + using LeafConstPtr = const Leaf *; + + public: + + /** \brief Constructor. + * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. + */ + VoxelGridCovariance () : + searchable_ (true), + min_points_per_voxel_ (6), + min_covar_eigvalue_mult_ (0.01), + leaves_ (), + voxel_centroids_ (), + kdtree_ () + { + downsample_all_data_ = false; + save_leaf_layout_ = false; + leaf_size_.setZero (); + min_b_.setZero (); + max_b_.setZero (); + filter_name_ = "VoxelGridCovariance"; + } + + /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation). + * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used + */ + inline void + setMinPointPerVoxel (int min_points_per_voxel) + { + if(min_points_per_voxel > 2) + { + min_points_per_voxel_ = min_points_per_voxel; + } + else + { + PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ()); + min_points_per_voxel_ = 3; + } + } + + /** \brief Get the minimum number of points required for a cell to be used. + * \return the minimum number of points for required for a voxel to be used + */ + inline int + getMinPointPerVoxel () + { + return min_points_per_voxel_; + } + + /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. + * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues + */ + inline void + setCovEigValueInflationRatio (double min_covar_eigvalue_mult) + { + min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; + } + + /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. + * \return the minimum allowable ratio between eigenvalues + */ + inline double + getCovEigValueInflationRatio () + { + return min_covar_eigvalue_mult_; + } + + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient number of points + * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built + */ + inline void + filter (PointCloud &output, bool searchable = false) + { + searchable_ = searchable; + applyFilter (output); + + voxel_centroids_ = PointCloudPtr (new PointCloud (output)); + + if (searchable_ && !voxel_centroids_->empty ()) + { + // Initiates kdtree of the centroids of voxels containing a sufficient number of points + kdtree_.setInputCloud (voxel_centroids_); + } + } + + /** \brief Initializes voxel structure. + * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built + */ + inline void + filter (bool searchable = false) + { + searchable_ = searchable; + voxel_centroids_ = PointCloudPtr (new PointCloud); + applyFilter (*voxel_centroids_); + + if (searchable_ && !voxel_centroids_->empty ()) + { + // Initiates kdtree of the centroids of voxels containing a sufficient number of points + kdtree_.setInputCloud (voxel_centroids_); + } + } + + /** \brief Get the voxel containing point p. + * \param[in] index the index of the leaf structure node + * \return const pointer to leaf structure + */ + inline LeafConstPtr + getLeaf (int index) + { + typename std::map::iterator leaf_iter = leaves_.find (index); + if (leaf_iter != leaves_.end ()) + { + LeafConstPtr ret (&(leaf_iter->second)); + return ret; + } + return nullptr; + } + + /** \brief Get the voxel containing point p. + * \param[in] p the point to get the leaf structure at + * \return const pointer to leaf structure + */ + inline LeafConstPtr + getLeaf (PointT &p) + { + // Generate index associated with p + int ijk0 = static_cast (std::floor (p.x * inverse_leaf_size_[0]) - min_b_[0]); + int ijk1 = static_cast (std::floor (p.y * inverse_leaf_size_[1]) - min_b_[1]); + int ijk2 = static_cast (std::floor (p.z * inverse_leaf_size_[2]) - min_b_[2]); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + // Find leaf associated with index + typename std::map::iterator leaf_iter = leaves_.find (idx); + if (leaf_iter != leaves_.end ()) + { + // If such a leaf exists return the pointer to the leaf structure + LeafConstPtr ret (&(leaf_iter->second)); + return ret; + } + return nullptr; + } + + /** \brief Get the voxel containing point p. + * \param[in] p the point to get the leaf structure at + * \return const pointer to leaf structure + */ + inline LeafConstPtr + getLeaf (Eigen::Vector3f &p) + { + // Generate index associated with p + int ijk0 = static_cast (std::floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]); + int ijk1 = static_cast (std::floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]); + int ijk2 = static_cast (std::floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + + // Find leaf associated with index + typename std::map::iterator leaf_iter = leaves_.find (idx); + if (leaf_iter != leaves_.end ()) + { + // If such a leaf exists return the pointer to the leaf structure + LeafConstPtr ret (&(leaf_iter->second)); + return ret; + } + return nullptr; + + } + + /** \brief Get the voxels surrounding point p, not including the voxel containing point p. + * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice). + * \param[in] reference_point the point to get the leaf structure at + * \param[out] neighbors + * \return number of neighbors found + */ + int + getNeighborhoodAtPoint (const PointT& reference_point, std::vector &neighbors); + + /** \brief Get the leaf structure map + * \return a map contataining all leaves + */ + inline const std::map& + getLeaves () + { + return leaves_; + } + + /** \brief Get a pointcloud containing the voxel centroids + * \note Only voxels containing a sufficient number of points are used. + * \return a map contataining all leaves + */ + inline PointCloudPtr + getCentroids () + { + return voxel_centroids_; + } + + + /** \brief Get a cloud to visualize each voxels normal distribution. + * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel + */ + void + getDisplayCloud (pcl::PointCloud& cell_cloud); + + /** \brief Search for the k-nearest occupied voxels for the given query point. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \return number of neighbors found + */ + int + nearestKSearch (const PointT &point, int k, + std::vector &k_leaves, std::vector &k_sqr_distances) + { + k_leaves.clear (); + + // Check if kdtree has been built + if (!searchable_) + { + PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); + return 0; + } + + // Find k-nearest neighbors in the occupied voxel centroid cloud + std::vector k_indices; + k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances); + + // Find leaves corresponding to neighbors + k_leaves.reserve (k); + for (const int &k_index : k_indices) + { + k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]); + } + return k; + } + + /** \brief Search for the k-nearest occupied voxels for the given query point. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] cloud the given query point + * \param[in] index the index + * \param[in] k the number of neighbors to search for + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \return number of neighbors found + */ + inline int + nearestKSearch (const PointCloud &cloud, int index, int k, + std::vector &k_leaves, std::vector &k_sqr_distances) + { + if (index >= static_cast (cloud.points.size ()) || index < 0) + return (0); + return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances)); + } + + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + int + radiusSearch (const PointT &point, double radius, std::vector &k_leaves, + std::vector &k_sqr_distances, unsigned int max_nn = 0) + { + k_leaves.clear (); + + // Check if kdtree has been built + if (!searchable_) + { + PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); + return 0; + } + + // Find neighbors within radius in the occupied voxel centroid cloud + std::vector k_indices; + int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn); + + // Find leaves corresponding to neighbors + k_leaves.reserve (k); + for (const int &k_index : k_indices) + { + k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[k_index]]); + } + return k; + } + + /** \brief Search for all the nearest occupied voxels of the query point in a given radius. + * \note Only voxels containing a sufficient number of points are used. + * \param[in] cloud the given query point + * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_leaves the resultant leaves of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn + * \return number of neighbors found + */ + inline int + radiusSearch (const PointCloud &cloud, int index, double radius, + std::vector &k_leaves, std::vector &k_sqr_distances, + unsigned int max_nn = 0) + { + if (index >= static_cast (cloud.points.size ()) || index < 0) + return (0); + return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); + } + + protected: + + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient number of points + */ + void applyFilter (PointCloud &output) override; + + /** \brief Flag to determine if voxel structure is searchable. */ + bool searchable_; + + /** \brief Minimum points contained with in a voxel to allow it to be usable. */ + int min_points_per_voxel_; + + /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */ + double min_covar_eigvalue_mult_; + + /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */ + std::map leaves_; + + /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */ + PointCloudPtr voxel_centroids_; + + /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */ + std::vector voxel_centroids_leaf_indices_; + + /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ + KdTreeFLANN kdtree_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_label.h b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_label.h new file mode 100755 index 0000000..9d4a7f2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_label.h @@ -0,0 +1,72 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief + * + * + * \author Christian Potthast (potthast@usc.edu) + */ + class PCL_EXPORTS VoxelGridLabel : public VoxelGrid + { + public: + + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + + /** \brief Constructor. + * Sets \ref leaf_size_ to 0. + */ + VoxelGridLabel () {}; + + protected: + + /** \brief Filter cloud and initializes voxel structure. + * \param[out] output cloud containing centroids of voxels containing a sufficient number of points + */ + void + applyFilter (PointCloud &output) override; + + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_occlusion_estimation.h b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_occlusion_estimation.h new file mode 100755 index 0000000..4892373 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/filters/voxel_grid_occlusion_estimation.h @@ -0,0 +1,250 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief VoxelGrid to estimate occluded space in the scene. + * The ray traversal algorithm is implemented by the work of + * 'John Amanatides and Andrew Woo, A Fast Voxel Traversal Algorithm for Ray Tracing' + * + * \author Christian Potthast + * \ingroup filters + */ + template + class VoxelGridOcclusionEstimation: public VoxelGrid + { + protected: + using VoxelGrid::min_b_; + using VoxelGrid::max_b_; + using VoxelGrid::div_b_; + using VoxelGrid::leaf_size_; + using VoxelGrid::inverse_leaf_size_; + + using PointCloud = typename Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + /** \brief Empty constructor. */ + VoxelGridOcclusionEstimation () + { + initialized_ = false; + this->setSaveLeafLayout (true); + } + + /** \brief Destructor. */ + ~VoxelGridOcclusionEstimation () + { + } + + /** \brief Initialize the voxel grid, needs to be called first + * Builts the voxel grid and computes additional values for + * the ray traversal algorithm. + */ + void + initializeVoxelGrid (); + + /** \brief Computes the state (free = 0, occluded = 1) of the voxel + * after utilizing a ray traversal algorithm to a target voxel + * in (i, j, k) coordinates. + * \param[out] out_state The state of the voxel. + * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel. + * \return 0 upon success and -1 if an error occurs + */ + int + occlusionEstimation (int& out_state, + const Eigen::Vector3i& in_target_voxel); + + /** \brief Computes the state (free = 0, occluded = 1) of the voxel + * after utilizing a ray traversal algorithm to a target voxel + * in (i, j, k) coordinates. Additionally, this function returns + * the voxels penetrated of the ray-traversal algorithm till reaching + * the target voxel. + * \param[out] out_state The state of the voxel. + * \param[out] out_ray The voxels penetrated of the ray-traversal algorithm. + * \param[in] in_target_voxel The target voxel coordinate (i, j, k) of the voxel. + * \return 0 upon success and -1 if an error occurs + */ + int + occlusionEstimation (int& out_state, + std::vector >& out_ray, + const Eigen::Vector3i& in_target_voxel); + + /** \brief Computes the voxel coordinates (i, j, k) of all occluded + * voxels in the voxel grid. + * \param[out] occluded_voxels the coordinates (i, j, k) of all occluded voxels + * \return 0 upon success and -1 if an error occurs + */ + int + occlusionEstimationAll (std::vector >& occluded_voxels); + + /** \brief Returns the voxel grid filtered point cloud + * \return The voxel grid filtered point cloud + */ + inline PointCloud + getFilteredPointCloud () { return filtered_cloud_; } + + + /** \brief Returns the minimum bounding of coordinates of the voxel grid (x,y,z). + * \return the minimum coordinates (x,y,z) + */ + inline Eigen::Vector3f + getMinBoundCoordinates () { return (b_min_.head<3> ()); } + + /** \brief Returns the maximum bounding of coordinates of the voxel grid (x,y,z). + * \return the maximum coordinates (x,y,z) + */ + inline Eigen::Vector3f + getMaxBoundCoordinates () { return (b_max_.head<3> ()); } + + /** \brief Returns the corresponding centroid (x,y,z) coordinates + * in the grid of voxel (i,j,k). + * \param[in] ijk the coordinate (i, j, k) of the voxel + * \return the (x,y,z) coordinate of the voxel centroid + */ + inline Eigen::Vector4f + getCentroidCoordinate (const Eigen::Vector3i& ijk) + { + int i,j,k; + i = ((b_min_[0] < 0) ? (std::abs (min_b_[0]) + ijk[0]) : (ijk[0] - min_b_[0])); + j = ((b_min_[1] < 0) ? (std::abs (min_b_[1]) + ijk[1]) : (ijk[1] - min_b_[1])); + k = ((b_min_[2] < 0) ? (std::abs (min_b_[2]) + ijk[2]) : (ijk[2] - min_b_[2])); + + Eigen::Vector4f xyz; + xyz[0] = b_min_[0] + (leaf_size_[0] * 0.5f) + (static_cast (i) * leaf_size_[0]); + xyz[1] = b_min_[1] + (leaf_size_[1] * 0.5f) + (static_cast (j) * leaf_size_[1]); + xyz[2] = b_min_[2] + (leaf_size_[2] * 0.5f) + (static_cast (k) * leaf_size_[2]); + xyz[3] = 0; + return xyz; + } + + // inline void + // setSensorOrigin (const Eigen::Vector4f origin) { sensor_origin_ = origin; } + + // inline void + // setSensorOrientation (const Eigen::Quaternionf orientation) { sensor_orientation_ = orientation; } + + protected: + + /** \brief Returns the scaling value (tmin) were the ray intersects with the + * voxel grid bounding box. (p_entry = origin + tmin * orientation) + * \param[in] origin The sensor origin + * \param[in] direction The sensor orientation + * \return the scaling value + */ + float + rayBoxIntersection (const Eigen::Vector4f& origin, + const Eigen::Vector4f& direction); + + /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) + * unsing a ray traversal algorithm. + * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k). + * \param[in] origin The sensor origin. + * \param[in] direction The sensor orientation + * \param[in] t_min The scaling value (tmin). + * \return The estimated voxel state. + */ + int + rayTraversal (const Eigen::Vector3i& target_voxel, + const Eigen::Vector4f& origin, + const Eigen::Vector4f& direction, + const float t_min); + + /** \brief Returns the state of the target voxel (0 = visible, 1 = occupied) and + * the voxels penetrated by the ray unsing a ray traversal algorithm. + * \param[out] out_ray The voxels penetrated by the ray in (i, j, k) coordinates + * \param[in] target_voxel The target voxel in the voxel grid with coordinate (i, j, k). + * \param[in] origin The sensor origin. + * \param[in] direction The sensor orientation + * \param[in] t_min The scaling value (tmin). + * \return The estimated voxel state. + */ + int + rayTraversal (std::vector >& out_ray, + const Eigen::Vector3i& target_voxel, + const Eigen::Vector4f& origin, + const Eigen::Vector4f& direction, + const float t_min); + + /** \brief Returns a rounded value. + * \param[in] d + * \return rounded value + */ + inline float + round (float d) + { + return static_cast (std::floor (d + 0.5f)); + } + + // We use round here instead of std::floor due to some numerical issues. + /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). + * \param[in] x the X point coordinate to get the (i, j, k) index at + * \param[in] y the Y point coordinate to get the (i, j, k) index at + * \param[in] z the Z point coordinate to get the (i, j, k) index at + */ + inline Eigen::Vector3i + getGridCoordinatesRound (float x, float y, float z) + { + return Eigen::Vector3i (static_cast (round (x * inverse_leaf_size_[0])), + static_cast (round (y * inverse_leaf_size_[1])), + static_cast (round (z * inverse_leaf_size_[2]))); + } + + // initialization flag + bool initialized_; + + Eigen::Vector4f sensor_origin_; + Eigen::Quaternionf sensor_orientation_; + + // minimum and maximum bounding box coordinates + Eigen::Vector4f b_min_, b_max_; + + // voxel grid filtered cloud + PointCloud filtered_cloud_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/for_each_type.h b/deps_install/include/pcl-1.10/pcl/for_each_type.h new file mode 100755 index 0000000..3abbad7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/for_each_type.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace pcl +{ + ////////////////////////////////////////////////////////////////////////////////////////////// + template + struct for_each_type_impl + { + template + static void execute (F) {} + }; + + ////////////////////////////////////////////////////////////////////////////////////////////// + template <> + struct for_each_type_impl + { + template + static void execute (F f) + { + using arg = typename boost::mpl::deref::type; + +#if (defined _WIN32 && defined _MSC_VER) + boost::mpl::aux::unwrap (f, 0).operator() (); +#else + boost::mpl::aux::unwrap (f, 0).template operator() (); +#endif + + using iter = typename boost::mpl::next::type; + for_each_type_impl::value> + ::template execute (f); + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////// + template inline void + for_each_type (F f) + { + BOOST_MPL_ASSERT (( boost::mpl::is_sequence )); + using first = typename boost::mpl::begin::type; + using last = typename boost::mpl::end::type; + for_each_type_impl::value>::template execute (f); + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template + struct intersect + { + using type = typename boost::mpl::remove_if > >::type; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/geometry/boost.h b/deps_install/include/pcl-1.10/pcl/geometry/boost.h new file mode 100755 index 0000000..d5421ca --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/boost.h @@ -0,0 +1,49 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifdef __GNUC__ +# pragma GCC system_header +#endif + +#include +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/geometry/eigen.h b/deps_install/include/pcl-1.10/pcl/geometry/eigen.h new file mode 100755 index 0000000..8975e64 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/eigen.h @@ -0,0 +1,48 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifdef __GNUC__ +# pragma GCC system_header +#endif + +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/geometry/get_boundary.h b/deps_install/include/pcl-1.10/pcl/geometry/get_boundary.h new file mode 100755 index 0000000..47d211b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/get_boundary.h @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace geometry + { + /** \brief Get a collection of boundary half-edges for the input mesh. + * \param[in] mesh The input mesh. + * \param[out] boundary_he_collection Collection of boundary half-edges. Each element in the vector is one connected boundary. The whole boundary is the union of all elements. + * \param [in] expected_size If you already know the size of the longest boundary you can tell this here. Defaults to 3 (minimum possible boundary). + * \author Martin Saelzle + * \ingroup geometry + */ + template void + getBoundBoundaryHalfEdges (const MeshT& mesh, + std::vector & boundary_he_collection, + const std::size_t expected_size = 3) + { + using Mesh = MeshT; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + using HalfEdgeIndices = typename Mesh::HalfEdgeIndices; + using IHEAFC = typename Mesh::InnerHalfEdgeAroundFaceCirculator; + + boundary_he_collection.clear (); + + HalfEdgeIndices boundary_he; boundary_he.reserve (expected_size); + std::vector visited (mesh.sizeEdges (), false); + IHEAFC circ, circ_end; + + for (HalfEdgeIndex i (0); i + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::approximatePolygon (const PlanarPolygon& polygon, PlanarPolygon& approx_polygon, float threshold, bool refine, bool closed) +{ + const Eigen::Vector4f& coefficients = polygon.getCoefficients (); + const typename pcl::PointCloud::VectorType &contour = polygon.getContour (); + + Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f); + rotation_axis.normalize (); + + float rotation_angle = acosf (coefficients [2]); + Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis); + + typename pcl::PointCloud::VectorType polygon2D (contour.size ()); + for (std::size_t pIdx = 0; pIdx < polygon2D.size (); ++pIdx) + polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap (); + + typename pcl::PointCloud::VectorType approx_polygon2D; + approximatePolygon2D (polygon2D, approx_polygon2D, threshold, refine, closed); + + typename pcl::PointCloud::VectorType &approx_contour = approx_polygon.getContour (); + approx_contour.resize (approx_polygon2D.size ()); + + Eigen::Affine3f inv_transformation = transformation.inverse (); + for (std::size_t pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx) + approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap (); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::approximatePolygon2D (const typename pcl::PointCloud::VectorType &polygon, + typename pcl::PointCloud::VectorType &approx_polygon, + float threshold, bool refine, bool closed) +{ + approx_polygon.clear (); + if (polygon.size () < 3) + return; + + std::vector > intervals; + std::pair interval (0, 0); + + if (closed) + { + float max_distance = .0f; + for (std::size_t idx = 1; idx < polygon.size (); ++idx) + { + float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) + + (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y); + + if (distance > max_distance) + { + max_distance = distance; + interval.second = idx; + } + } + + for (std::size_t idx = 1; idx < polygon.size (); ++idx) + { + float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) + + (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y); + + if (distance > max_distance) + { + max_distance = distance; + interval.first = idx; + } + } + + if (max_distance < threshold * threshold) + return; + + intervals.push_back (interval); + std::swap (interval.first, interval.second); + intervals.push_back (interval); + } + else + { + interval.first = 0; + interval.second = static_cast (polygon.size ()) - 1; + intervals.push_back (interval); + } + + std::vector result; + // recursively refine + while (!intervals.empty ()) + { + std::pair& currentInterval = intervals.back (); + float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y; + float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x; + float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x; + + float linelen = 1.0f / std::sqrt (line_x * line_x + line_y * line_y); + + line_x *= linelen; + line_y *= linelen; + line_d *= linelen; + + float max_distance = 0.0; + unsigned first_index = currentInterval.first + 1; + unsigned max_index = 0; + + // => 0-crossing + if (currentInterval.first > currentInterval.second) + { + for (std::size_t idx = first_index; idx < polygon.size(); idx++) + { + float distance = std::abs (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d); + if (distance > max_distance) + { + max_distance = distance; + max_index = idx; + } + } + first_index = 0; + } + + for (unsigned int idx = first_index; idx < currentInterval.second; idx++) + { + float distance = std::abs (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d); + if (distance > max_distance) + { + max_distance = distance; + max_index = idx; + } + } + + if (max_distance > threshold) + { + std::pair interval (max_index, currentInterval.second); + currentInterval.second = max_index; + intervals.push_back (interval); + } + else + { + result.push_back (currentInterval.second); + intervals.pop_back (); + } + } + + approx_polygon.reserve (result.size ()); + if (refine) + { + std::vector > lines (result.size ()); + std::reverse (result.begin (), result.end ()); + for (std::size_t rIdx = 0; rIdx < result.size (); ++rIdx) + { + std::size_t nIdx = rIdx + 1; + if (nIdx == result.size ()) + nIdx = 0; + + Eigen::Vector2f centroid = Eigen::Vector2f::Zero (); + Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero (); + std::size_t pIdx = result[rIdx]; + unsigned num_points = 0; + if (pIdx > result[nIdx]) + { + num_points = static_cast (polygon.size ()) - pIdx; + for (; pIdx < polygon.size (); ++pIdx) + { + covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x; + covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y; + covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y; + centroid [0] += polygon [pIdx].x; + centroid [1] += polygon [pIdx].y; + } + pIdx = 0; + } + + num_points += result[nIdx] - pIdx; + for (; pIdx < result[nIdx]; ++pIdx) + { + covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x; + covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y; + covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y; + centroid [0] += polygon [pIdx].x; + centroid [1] += polygon [pIdx].y; + } + + covariance.coeffRef (2) = covariance.coeff (1); + + float norm = 1.0f / float (num_points); + centroid *= norm; + covariance *= norm; + covariance.coeffRef (0) -= centroid [0] * centroid [0]; + covariance.coeffRef (1) -= centroid [0] * centroid [1]; + covariance.coeffRef (3) -= centroid [1] * centroid [1]; + + float eval; + Eigen::Vector2f normal; + eigen22 (covariance, eval, normal); + + // select the one which is more "parallel" to the original line + Eigen::Vector2f direction; + direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x; + direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y; + direction.normalize (); + + if (std::abs (direction.dot (normal)) > float(M_SQRT1_2)) + { + std::swap (normal [0], normal [1]); + normal [0] = -normal [0]; + } + + // needs to be on the left side of the edge + if (direction [0] * normal [1] < direction [1] * normal [0]) + normal *= -1.0; + + lines [rIdx].head<2> ().matrix () = normal; + lines [rIdx] [2] = -normal.dot (centroid); + } + + float threshold2 = threshold * threshold; + for (std::size_t rIdx = 0; rIdx < lines.size (); ++rIdx) + { + std::size_t nIdx = rIdx + 1; + if (nIdx == result.size ()) + nIdx = 0; + + Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]); + vertex /= vertex [2]; + vertex [2] = 0.0; + + PointT point; + // test whether we need another edge since the intersection point is too far away from the original vertex + Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex; + pq [2] = 0.0; + + float distance = pq.squaredNorm (); + if (distance > threshold2) + { + // test whether the old point is inside the new polygon or outside + if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) && + (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) ) + { + float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2]; + float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2]; + + point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0]; + point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1]; + + approx_polygon.push_back (point); + + vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0]; + vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1]; + } + } + point.getVector3fMap () = vertex; + approx_polygon.push_back (point); + } + } + else + { + // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise) + for (std::vector::reverse_iterator it = result.rbegin (); it != result.rend (); ++it) + approx_polygon.push_back (polygon [*it]); + } +} + +#endif // PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/geometry/line_iterator.h b/deps_install/include/pcl-1.10/pcl/geometry/line_iterator.h new file mode 100755 index 0000000..a8223d1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/line_iterator.h @@ -0,0 +1,273 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "organized_index_iterator.h" + +namespace pcl +{ +/** \brief Organized Index Iterator for iterating over the "pixels" for a given line using the Bresenham algorithm. + * Supports 4 and 8 neighborhood connectivity + * \note iterator does not visit the given end-point (on purpose). + * \author Suat Gedikli + * \ingroup geometry + */ +class LineIterator : public OrganizedIndexIterator +{ + public: + /** \brief Neighborhood connectivity */ + enum Neighborhood {Neighbor4 = 4, Neighbor8 = 8}; + public: + /** \brief Constructor + * \param x_start column of the start pixel of the line + * \param y_start row of the start pixel of the line + * \param x_end column of the end pixel of the line + * \param y_end row of the end pixel of the line + * \param width width of the organized structure e.g. image/cloud/map etc.. + * \param neighborhood connectivity of the neighborhood + */ + LineIterator (unsigned x_start, unsigned y_start, unsigned x_end, unsigned y_end, unsigned width, const Neighborhood& neighborhood = Neighbor8); + + /** \brief Destructor*/ + ~LineIterator (); + + void operator ++ () override; + unsigned getRowIndex () const override; + unsigned getColumnIndex () const override; + bool isValid () const override; + void reset () override; + protected: + /** \brief initializes the variables for the Bresenham algorithm + * \param[in] neighborhood connectivity to the neighborhood. Either 4 or 8 + */ + void init (const Neighborhood& neighborhood); + + /** \brief current column index*/ + unsigned x_; + + /** \brief current row index*/ + unsigned y_; + + /** \brief column index of first pixel/point*/ + unsigned x_start_; + + /** \brief row index of first pixel/point*/ + unsigned y_start_; + + /** \brief column index of end pixel/point*/ + unsigned x_end_; + + /** \brief row index of end pixel/point*/ + unsigned y_end_; + + // calculated values + /** \brief current distance to the line*/ + int error_; + + /** \brief error threshold*/ + int error_max_; + + /** \brief increment of error (distance) value in case of an y-step (if dx > dy)*/ + int error_minus_; + + /** \brief increment of error (distance) value in case of just an x-step (if dx > dy)*/ + int error_plus_; + + /** \brief increment of column index in case of just an x-step (if dx > dy)*/ + int x_plus_; + + /** \brief increment of row index in case of just an x-step (if dx > dy)*/ + int y_plus_; + + /** \brief increment of column index in case of just an y-step (if dx > dy)*/ + int x_minus_; + + /** \brief increment of row index in case of just an y-step (if dx > dy)*/ + int y_minus_; + + /** \brief increment pixel/point index in case of just an x-step (if dx > dy)*/ + int index_plus_; + + /** \brief increment pixel/point index in case of just an y-step (if dx > dy)*/ + int index_minus_; +}; + +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////////////////// +inline LineIterator::LineIterator (unsigned x_start, unsigned y_start, unsigned x_end, unsigned y_end, unsigned width, const Neighborhood& neighborhood) +: OrganizedIndexIterator (width) +, x_start_ (x_start) +, y_start_ (y_start) +, x_end_ (x_end) +, y_end_ (y_end) +{ + init (neighborhood); +} + +//////////////////////////////////////////////////////////////////////////////// +inline LineIterator::~LineIterator () +{ +} + +//////////////////////////////////////////////////////////////////////////////// +inline void +LineIterator::init (const Neighborhood& neighborhood) +{ + x_ = x_start_; + y_ = y_start_; + index_ = x_ * width_ + y_; + error_ = 0; + + int delta_x = x_end_ - x_start_; + int delta_y = y_end_ - y_start_; + + int x_dir = ( (delta_x > 0) ? 1 : -1 ) ; + int y_dir = ( (delta_y > 0) ? 1 : -1 ) ; + + delta_x *= x_dir; + delta_y *= y_dir; + + if(delta_x >= delta_y) + { + if( neighborhood == Neighbor4 ) + { + error_max_ = delta_x - delta_y; + x_minus_ = 0; + y_minus_ = y_dir; + x_plus_ = x_dir; + y_plus_ = 0; + + error_minus_ = -(delta_x * 2); + error_plus_ = (delta_y * 2); + } + else + { + error_max_ = delta_x - (delta_y * 2); + x_minus_ = x_dir; + y_minus_ = y_dir; + x_plus_ = x_dir; + y_plus_ = 0; + + error_minus_ = (delta_y - delta_x) * 2; + error_plus_ = (delta_y * 2); + } + } + else + { + if( neighborhood == Neighbor4 ) + { + error_max_ = delta_y - delta_x; + x_minus_ = x_dir; + y_minus_ = 0; + x_plus_ = 0; + y_plus_ = y_dir; + + error_minus_ = -(delta_y * 2); + error_plus_ = (delta_x * 2); + } + else + { + error_max_ = delta_y - (delta_x * 2); + x_minus_ = x_dir; + y_minus_ = y_dir; + x_plus_ = 0; + y_plus_ = y_dir; + + error_minus_ = (delta_x - delta_y) * 2; + error_plus_ = (delta_x * 2); + } + } + + index_minus_ = x_minus_ + y_minus_ * width_; + index_plus_ = x_plus_ + y_plus_ * width_; +} + +//////////////////////////////////////////////////////////////////////////////// +inline void +LineIterator::operator ++ () +{ + if (error_ >= error_max_ ) + { + x_ += x_minus_; + y_ += y_minus_; + error_ += error_minus_; + index_ += index_minus_; + } + else + { + x_ += x_plus_; + y_ += y_plus_; + error_ += error_plus_; + index_ += index_plus_; + } +} + +//////////////////////////////////////////////////////////////////////////////// +inline unsigned +LineIterator::getRowIndex () const +{ + return y_; +} + +//////////////////////////////////////////////////////////////////////////////// +inline unsigned +LineIterator::getColumnIndex () const +{ + return x_; +} + +//////////////////////////////////////////////////////////////////////////////// +inline bool +LineIterator::isValid () const +{ + return (x_ != x_end_ || y_ != y_end_); +} + +//////////////////////////////////////////////////////////////////////////////// +inline void +LineIterator::reset () +{ + x_ = x_start_; + y_ = y_start_; + error_ = 0; + index_ = x_ * width_ + y_; +} + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/mesh_base.h b/deps_install/include/pcl-1.10/pcl/geometry/mesh_base.h new file mode 100755 index 0000000..9a3f20e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/mesh_base.h @@ -0,0 +1,2114 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +//////////////////////////////////////////////////////////////////////////////// +// Global variables used during testing +//////////////////////////////////////////////////////////////////////////////// + +#ifdef PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2 +namespace pcl +{ + namespace geometry + { + bool g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success; + } // End namespace geometry +} // End namespace pcl +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Forward declarations +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + template + class MeshIO; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// MeshBase +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Base class for the half-edge mesh. + * \tparam DerivedT Has to implement the method 'addFaceImpl'. Please have a look at pcl::geometry::TriangleMesh, pcl::geometry::QuadMesh and pcl::geometry::PolygonMesh. + * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits. + * \tparam MeshTagT Tag describing the type of the mesh, e.g. TriangleMeshTag, QuadMeshTag, PolygonMeshTag. + * \author Martin Saelzle + * \ingroup geometry + * \todo Add documentation + */ + template + class MeshBase + { + public: + + using Self = MeshBase ; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using Derived = DerivedT; + + // These have to be defined in the traits class. + using VertexData = typename MeshTraitsT::VertexData; + using HalfEdgeData = typename MeshTraitsT::HalfEdgeData; + using EdgeData = typename MeshTraitsT::EdgeData; + using FaceData = typename MeshTraitsT::FaceData; + using IsManifold = typename MeshTraitsT::IsManifold; + + // Check if the mesh traits are defined correctly. + static_assert (std::is_convertible::value, "MeshTraitsT::IsManifold is not convertible to bool"); + + using MeshTag = MeshTagT; + + // Data + using HasVertexData = std::integral_constant ::value>; + using HasHalfEdgeData = std::integral_constant ::value>; + using HasEdgeData = std::integral_constant ::value>; + using HasFaceData = std::integral_constant ::value>; + + using VertexDataCloud = pcl::PointCloud; + using HalfEdgeDataCloud = pcl::PointCloud; + using EdgeDataCloud = pcl::PointCloud; + using FaceDataCloud = pcl::PointCloud; + + // Indices + using VertexIndex = pcl::geometry::VertexIndex; + using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex; + using EdgeIndex = pcl::geometry::EdgeIndex; + using FaceIndex = pcl::geometry::FaceIndex; + + using VertexIndices = std::vector; + using HalfEdgeIndices = std::vector; + using EdgeIndices = std::vector; + using FaceIndices = std::vector; + + // Circulators + using VertexAroundVertexCirculator = pcl::geometry::VertexAroundVertexCirculator; + using OutgoingHalfEdgeAroundVertexCirculator = pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator; + using IncomingHalfEdgeAroundVertexCirculator = pcl::geometry::IncomingHalfEdgeAroundVertexCirculator; + using FaceAroundVertexCirculator = pcl::geometry::FaceAroundVertexCirculator; + using VertexAroundFaceCirculator = pcl::geometry::VertexAroundFaceCirculator; + using InnerHalfEdgeAroundFaceCirculator = pcl::geometry::InnerHalfEdgeAroundFaceCirculator; + using OuterHalfEdgeAroundFaceCirculator = pcl::geometry::OuterHalfEdgeAroundFaceCirculator; + using FaceAroundFaceCirculator = pcl::geometry::FaceAroundFaceCirculator; + + /** \brief Constructor. */ + MeshBase () + : vertex_data_cloud_ (), + half_edge_data_cloud_ (), + edge_data_cloud_ (), + face_data_cloud_ () + { + } + + //////////////////////////////////////////////////////////////////////// + // addVertex / addFace / deleteVertex / deleteEdge / deleteFace / cleanUp + //////////////////////////////////////////////////////////////////////// + + /** \brief Add a vertex to the mesh. + * \param[in] vertex_data Data that is stored in the vertex. This is only added if the mesh has data associated with the vertices. + * \return Index to the new vertex. + */ + inline VertexIndex + addVertex (const VertexData& vertex_data=VertexData ()) + { + vertices_.push_back (Vertex ()); + this->addData (vertex_data_cloud_, vertex_data, HasVertexData ()); + return (VertexIndex (static_cast (this->sizeVertices () - 1))); + } + + /** \brief Add a face to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one. + * \param[in] vertices Indices to the vertices of the new face. + * \param[in] face_data Data that is set for the face. + * \param[in] half_edge_data Data that is set for all added half-edges. + * \param[in] edge_data Data that is set for all added edges. + * \return Index to the new face. Failure is signaled by returning an invalid face index. + * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior! + */ + inline FaceIndex + addFace (const VertexIndices& vertices, + const FaceData& face_data = FaceData (), + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData& half_edge_data = HalfEdgeData ()) + { + // NOTE: The derived class has to implement addFaceImpl. If needed it can use the general method addFaceImplBase. + return (static_cast (this)->addFaceImpl (vertices, face_data, edge_data, half_edge_data)); + } + + /** \brief Mark the given vertex and all connected half-edges and faces as deleted. + * \note Call cleanUp () to finally delete all mesh-elements. + */ + void + deleteVertex (const VertexIndex& idx_vertex) + { + assert (this->isValid (idx_vertex)); + if (this->isDeleted (idx_vertex)) return; + + delete_faces_vertex_.clear (); + FaceAroundVertexCirculator circ = this->getFaceAroundVertexCirculator (idx_vertex); + const FaceAroundVertexCirculator circ_end = circ; + do + { + if (circ.getTargetIndex ().isValid ()) // Check for boundary. + { + delete_faces_vertex_.push_back (circ.getTargetIndex ()); + } + } while (++circ!=circ_end); + + for (FaceIndices::const_iterator it = delete_faces_vertex_.begin (); it!=delete_faces_vertex_.end (); ++it) + { + this->deleteFace (*it); + } + } + + /** \brief Mark the given half-edge, the opposite half-edge and the associated faces as deleted. + * \note Call cleanUp () to finally delete all mesh-elements. + */ + void + deleteEdge (const HalfEdgeIndex& idx_he) + { + assert (this->isValid (idx_he)); + if (this->isDeleted (idx_he)) return; + + HalfEdgeIndex opposite = this->getOppositeHalfEdgeIndex (idx_he); + + if (this->isBoundary (idx_he)) this->markDeleted (idx_he); + else this->deleteFace (this->getFaceIndex (idx_he)); + if (this->isBoundary (opposite)) this->markDeleted (opposite); + else this->deleteFace (this->getFaceIndex (opposite)); + } + + /** \brief Mark the given edge (both half-edges) and the associated faces as deleted. + * \note Call cleanUp () to finally delete all mesh-elements. + */ + inline void + deleteEdge (const EdgeIndex& idx_edge) + { + assert (this->isValid (idx_edge)); + this->deleteEdge (pcl::geometry::toHalfEdgeIndex (idx_edge)); + assert (this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false))); // Bug in this class! + } + + /** \brief Mark the given face as deleted. More faces are deleted if the manifold mesh would become non-manifold. + * \note Call cleanUp () to finally delete all mesh-elements. + */ + inline void + deleteFace (const FaceIndex& idx_face) + { + assert (this->isValid (idx_face)); + if (this->isDeleted (idx_face)) return; + + this->deleteFace (idx_face, IsManifold ()); + } + + /** \brief Removes all mesh elements and data that are marked as deleted. + * \note This removes all isolated vertices as well. + */ + void + cleanUp () + { + // Copy the non-deleted mesh elements and store the index to their new position + const VertexIndices new_vertex_indices = + this->remove + (vertices_, vertex_data_cloud_); + const HalfEdgeIndices new_half_edge_indices = + this->remove + (half_edges_, half_edge_data_cloud_); + const FaceIndices new_face_indices = + this->remove + (faces_, face_data_cloud_); + + // Remove deleted edge data + if (HasEdgeData::value) + { + auto it_ed_old = edge_data_cloud_.begin (); + auto it_ed_new = edge_data_cloud_.begin (); + + for (auto it_ind = new_half_edge_indices.cbegin (), it_ind_end = new_half_edge_indices.cend (); it_ind!=it_ind_end; it_ind+=2, ++it_ed_old) + { + if (it_ind->isValid ()) + { + *it_ed_new++ = *it_ed_old; + } + } + edge_data_cloud_.resize (this->sizeEdges ()); + } + + // Adjust the indices + for (VertexIterator it = vertices_.begin (); it!=vertices_.end (); ++it) + { + if (it->idx_outgoing_half_edge_.isValid ()) + { + it->idx_outgoing_half_edge_ = new_half_edge_indices [it->idx_outgoing_half_edge_.get ()]; + } + } + + for (HalfEdgeIterator it = half_edges_.begin (); it!=half_edges_.end (); ++it) + { + it->idx_terminating_vertex_ = new_vertex_indices [it->idx_terminating_vertex_.get ()]; + it->idx_next_half_edge_ = new_half_edge_indices [it->idx_next_half_edge_.get ()]; + it->idx_prev_half_edge_ = new_half_edge_indices [it->idx_prev_half_edge_.get ()]; + if (it->idx_face_.isValid ()) + { + it->idx_face_ = new_face_indices [it->idx_face_.get ()]; + } + } + + for (FaceIterator it = faces_.begin (); it!=faces_.end (); ++it) + { + it->idx_inner_half_edge_ = new_half_edge_indices [it->idx_inner_half_edge_.get ()]; + } + } + + //////////////////////////////////////////////////////////////////////// + // Vertex connectivity + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the outgoing half-edge index to a given vertex. */ + inline HalfEdgeIndex + getOutgoingHalfEdgeIndex (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (this->getVertex (idx_vertex).idx_outgoing_half_edge_); + } + + /** \brief Get the incoming half-edge index to a given vertex. */ + inline HalfEdgeIndex + getIncomingHalfEdgeIndex (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (this->getOppositeHalfEdgeIndex (this->getOutgoingHalfEdgeIndex (idx_vertex))); + } + + //////////////////////////////////////////////////////////////////////// + // Half-edge connectivity + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the terminating vertex index to a given half-edge. */ + inline VertexIndex + getTerminatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const + { + assert (this->isValid (idx_half_edge)); + return (this->getHalfEdge (idx_half_edge).idx_terminating_vertex_); + } + + /** \brief Get the originating vertex index to a given half-edge. */ + inline VertexIndex + getOriginatingVertexIndex (const HalfEdgeIndex& idx_half_edge) const + { + assert (this->isValid (idx_half_edge)); + return (this->getTerminatingVertexIndex (this->getOppositeHalfEdgeIndex (idx_half_edge))); + } + + /** \brief Get the opposite half-edge index to a given half-edge. */ + inline HalfEdgeIndex + getOppositeHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const + { + assert (this->isValid (idx_half_edge)); + // Check if the index is even or odd and return the other index. + return (HalfEdgeIndex (idx_half_edge.get () & 1 ? idx_half_edge.get () - 1 : idx_half_edge.get () + 1)); + } + + /** \brief Get the next half-edge index to a given half-edge. */ + inline HalfEdgeIndex + getNextHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const + { + assert (this->isValid (idx_half_edge)); + return (this->getHalfEdge (idx_half_edge).idx_next_half_edge_); + } + + /** \brief Get the previous half-edge index to a given half-edge. */ + inline HalfEdgeIndex + getPrevHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge) const + { + assert (this->isValid (idx_half_edge)); + return (this->getHalfEdge (idx_half_edge).idx_prev_half_edge_); + } + + /** \brief Get the face index to a given half-edge. */ + inline FaceIndex + getFaceIndex (const HalfEdgeIndex& idx_half_edge) const + { + assert (this->isValid (idx_half_edge)); + return (this->getHalfEdge (idx_half_edge).idx_face_); + } + + /** \brief Get the face index to a given half-edge. */ + inline FaceIndex + getOppositeFaceIndex (const HalfEdgeIndex& idx_half_edge) const + { + assert (this->isValid (idx_half_edge)); + return (this->getFaceIndex (this->getOppositeHalfEdgeIndex (idx_half_edge))); + } + + //////////////////////////////////////////////////////////////////////// + // Face connectivity + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the inner half-edge index to a given face. */ + inline HalfEdgeIndex + getInnerHalfEdgeIndex (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (this->getFace (idx_face).idx_inner_half_edge_); + } + + /** \brief Get the outer half-edge inex to a given face. */ + inline HalfEdgeIndex + getOuterHalfEdgeIndex (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (this->getOppositeHalfEdgeIndex (this->getInnerHalfEdgeIndex (idx_face))); + } + + //////////////////////////////////////////////////////////////////////// + // Circulators + //////////////////////////////////////////////////////////////////////// + + /** \see pcl::geometry::VertexAroundVertexCirculator */ + inline VertexAroundVertexCirculator + getVertexAroundVertexCirculator (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (VertexAroundVertexCirculator (idx_vertex, this)); + } + + /** \see pcl::geometry::VertexAroundVertexCirculator */ + inline VertexAroundVertexCirculator + getVertexAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const + { + assert (this->isValid (idx_outgoing_half_edge)); + return (VertexAroundVertexCirculator (idx_outgoing_half_edge, this)); + } + + /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */ + inline OutgoingHalfEdgeAroundVertexCirculator + getOutgoingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (OutgoingHalfEdgeAroundVertexCirculator (idx_vertex, this)); + } + + /** \see pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator */ + inline OutgoingHalfEdgeAroundVertexCirculator + getOutgoingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const + { + assert (this->isValid (idx_outgoing_half_edge)); + return (OutgoingHalfEdgeAroundVertexCirculator (idx_outgoing_half_edge, this)); + } + + /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */ + inline IncomingHalfEdgeAroundVertexCirculator + getIncomingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (IncomingHalfEdgeAroundVertexCirculator (idx_vertex, this)); + } + + /** \see pcl::geometry::IncomingHalfEdgeAroundVertexCirculator */ + inline IncomingHalfEdgeAroundVertexCirculator + getIncomingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_incoming_half_edge) const + { + assert (this->isValid (idx_incoming_half_edge)); + return (IncomingHalfEdgeAroundVertexCirculator (idx_incoming_half_edge, this)); + } + + /** \see pcl::geometry::FaceAroundVertexCirculator */ + inline FaceAroundVertexCirculator + getFaceAroundVertexCirculator (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (FaceAroundVertexCirculator (idx_vertex, this)); + } + + /** \see pcl::geometry::FaceAroundVertexCirculator */ + inline FaceAroundVertexCirculator + getFaceAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge) const + { + assert (this->isValid (idx_outgoing_half_edge)); + return (FaceAroundVertexCirculator (idx_outgoing_half_edge, this)); + } + + /** \see pcl::geometry::VertexAroundFaceCirculator */ + inline VertexAroundFaceCirculator + getVertexAroundFaceCirculator (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (VertexAroundFaceCirculator (idx_face, this)); + } + + /** \see pcl::geometry::VertexAroundFaceCirculator */ + inline VertexAroundFaceCirculator + getVertexAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const + { + assert (this->isValid (idx_inner_half_edge)); + return (VertexAroundFaceCirculator (idx_inner_half_edge, this)); + } + + /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */ + inline InnerHalfEdgeAroundFaceCirculator + getInnerHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (InnerHalfEdgeAroundFaceCirculator (idx_face, this)); + } + + /** \see pcl::geometry::InnerHalfEdgeAroundFaceCirculator */ + inline InnerHalfEdgeAroundFaceCirculator + getInnerHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const + { + assert (this->isValid (idx_inner_half_edge)); + return (InnerHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this)); + } + + /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */ + inline OuterHalfEdgeAroundFaceCirculator + getOuterHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (OuterHalfEdgeAroundFaceCirculator (idx_face, this)); + } + + /** \see pcl::geometry::OuterHalfEdgeAroundFaceCirculator */ + inline OuterHalfEdgeAroundFaceCirculator + getOuterHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const + { + assert (this->isValid (idx_inner_half_edge)); + return (OuterHalfEdgeAroundFaceCirculator (idx_inner_half_edge, this)); + } + + /** \see pcl::geometry::FaceAroundFaceCirculator */ + inline FaceAroundFaceCirculator + getFaceAroundFaceCirculator (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (FaceAroundFaceCirculator (idx_face, this)); + } + + /** \see pcl::geometry::FaceAroundFaceCirculator */ + inline FaceAroundFaceCirculator + getFaceAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge) const + { + assert (this->isValid (idx_inner_half_edge)); + return (FaceAroundFaceCirculator (idx_inner_half_edge, this)); + } + + ////////////////////////////////////////////////////////////////////////// + // isEqualTopology + ////////////////////////////////////////////////////////////////////////// + + /** \brief Check if the other mesh has the same topology as this mesh. */ + bool + isEqualTopology (const Self& other) const + { + if (this->sizeVertices () != other.sizeVertices ()) return (false); + if (this->sizeHalfEdges () != other.sizeHalfEdges ()) return (false); + if (this->sizeFaces () != other.sizeFaces ()) return (false); + + for (std::size_t i=0; isizeVertices (); ++i) + { + if (this->getOutgoingHalfEdgeIndex (VertexIndex (i)) != + other.getOutgoingHalfEdgeIndex (VertexIndex (i))) return (false); + } + + for (std::size_t i=0; isizeHalfEdges (); ++i) + { + if (this->getTerminatingVertexIndex (HalfEdgeIndex (i)) != + other.getTerminatingVertexIndex (HalfEdgeIndex (i))) return (false); + + if (this->getNextHalfEdgeIndex (HalfEdgeIndex (i)) != + other.getNextHalfEdgeIndex (HalfEdgeIndex (i))) return (false); + + if (this->getPrevHalfEdgeIndex (HalfEdgeIndex (i)) != + other.getPrevHalfEdgeIndex (HalfEdgeIndex (i))) return (false); + + if (this->getFaceIndex (HalfEdgeIndex (i)) != + other.getFaceIndex (HalfEdgeIndex (i))) return (false); + } + + for (std::size_t i=0; isizeFaces (); ++i) + { + if (this->getInnerHalfEdgeIndex (FaceIndex (i)) != + other.getInnerHalfEdgeIndex (FaceIndex (i))) return (false); + } + + return (true); + } + + //////////////////////////////////////////////////////////////////////// + // isValid + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if the given vertex index is a valid index into the mesh. */ + inline bool + isValid (const VertexIndex& idx_vertex) const + { + return (idx_vertex >= VertexIndex (0) && idx_vertex < VertexIndex (int (vertices_.size ()))); + } + + /** \brief Check if the given half-edge index is a valid index into the mesh. */ + inline bool + isValid (const HalfEdgeIndex& idx_he) const + { + return (idx_he >= HalfEdgeIndex (0) && idx_he < HalfEdgeIndex (half_edges_.size ())); + } + + /** \brief Check if the given edge index is a valid index into the mesh. */ + inline bool + isValid (const EdgeIndex& idx_edge) const + { + return (idx_edge >= EdgeIndex (0) && idx_edge < EdgeIndex (half_edges_.size () / 2)); + } + + /** \brief Check if the given face index is a valid index into the mesh. */ + inline bool + isValid (const FaceIndex& idx_face) const + { + return (idx_face >= FaceIndex (0) && idx_face < FaceIndex (faces_.size ())); + } + + //////////////////////////////////////////////////////////////////////// + // isDeleted + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if the given vertex is marked as deleted. */ + inline bool + isDeleted (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ()); + } + + /** \brief Check if the given half-edge is marked as deleted. */ + inline bool + isDeleted (const HalfEdgeIndex& idx_he) const + { + assert (this->isValid (idx_he)); + return (!this->getTerminatingVertexIndex (idx_he).isValid ()); + } + + /** \brief Check if the given edge (any of the two half-edges) is marked as deleted. */ + inline bool + isDeleted (const EdgeIndex& idx_edge) const + { + assert (this->isValid (idx_edge)); + return (this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, true)) || + this->isDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false))); + } + + /** \brief Check if the given face is marked as deleted. */ + inline bool + isDeleted (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (!this->getInnerHalfEdgeIndex (idx_face).isValid ()); + } + + //////////////////////////////////////////////////////////////////////// + // isIsolated + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if the given vertex is isolated (not connected to other elements). */ + inline bool + isIsolated (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (!this->getOutgoingHalfEdgeIndex (idx_vertex).isValid ()); + } + + //////////////////////////////////////////////////////////////////////// + // isBoundary + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if the given vertex lies on the boundary. Isolated vertices are considered to be on the boundary. */ + inline bool + isBoundary (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + if (this->isIsolated (idx_vertex)) return (true); + return (this->isBoundary (this->getOutgoingHalfEdgeIndex (idx_vertex))); + } + + /** \brief Check if the given half-edge lies on the bounddary. */ + inline bool + isBoundary (const HalfEdgeIndex& idx_he) const + { + assert (this->isValid (idx_he)); + return (!this->getFaceIndex (idx_he).isValid ()); + } + + /** \brief Check if the given edge lies on the boundary (any of the two half-edges lies on the boundary. */ + inline bool + isBoundary (const EdgeIndex& idx_edge) const + { + assert (this->isValid (idx_edge)); + const HalfEdgeIndex& idx = pcl::geometry::toHalfEdgeIndex (idx_edge); + return (this->isBoundary (idx) || this->isBoundary (this->getOppositeHalfEdgeIndex (idx))); + } + + /** \brief Check if the given face lies on the boundary. There are two versions of this method, selected by the template parameter. + * \tparam CheckVerticesT Check if any vertex lies on the boundary (true) or check if any edge lies on the boundary (false). + */ + template inline bool + isBoundary (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (this->isBoundary (idx_face, std::integral_constant ())); + } + + /** \brief Check if the given face lies on the boundary. This method uses isBoundary \c true which checks if any vertex lies on the boundary. */ + inline bool + isBoundary (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (this->isBoundary (idx_face, std::true_type ())); + } + + //////////////////////////////////////////////////////////////////////// + // isManifold + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if the given vertex is manifold. Isolated vertices are manifold. */ + inline bool + isManifold (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + if (this->isIsolated (idx_vertex)) return (true); + return (this->isManifold (idx_vertex, IsManifold ())); + } + + /** \brief Check if the mesh is manifold. */ + inline bool + isManifold () const + { + return (this->isManifold (IsManifold ())); + } + + //////////////////////////////////////////////////////////////////////// + // size + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the number of the vertices. */ + inline std::size_t + sizeVertices () const + { + return (vertices_.size ()); + } + + /** \brief Get the number of the half-edges. */ + inline std::size_t + sizeHalfEdges () const + { + assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh. + return (half_edges_.size ()); + } + + /** \brief Get the number of the edges. */ + inline std::size_t + sizeEdges () const + { + assert (half_edges_.size () % 2 == 0); // This would be a bug in the mesh. + return (half_edges_.size () / 2); + } + + /** \brief Get the number of the faces. */ + inline std::size_t + sizeFaces () const + { + return (faces_.size ()); + } + + //////////////////////////////////////////////////////////////////////// + // empty + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if the mesh is empty. */ + inline bool + empty () const + { + return (this->emptyVertices () && this->emptyEdges () && this->emptyFaces ()); + } + + /** \brief Check if the vertices are empty. */ + inline bool + emptyVertices () const + { + return (vertices_.empty ()); + } + + /** \brief Check if the edges are empty. */ + inline bool + emptyEdges () const + { + return (half_edges_.empty ()); + } + + /** \brief Check if the faces are empty. */ + inline bool + emptyFaces () const + { + return (faces_.empty ()); + } + + //////////////////////////////////////////////////////////////////////// + // reserve + //////////////////////////////////////////////////////////////////////// + + /** \brief Reserve storage space n vertices. */ + inline void + reserveVertices (const std::size_t n) + { + vertices_.reserve (n); + this->reserveData (vertex_data_cloud_, n, HasVertexData ()); + } + + /** \brief Reserve storage space for n edges (2*n storage space is reserved for the half-edges). */ + inline void + reserveEdges (const std::size_t n) + { + half_edges_.reserve (2*n); + this->reserveData (half_edge_data_cloud_, 2*n, HasHalfEdgeData ()); + this->reserveData (edge_data_cloud_ , n, HasEdgeData ()); + } + + /** \brief Reserve storage space for n faces. */ + inline void + reserveFaces (const std::size_t n) + { + faces_.reserve (n); + this->reserveData (face_data_cloud_, n, HasFaceData ()); + } + + //////////////////////////////////////////////////////////////////////// + // resize + //////////////////////////////////////////////////////////////////////// + + /** \brief Resize the the vertices to n elements. */ + inline void + resizeVertices (const std::size_t n, const VertexData& data = VertexData ()) + { + vertices_.resize (n); + this->resizeData (vertex_data_cloud_, n, data, HasVertexData ()); + } + + /** \brief Resize the edges to n elements (half-edges will hold 2*n elements). */ + inline void + resizeEdges (const std::size_t n, + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData he_data = HalfEdgeData ()) + { + half_edges_.resize (2*n); + this->resizeData (half_edge_data_cloud_, 2*n, he_data , HasHalfEdgeData ()); + this->resizeData (edge_data_cloud_ , n, edge_data, HasEdgeData ()); + } + + /** \brief Resize the faces to n elements. */ + inline void + resizeFaces (const std::size_t n, const FaceData& data = FaceData ()) + { + faces_.resize (n); + this->resizeData (face_data_cloud_, n, data, HasFaceData ()); + } + + //////////////////////////////////////////////////////////////////////// + // clear + //////////////////////////////////////////////////////////////////////// + + /** \brief Clear all mesh elements and data. */ + void + clear () + { + vertices_.clear (); + half_edges_.clear (); + faces_.clear (); + + this->clearData (vertex_data_cloud_ , HasVertexData ()); + this->clearData (half_edge_data_cloud_, HasHalfEdgeData ()); + this->clearData (edge_data_cloud_ , HasEdgeData ()); + this->clearData (face_data_cloud_ , HasFaceData ()); + } + + //////////////////////////////////////////////////////////////////////// + // get / set the vertex data cloud + //////////////////////////////////////////////////////////////////////// + + /** \brief Get access to the stored vertex data. + * \warning Please make sure to NOT add or remove elements from the cloud. + */ + inline VertexDataCloud& + getVertexDataCloud () + { + return (vertex_data_cloud_); + } + + /** \brief Get the stored vertex data. */ + inline VertexDataCloud + getVertexDataCloud () const + { + return (vertex_data_cloud_); + } + + /** \brief Change the stored vertex data. + * \param[in] vertex_data_cloud The new vertex data. Must be the same as the current data. + * \return true if the cloud could be set. + */ + inline bool + setVertexDataCloud (const VertexDataCloud& vertex_data_cloud) + { + if (vertex_data_cloud.size () == vertex_data_cloud_.size ()) + { + vertex_data_cloud_ = vertex_data_cloud; + return (true); + } + return (false); + } + + //////////////////////////////////////////////////////////////////////// + // get / set the half-edge data cloud + //////////////////////////////////////////////////////////////////////// + + /** \brief Get access to the stored half-edge data. + * \warning Please make sure to NOT add or remove elements from the cloud. + */ + inline HalfEdgeDataCloud& + getHalfEdgeDataCloud () + { + return (half_edge_data_cloud_); + } + + /** \brief Get the stored half-edge data. */ + inline HalfEdgeDataCloud + getHalfEdgeDataCloud () const + { + return (half_edge_data_cloud_); + } + + /** \brief Change the stored half-edge data. + * \param[in] half_edge_data_cloud The new half-edge data. Must be the same as the current data. + * \return true if the cloud could be set. + */ + inline bool + setHalfEdgeDataCloud (const HalfEdgeDataCloud& half_edge_data_cloud) + { + if (half_edge_data_cloud.size () == half_edge_data_cloud_.size ()) + { + half_edge_data_cloud_ = half_edge_data_cloud; + return (true); + } + return (false); + } + + //////////////////////////////////////////////////////////////////////// + // get / set the edge data cloud + //////////////////////////////////////////////////////////////////////// + + /** \brief Get access to the stored edge data. + * \warning Please make sure to NOT add or remove elements from the cloud. + */ + inline EdgeDataCloud& + getEdgeDataCloud () + { + return (edge_data_cloud_); + } + + /** \brief Get the stored edge data. */ + inline EdgeDataCloud + getEdgeDataCloud () const + { + return (edge_data_cloud_); + } + + /** \brief Change the stored edge data. + * \param[in] edge_data_cloud The new edge data. Must be the same as the current data. + * \return true if the cloud could be set. + */ + inline bool + setEdgeDataCloud (const EdgeDataCloud& edge_data_cloud) + { + if (edge_data_cloud.size () == edge_data_cloud_.size ()) + { + edge_data_cloud_ = edge_data_cloud; + return (true); + } + return (false); + } + + //////////////////////////////////////////////////////////////////////// + // get / set the face data cloud + //////////////////////////////////////////////////////////////////////// + + /** \brief Get access to the stored face data. + * \warning Please make sure to NOT add or remove elements from the cloud. + */ + inline FaceDataCloud& + getFaceDataCloud () + { + return (face_data_cloud_); + } + + /** \brief Get the stored face data. */ + inline FaceDataCloud + getFaceDataCloud () const + { + return (face_data_cloud_); + } + + /** \brief Change the stored face data. + * \param[in] face_data_cloud The new face data. Must be the same as the current data. + * \return true if the cloud could be set. + */ + inline bool + setFaceDataCloud (const FaceDataCloud& face_data_cloud) + { + if (face_data_cloud.size () == face_data_cloud_.size ()) + { + face_data_cloud_ = face_data_cloud; + return (true); + } + return (false); + } + + //////////////////////////////////////////////////////////////////////// + // getVertexIndex / getHalfEdgeIndex / getEdgeIndex / getFaceIndex + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the index associated to the given vertex data. + * \return Invalid index if the mesh does not have associated vertex data. + */ + inline VertexIndex + getVertexIndex (const VertexData& vertex_data) const + { + if (HasVertexData::value) + { + assert (&vertex_data >= &vertex_data_cloud_.front () && &vertex_data <= &vertex_data_cloud_.back ()); + return (VertexIndex (std::distance (&vertex_data_cloud_.front (), &vertex_data))); + } + return (VertexIndex ()); + } + + /** \brief Get the index associated to the given half-edge data. */ + inline HalfEdgeIndex + getHalfEdgeIndex (const HalfEdgeData& half_edge_data) const + { + if (HasHalfEdgeData::value) + { + assert (&half_edge_data >= &half_edge_data_cloud_.front () && &half_edge_data <= &half_edge_data_cloud_.back ()); + return (HalfEdgeIndex (std::distance (&half_edge_data_cloud_.front (), &half_edge_data))); + } + return (HalfEdgeIndex ()); + } + + /** \brief Get the index associated to the given edge data. */ + inline EdgeIndex + getEdgeIndex (const EdgeData& edge_data) const + { + if (HasEdgeData::value) + { + assert (&edge_data >= &edge_data_cloud_.front () && &edge_data <= &edge_data_cloud_.back ()); + return (EdgeIndex (std::distance (&edge_data_cloud_.front (), &edge_data))); + } + return (EdgeIndex ()); + } + + /** \brief Get the index associated to the given face data. */ + inline FaceIndex + getFaceIndex (const FaceData& face_data) const + { + if (HasFaceData::value) + { + assert (&face_data >= &face_data_cloud_.front () && &face_data <= &face_data_cloud_.back ()); + return (FaceIndex (std::distance (&face_data_cloud_.front (), &face_data))); + } + return (FaceIndex ()); + } + + protected: + + //////////////////////////////////////////////////////////////////////// + // Types + //////////////////////////////////////////////////////////////////////// + + // Elements + using Vertex = pcl::geometry::Vertex; + using HalfEdge = pcl::geometry::HalfEdge; + using Face = pcl::geometry::Face; + + using Vertices = std::vector; + using HalfEdges = std::vector; + using Faces = std::vector; + + using VertexIterator = typename Vertices::iterator; + using HalfEdgeIterator = typename HalfEdges::iterator; + using FaceIterator = typename Faces::iterator; + + using VertexConstIterator = typename Vertices::const_iterator; + using HalfEdgeConstIterator = typename HalfEdges::const_iterator; + using FaceConstIterator = typename Faces::const_iterator; + + /** \brief General implementation of addFace. */ + FaceIndex + addFaceImplBase (const VertexIndices& vertices, + const FaceData& face_data, + const EdgeData& edge_data, + const HalfEdgeData& half_edge_data) + { + const int n = static_cast (vertices.size ()); + if (n < 3) return (FaceIndex ()); + + // Check for topological errors + inner_he_.resize (n); + free_he_.resize (n); + is_new_.resize (n); + make_adjacent_.resize (n); + for (int i=0; icheckTopology1 (vertices [i], vertices [(i+1)%n], inner_he_ [i], is_new_ [i], IsManifold ())) + { + return (FaceIndex ()); + } + } + for (int i=0; icheckTopology2 (inner_he_ [i], inner_he_ [j], is_new_ [i], is_new_ [j], this->isIsolated (vertices [j]), make_adjacent_ [i], free_he_ [i], IsManifold ())) + { + return (FaceIndex ()); + } + } + + // Reconnect the existing half-edges if needed + if (!IsManifold::value) + { + for (int i=0; imakeAdjacent (inner_he_ [i], inner_he_ [(i+1)%n], free_he_ [i]); + } + } + } + + // Add new half-edges if needed + for (int i=0; iaddEdge (vertices [i], vertices [(i+1)%n], half_edge_data, edge_data); + } + } + + // Connect + for (int i=0; iconnectNewNew (inner_he_ [i], inner_he_ [j], vertices [j], IsManifold ()); + else if ( is_new_ [i] && !is_new_ [j]) this->connectNewOld (inner_he_ [i], inner_he_ [j], vertices [j]); + else if (!is_new_ [i] && is_new_ [j]) this->connectOldNew (inner_he_ [i], inner_he_ [j], vertices [j]); + else this->connectOldOld (inner_he_ [i], inner_he_ [j], vertices [j], IsManifold ()); + } + return (this->connectFace (inner_he_, face_data)); + } + + //////////////////////////////////////////////////////////////////////// + // addEdge + //////////////////////////////////////////////////////////////////////// + + /** \brief Add an edge between the two given vertices and connect them with the vertices. + * \param[in] idx_v_a The first vertex index + * \param[in] idx_v_b The second vertex index + * \param[in] he_data Data associated with the half-edges. This is only added if the mesh has data associated with the half-edges. + * \param[in] edge_data Data associated with the edge. This is only added if the mesh has data associated with the edges. + * \return Index to the half-edge from vertex a to vertex b. + */ + HalfEdgeIndex + addEdge (const VertexIndex& idx_v_a, + const VertexIndex& idx_v_b, + const HalfEdgeData& he_data, + const EdgeData& edge_data) + { + half_edges_.push_back (HalfEdge (idx_v_b)); + half_edges_.push_back (HalfEdge (idx_v_a)); + + this->addData (half_edge_data_cloud_, he_data , HasHalfEdgeData ()); + this->addData (half_edge_data_cloud_, he_data , HasHalfEdgeData ()); + this->addData (edge_data_cloud_ , edge_data, HasEdgeData ()); + + return (HalfEdgeIndex (static_cast (half_edges_.size () - 2))); + } + + //////////////////////////////////////////////////////////////////////// + // topology checks + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if the edge between the two vertices can be added. + * \param[in] idx_v_a Index to the first vertex. + * \param[in] idx_v_b Index to the second vertex. + * \param[out] idx_he_ab Index to the half-edge ab if is_new_ab=false. + * \param[out] is_new_ab true if the edge between the vertices exists already. Must be initialized with true! + * \return true if the half-edge may be added. + */ + bool + checkTopology1 (const VertexIndex& idx_v_a, + const VertexIndex& idx_v_b, + HalfEdgeIndex& idx_he_ab, + std::vector ::reference is_new_ab, + std::true_type /*is_manifold*/) const + { + is_new_ab = true; + if (this->isIsolated (idx_v_a)) return (true); + + idx_he_ab = this->getOutgoingHalfEdgeIndex (idx_v_a); + + if (!this->isBoundary (idx_he_ab)) return (false); + if (this->getTerminatingVertexIndex (idx_he_ab) == idx_v_b) is_new_ab = false; + return (true); + } + + /** \brief Non manifold version of checkTopology1 */ + bool + checkTopology1 (const VertexIndex& idx_v_a, + const VertexIndex& idx_v_b, + HalfEdgeIndex& idx_he_ab, + std::vector ::reference is_new_ab, + std::false_type /*is_manifold*/) const + { + is_new_ab = true; + if (this->isIsolated (idx_v_a)) return (true); + if (!this->isBoundary (this->getOutgoingHalfEdgeIndex (idx_v_a))) return (false); + + VertexAroundVertexCirculator circ = this->getVertexAroundVertexCirculator (this->getOutgoingHalfEdgeIndex (idx_v_a)); + const VertexAroundVertexCirculator circ_end = circ; + + do + { + if (circ.getTargetIndex () == idx_v_b) + { + idx_he_ab = circ.getCurrentHalfEdgeIndex (); + if (!this->isBoundary (idx_he_ab)) return (false); + + is_new_ab = false; + return (true); + } + } while (++circ!=circ_end); + + return (true); + } + + /** \brief Check if the face may be added (mesh does not become non-manifold). */ + inline bool + checkTopology2 (const HalfEdgeIndex& /*idx_he_ab*/, + const HalfEdgeIndex& /*idx_he_bc*/, + const bool is_new_ab, + const bool is_new_bc, + const bool is_isolated_b, + std::vector ::reference /*make_adjacent_ab_bc*/, + HalfEdgeIndex& /*idx_free_half_edge*/, + std::true_type /*is_manifold*/) const + { + return !(is_new_ab && is_new_bc && !is_isolated_b); + } + + /** \brief Check if the half-edge bc is the next half-edge of ab. + * \param[in] idx_he_ab Index to the half-edge between the vertices a and b. + * \param[in] idx_he_bc Index to the half-edge between the vertices b and c. + * \param[in] is_new_ab Half-edge ab is new. + * \param[in] is_new_bc Half-edge bc is new. + * \param[out] make_adjacent_ab_bc Half-edges ab and bc need to be made adjacent. + * \param[out] idx_free_half_edge Free half-edge (needed for makeAdjacent) + * \return true if addFace may be continued. + */ + inline bool + checkTopology2 (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc, + const bool is_new_ab, + const bool is_new_bc, + const bool /*is_isolated_b*/, + std::vector ::reference make_adjacent_ab_bc, + HalfEdgeIndex& idx_free_half_edge, + std::false_type /*is_manifold*/) const + { + if (is_new_ab || is_new_bc) + { + make_adjacent_ab_bc = false; + return (true); // Make adjacent is only needed for two old half-edges + } + + if (this->getNextHalfEdgeIndex (idx_he_ab) == idx_he_bc) + { + make_adjacent_ab_bc = false; + return (true); // already adjacent + } + + make_adjacent_ab_bc = true; + + // Find the next boundary half edge + IncomingHalfEdgeAroundVertexCirculator circ = this->getIncomingHalfEdgeAroundVertexCirculator (this->getOppositeHalfEdgeIndex (idx_he_bc)); + + do ++circ; while (!this->isBoundary (circ.getTargetIndex ())); + idx_free_half_edge = circ.getTargetIndex (); + + // This would detach the faces around the vertex from each other. + return (circ.getTargetIndex () != idx_he_ab); + } + + /** \brief Make the half-edges bc the next half-edge of ab. + * \param[in] idx_he_ab Index to the half-edge between the vertices a and b. + * \param[in] idx_he_bc Index to the half-edge between the vertices b and c. + * \param[in, out] idx_free_half_edge Free half-edge needed to re-connect the half-edges around vertex b. + */ + void + makeAdjacent (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc, + HalfEdgeIndex& idx_free_half_edge) + { + // Re-link. No references! + const HalfEdgeIndex idx_he_ab_next = this->getNextHalfEdgeIndex (idx_he_ab); + const HalfEdgeIndex idx_he_bc_prev = this->getPrevHalfEdgeIndex (idx_he_bc); + const HalfEdgeIndex idx_he_free_next = this->getNextHalfEdgeIndex (idx_free_half_edge); + + this->connectPrevNext (idx_he_ab, idx_he_bc); + this->connectPrevNext (idx_free_half_edge, idx_he_ab_next); + this->connectPrevNext (idx_he_bc_prev, idx_he_free_next); + } + + //////////////////////////////////////////////////////////////////////// + // connect + //////////////////////////////////////////////////////////////////////// + + /** \brief Add a face to the mesh and connect it to the half-edges. + * \param[in] inner_he Inner half-edges of the face. + * \param[in] face_data Data that is stored in the face. This is only added if the mesh has data associated with the faces. + * \return Index to the new face. + */ + FaceIndex + connectFace (const HalfEdgeIndices& inner_he, + const FaceData& face_data) + { + faces_.push_back (Face (inner_he.back ())); + this->addData (face_data_cloud_, face_data, HasFaceData ()); + + const FaceIndex idx_face (static_cast (this->sizeFaces () - 1)); + + for (const auto &idx_half_edge : inner_he) + { + this->setFaceIndex (idx_half_edge, idx_face); + } + + return (idx_face); + } + + /** \brief Connect the next and prev indices of the two half-edges with each other. */ + inline void + connectPrevNext (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc) + { + this->setNextHalfEdgeIndex (idx_he_ab, idx_he_bc); + this->setPrevHalfEdgeIndex (idx_he_bc, idx_he_ab); + } + + /** \brief Both half-edges are new (manifold version). */ + void + connectNewNew (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc, + const VertexIndex& idx_v_b, + std::true_type /*is_manifold*/) + { + const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab); + const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc); + + this->connectPrevNext (idx_he_ab, idx_he_bc); + this->connectPrevNext (idx_he_cb, idx_he_ba); + + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ba); + } + + /** \brief Both half-edges are new (non-manifold version). */ + void + connectNewNew (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc, + const VertexIndex& idx_v_b, + std::false_type /*is_manifold*/) + { + if (this->isIsolated (idx_v_b)) + { + this->connectNewNew (idx_he_ab, idx_he_bc, idx_v_b, std::true_type ()); + } + else + { + const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab); + const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc); + + // No references! + const HalfEdgeIndex idx_he_b_out = this->getOutgoingHalfEdgeIndex (idx_v_b); + const HalfEdgeIndex idx_he_b_out_prev = this->getPrevHalfEdgeIndex (idx_he_b_out); + + this->connectPrevNext (idx_he_ab, idx_he_bc); + this->connectPrevNext (idx_he_cb, idx_he_b_out); + this->connectPrevNext (idx_he_b_out_prev, idx_he_ba); + } + } + + /** \brief The first half-edge is new. */ + void + connectNewOld (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc, + const VertexIndex& idx_v_b) + { + const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab); + const HalfEdgeIndex idx_he_bc_prev = this->getPrevHalfEdgeIndex (idx_he_bc); // No reference! + + this->connectPrevNext (idx_he_ab, idx_he_bc); + this->connectPrevNext (idx_he_bc_prev, idx_he_ba); + + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ba); + } + + /** \brief The second half-edge is new. */ + void + connectOldNew (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc, + const VertexIndex& idx_v_b) + { + const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc); + const HalfEdgeIndex idx_he_ab_next = this->getNextHalfEdgeIndex (idx_he_ab); // No reference! + + this->connectPrevNext (idx_he_ab, idx_he_bc); + this->connectPrevNext (idx_he_cb, idx_he_ab_next); + + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ab_next); + } + + /** \brief Both half-edges are old (manifold version). */ + void + connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/, + const HalfEdgeIndex& /*idx_he_bc*/, + const VertexIndex& /*idx_v_b*/, + std::true_type /*is_manifold*/) + { + } + + /** \brief Both half-edges are old (non-manifold version). */ + void + connectOldOld (const HalfEdgeIndex& /*idx_he_ab*/, + const HalfEdgeIndex& idx_he_bc, + const VertexIndex& idx_v_b, + std::false_type /*is_manifold*/) + { + const HalfEdgeIndex& idx_he_b_out = this->getOutgoingHalfEdgeIndex (idx_v_b); + + // The outgoing half edge MUST be a boundary half-edge (if there is one) + if (idx_he_b_out == idx_he_bc) // he_bc is no longer on the boundary + { + OutgoingHalfEdgeAroundVertexCirculator circ = this->getOutgoingHalfEdgeAroundVertexCirculator (idx_he_b_out); + const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ; + + while (++circ!=circ_end) + { + if (this->isBoundary (circ.getTargetIndex ())) + { + this->setOutgoingHalfEdgeIndex (idx_v_b, circ.getTargetIndex ()); + return; + } + } + } + } + + //////////////////////////////////////////////////////////////////////// + // addData + //////////////////////////////////////////////////////////////////////// + + /** \brief Add mesh data. */ + template + inline void + addData (pcl::PointCloud & cloud, const DataT& data, std::true_type /*has_data*/) + { + cloud.push_back (data); + } + + /** \brief Does nothing. */ + template + inline void + addData (pcl::PointCloud & /*cloud*/, const DataT& /*data*/, std::false_type /*has_data*/) + { + } + + //////////////////////////////////////////////////////////////////////// + // deleteFace + //////////////////////////////////////////////////////////////////////// + + /** \brief Manifold version of deleteFace. If the mesh becomes non-manifold due to the delete operation the faces around the non-manifold vertex are deleted until the mesh becomes manifold again. */ + void + deleteFace (const FaceIndex& idx_face, + std::true_type /*is_manifold*/) + { + assert (this->isValid (idx_face)); + delete_faces_face_.clear (); + delete_faces_face_.push_back (idx_face); + + while (!delete_faces_face_.empty ()) + { + const FaceIndex idx_face_cur = delete_faces_face_.back (); + delete_faces_face_.pop_back (); + + // This calls the non-manifold version of deleteFace, which will call the manifold version of reconnect. + this->deleteFace (idx_face_cur, std::false_type ()); + } + } + + /** \brief Non-manifold version of deleteFace. */ + void + deleteFace (const FaceIndex& idx_face, + std::false_type /*is_manifold*/) + { + assert (this->isValid (idx_face)); + if (this->isDeleted (idx_face)) return; + + // Store all half-edges in the face + inner_he_.clear (); + is_boundary_.clear (); + InnerHalfEdgeAroundFaceCirculator circ = this->getInnerHalfEdgeAroundFaceCirculator (idx_face); + const InnerHalfEdgeAroundFaceCirculator circ_end = circ; + do + { + inner_he_.push_back (circ.getTargetIndex ()); + is_boundary_.push_back (this->isBoundary (this->getOppositeHalfEdgeIndex (circ.getTargetIndex ()))); + } while (++circ != circ_end); + assert (inner_he_.size () >= 3); // Minimum should be a triangle. + + const int n = static_cast (inner_he_.size ()); + int j; + + if (IsManifold::value) + { + for (int i=0; ireconnect (inner_he_ [i], inner_he_ [j], is_boundary_ [i], is_boundary_ [j]); + } + for (int i=0; igetHalfEdge (inner_he_ [i]).idx_face_.invalidate (); + } + } + else + { + for (int i=0; ireconnect (inner_he_ [i], inner_he_ [j], is_boundary_ [i], is_boundary_ [j]); + this->getHalfEdge (inner_he_ [i]).idx_face_.invalidate (); + } + } + + this->markDeleted (idx_face); + } + + //////////////////////////////////////////////////////////////////////// + // reconnect + //////////////////////////////////////////////////////////////////////// + + /** \brief Deconnect the input half-edges from the mesh and adjust the indices of the connected half-edges. */ + void + reconnect (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_bc, + const bool is_boundary_ba, + const bool is_boundary_cb) + { + const HalfEdgeIndex idx_he_ba = this->getOppositeHalfEdgeIndex (idx_he_ab); + const HalfEdgeIndex idx_he_cb = this->getOppositeHalfEdgeIndex (idx_he_bc); + const VertexIndex idx_v_b = this->getTerminatingVertexIndex (idx_he_ab); + + if (is_boundary_ba && is_boundary_cb) // boundary - boundary + { + const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex (idx_he_cb); + + if (idx_he_cb_next == idx_he_ba) // Vertex b is isolated + { + this->markDeleted (idx_v_b); + } + else + { + this->connectPrevNext (this->getPrevHalfEdgeIndex (idx_he_ba), idx_he_cb_next); + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_cb_next); + } + + this->markDeleted (idx_he_ab); + this->markDeleted (idx_he_ba); + } + else if (is_boundary_ba && !is_boundary_cb) // boundary - no boundary + { + this->connectPrevNext (this->getPrevHalfEdgeIndex (idx_he_ba), idx_he_bc); + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc); + + this->markDeleted (idx_he_ab); + this->markDeleted (idx_he_ba); + } + else if (!is_boundary_ba && is_boundary_cb) // no boundary - boundary + { + const HalfEdgeIndex& idx_he_cb_next = this->getNextHalfEdgeIndex (idx_he_cb); + this->connectPrevNext (idx_he_ab, idx_he_cb_next); + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_cb_next); + } + else // no boundary - no boundary + { + this->reconnectNBNB (idx_he_bc, idx_he_cb, idx_v_b, IsManifold ()); + } + } + + /** \brief Both edges are not on the boundary. Manifold version. */ + void + reconnectNBNB (const HalfEdgeIndex& idx_he_bc, + const HalfEdgeIndex& idx_he_cb, + const VertexIndex& idx_v_b, + std::true_type /*is_manifold*/) + { + if (this->isBoundary (idx_v_b)) + { + // Deletion of this face makes the mesh non-manifold + // -> delete the neighboring faces until it is manifold again + IncomingHalfEdgeAroundVertexCirculator circ = this->getIncomingHalfEdgeAroundVertexCirculator (idx_he_cb); + + while (!this->isBoundary (circ.getTargetIndex ())) + { + delete_faces_face_.push_back (this->getFaceIndex ((circ++).getTargetIndex ())); + +#ifdef PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2 + if (circ == this->getIncomingHalfEdgeAroundVertexCirculator (idx_he_cb)) // Abort infinity loop + { + // In a manifold mesh we can't invalidate the face while reconnecting! + // See the implementation of + // deleteFace (const FaceIndex& idx_face, + // std::false_type /*is_manifold*/) + pcl::geometry::g_pcl_geometry_mesh_base_test_delete_face_manifold_2_success = false; + return; + } +#endif + } + } + else + { + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc); + } + } + + /** \brief Both edges are not on the boundary. Non-manifold version. */ + void + reconnectNBNB (const HalfEdgeIndex& idx_he_bc, + const HalfEdgeIndex& /*idx_he_cb*/, + const VertexIndex& idx_v_b, + std::false_type /*is_manifold*/) + { + if (!this->isBoundary (idx_v_b)) + { + this->setOutgoingHalfEdgeIndex (idx_v_b, idx_he_bc); + } + } + + //////////////////////////////////////////////////////////////////////// + // markDeleted + //////////////////////////////////////////////////////////////////////// + + /** \brief Mark the given vertex as deleted. */ + inline void + markDeleted (const VertexIndex& idx_vertex) + { + assert (this->isValid (idx_vertex)); + this->getVertex (idx_vertex).idx_outgoing_half_edge_.invalidate (); + } + + /** \brief Mark the given half-edge as deleted. */ + inline void + markDeleted (const HalfEdgeIndex& idx_he) + { + assert (this->isValid (idx_he)); + this->getHalfEdge (idx_he).idx_terminating_vertex_.invalidate (); + } + + /** \brief Mark the given edge (both half-edges) as deleted. */ + inline void + markDeleted (const EdgeIndex& idx_edge) + { + assert (this->isValid (idx_edge)); + this->markDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, true)); + this->markDeleted (pcl::geometry::toHalfEdgeIndex (idx_edge, false)); + } + + /** \brief Mark the given face as deleted. */ + inline void + markDeleted (const FaceIndex& idx_face) + { + assert (this->isValid (idx_face)); + this->getFace (idx_face).idx_inner_half_edge_.invalidate (); + } + + //////////////////////////////////////////////////////////////////////// + // For cleanUp + //////////////////////////////////////////////////////////////////////// + + /** \brief Removes mesh elements and data that are marked as deleted from the container. + * \tparam ElementContainerT e.g. std::vector \ + * \tparam DataContainerT e.g. std::vector \ + * \tparam IndexContainerT e.g. std::vector \ + * \tparam HasDataT Integral constant specifying if the mesh has data associated with the elements. + * + * \param[in, out] elements Container for the mesh elements. Resized to the new size. + * \param[in, out] data_cloud Container for the mesh data. Resized to the new size. + * \return Container with the same size as the old input data. Holds the indices to the new elements for each non-deleted element and an invalid index if it is deleted. + */ + template IndexContainerT + remove (ElementContainerT& elements, DataContainerT& data_cloud) + { + using Index = typename IndexContainerT::value_type; + using Element = typename ElementContainerT::value_type; + + if (HasDataT::value) assert (elements.size () == data_cloud.size ()); + else assert (data_cloud.empty ()); // Bug in this class! + + IndexContainerT new_indices (elements.size (), typename IndexContainerT::value_type ()); + Index ind_old (0), ind_new (0); + + typename ElementContainerT::const_iterator it_e_old = elements.begin (); + typename ElementContainerT::iterator it_e_new = elements.begin (); + + typename DataContainerT::const_iterator it_d_old = data_cloud.begin (); + typename DataContainerT::iterator it_d_new = data_cloud.begin (); + + typename IndexContainerT::iterator it_ind_new = new_indices.begin (); + typename IndexContainerT::const_iterator it_ind_new_end = new_indices.end (); + + while (it_ind_new!=it_ind_new_end) + { + if (!this->isDeleted (ind_old)) + { + *it_ind_new = ind_new++; + + // TODO: Test for self assignment? + *it_e_new++ = *it_e_old; + this->assignIf (it_d_old, it_d_new, HasDataT ()); + this->incrementIf ( it_d_new, HasDataT ()); + } + ++ind_old; + ++it_e_old; + this->incrementIf (it_d_old, HasDataT ()); + ++it_ind_new; + } + + elements.resize (ind_new.get (), Element ()); + if (HasDataT::value) + { + data_cloud.resize (ind_new.get ()); + } + else if (it_d_old != data_cloud.begin () || it_d_new != data_cloud.begin ()) + { + std::cerr << "TODO: Bug in MeshBase::remove!\n"; + assert (false); + exit (EXIT_FAILURE); + } + + return (new_indices); + } + + /** \brief Increment the iterator. */ + template inline void + incrementIf (IteratorT& it, std::true_type /*has_data*/) const + { + ++it; + } + + /** \brief Does nothing. */ + template inline void + incrementIf (IteratorT& /*it*/, std::false_type /*has_data*/) const + { + } + + /** \brief Assign the source iterator to the target iterator. */ + template inline void + assignIf (const ConstIteratorT source, IteratorT target, std::true_type /*has_data*/) const + { + *target = *source; + } + + /** \brief Does nothing. */ + template inline void + assignIf (const ConstIteratorT /*source*/, IteratorT /*target*/, std::false_type /*has_data*/) const + { + } + + //////////////////////////////////////////////////////////////////////// + // Vertex / Half-edge / Face connectivity + //////////////////////////////////////////////////////////////////////// + + /** \brief Set the outgoing half-edge index to a given vertex. */ + inline void + setOutgoingHalfEdgeIndex (const VertexIndex& idx_vertex, const HalfEdgeIndex& idx_outgoing_half_edge) + { + assert (this->isValid (idx_vertex)); + this->getVertex (idx_vertex).idx_outgoing_half_edge_ = idx_outgoing_half_edge; + } + + /** \brief Set the terminating vertex index to a given half-edge. */ + inline void + setTerminatingVertexIndex (const HalfEdgeIndex& idx_half_edge, const VertexIndex& idx_terminating_vertex) + { + assert (this->isValid (idx_half_edge)); + this->getHalfEdge (idx_half_edge).idx_terminating_vertex_ = idx_terminating_vertex; + } + + /** \brief Set the next half_edge index to a given half-edge. */ + inline void + setNextHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge, const HalfEdgeIndex& idx_next_half_edge) + { + assert (this->isValid (idx_half_edge)); + this->getHalfEdge (idx_half_edge).idx_next_half_edge_ = idx_next_half_edge; + } + + /** \brief Set the previous half-edge index to a given half-edge. */ + inline void + setPrevHalfEdgeIndex (const HalfEdgeIndex& idx_half_edge, + const HalfEdgeIndex& idx_prev_half_edge) + { + assert (this->isValid (idx_half_edge)); + this->getHalfEdge (idx_half_edge).idx_prev_half_edge_ = idx_prev_half_edge; + } + + /** \brief Set the face index to a given half-edge. */ + inline void + setFaceIndex (const HalfEdgeIndex& idx_half_edge, const FaceIndex& idx_face) + { + assert (this->isValid (idx_half_edge)); + this->getHalfEdge (idx_half_edge).idx_face_ = idx_face; + } + + /** \brief Set the inner half-edge index to a given face. */ + inline void + setInnerHalfEdgeIndex (const FaceIndex& idx_face, const HalfEdgeIndex& idx_inner_half_edge) + { + assert (this->isValid (idx_face)); + this->getFace (idx_face).idx_inner_half_edge_ = idx_inner_half_edge; + } + + //////////////////////////////////////////////////////////////////////// + // isBoundary / isManifold + //////////////////////////////////////////////////////////////////////// + + /** \brief Check if any vertex of the face lies on the boundary. */ + bool + isBoundary (const FaceIndex& idx_face, std::true_type /*check_vertices*/) const + { + VertexAroundFaceCirculator circ = this->getVertexAroundFaceCirculator (idx_face); + const VertexAroundFaceCirculator circ_end = circ; + + do + { + if (this->isBoundary (circ.getTargetIndex ())) + { + return (true); + } + } while (++circ!=circ_end); + + return (false); + } + + /** \brief Check if any edge of the face lies on the boundary. */ + bool + isBoundary (const FaceIndex& idx_face, std::false_type /*check_vertices*/) const + { + OuterHalfEdgeAroundFaceCirculator circ = this->getOuterHalfEdgeAroundFaceCirculator (idx_face); + const OuterHalfEdgeAroundFaceCirculator circ_end = circ; + + do + { + if (this->isBoundary (circ.getTargetIndex ())) + { + return (true); + } + } while (++circ!=circ_end); + + return (false); + } + + /** \brief Always manifold. */ + inline bool + isManifold (const VertexIndex&, std::true_type /*is_manifold*/) const + { + return (true); + } + + /** \brief Check if the given vertex is manifold. */ + bool + isManifold (const VertexIndex& idx_vertex, std::false_type /*is_manifold*/) const + { + OutgoingHalfEdgeAroundVertexCirculator circ = this->getOutgoingHalfEdgeAroundVertexCirculator (idx_vertex); + const OutgoingHalfEdgeAroundVertexCirculator circ_end = circ; + + if (!this->isBoundary ((circ++).getTargetIndex ())) return (true); + do + { + if (this->isBoundary (circ.getTargetIndex ())) return (false); + } while (++circ != circ_end); + + return (true); + } + + /** \brief Always manifold. */ + inline bool + isManifold (std::true_type /*is_manifold*/) const + { + return (true); + } + + /** \brief Check if all vertices in the mesh are manifold. */ + bool + isManifold (std::false_type /*is_manifold*/) const + { + for (std::size_t i=0; isizeVertices (); ++i) + { + if (!this->isManifold (VertexIndex (i))) return (false); + } + return (true); + } + + //////////////////////////////////////////////////////////////////////// + // reserveData / resizeData / clearData + //////////////////////////////////////////////////////////////////////// + + /** \brief Reserve storage space for the mesh data. */ + template inline void + reserveData (DataCloudT& cloud, const std::size_t n, std::true_type /*has_data*/) const + { + cloud.reserve (n); + } + + /** \brief Does nothing */ + template inline void + reserveData (DataCloudT& /*cloud*/, const std::size_t /*n*/, std::false_type /*has_data*/) const + { + } + + /** \brief Resize the mesh data. */ + template inline void + resizeData (DataCloudT& /*data_cloud*/, const std::size_t n, const typename DataCloudT::value_type& data, std::true_type /*has_data*/) const + { + data.resize (n, data); + } + + /** \brief Does nothing. */ + template inline void + resizeData (DataCloudT& /*data_cloud*/, const std::size_t /*n*/, const typename DataCloudT::value_type& /*data*/, std::false_type /*has_data*/) const + { + } + + /** \brief Clear the mesh data. */ + template inline void + clearData (DataCloudT& cloud, std::true_type /*has_data*/) const + { + cloud.clear (); + } + + /** \brief Does nothing. */ + template inline void + clearData (DataCloudT& /*cloud*/, std::false_type /*has_data*/) const + { + } + + //////////////////////////////////////////////////////////////////////// + // get / set Vertex + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the vertex for the given index. */ + inline Vertex& + getVertex (const VertexIndex& idx_vertex) + { + assert (this->isValid (idx_vertex)); + return (vertices_ [idx_vertex.get ()]); + } + + /** \brief Get the vertex for the given index. */ + inline Vertex + getVertex (const VertexIndex& idx_vertex) const + { + assert (this->isValid (idx_vertex)); + return (vertices_ [idx_vertex.get ()]); + } + + /** \brief Set the vertex at the given index. */ + inline void + setVertex (const VertexIndex& idx_vertex, const Vertex& vertex) + { + assert (this->isValid (idx_vertex)); + vertices_ [idx_vertex.get ()] = vertex; + } + + //////////////////////////////////////////////////////////////////////// + // get / set HalfEdge + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the half-edge for the given index. */ + inline HalfEdge& + getHalfEdge (const HalfEdgeIndex& idx_he) + { + assert (this->isValid (idx_he)); + return (half_edges_ [idx_he.get ()]); + } + + /** \brief Get the half-edge for the given index. */ + inline HalfEdge + getHalfEdge (const HalfEdgeIndex& idx_he) const + { + assert (this->isValid (idx_he)); + return (half_edges_ [idx_he.get ()]); + } + + /** \brief Set the half-edge at the given index. */ + inline void + setHalfEdge (const HalfEdgeIndex& idx_he, const HalfEdge& half_edge) + { + assert (this->isValid (idx_he)); + half_edges_ [idx_he.get ()] = half_edge; + } + + //////////////////////////////////////////////////////////////////////// + // get / set Face + //////////////////////////////////////////////////////////////////////// + + /** \brief Get the face for the given index. */ + inline Face& + getFace (const FaceIndex& idx_face) + { + assert (this->isValid (idx_face)); + return (faces_ [idx_face.get ()]); + } + + /** \brief Get the face for the given index. */ + inline Face + getFace (const FaceIndex& idx_face) const + { + assert (this->isValid (idx_face)); + return (faces_ [idx_face.get ()]); + } + + /** \brief Set the face at the given index. */ + inline void + setFace (const FaceIndex& idx_face, const Face& face) + { + assert (this->isValid (idx_face)); + faces_ [idx_face.get ()] = face; + } + + private: + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + /** \brief Data stored for the vertices. */ + VertexDataCloud vertex_data_cloud_; + + /** \brief Data stored for the half-edges. */ + HalfEdgeDataCloud half_edge_data_cloud_; + + /** \brief Data stored for the edges. */ + EdgeDataCloud edge_data_cloud_; + + /** \brief Data stored for the faces. */ + FaceDataCloud face_data_cloud_; + + /** \brief Connectivity information for the vertices. */ + Vertices vertices_; + + /** \brief Connectivity information for the half-edges. */ + HalfEdges half_edges_; + + /** \brief Connectivity information for the faces. */ + Faces faces_; + + // NOTE: It is MUCH faster to store these variables permamently. + + /** \brief Storage for addFaceImplBase and deleteFace. */ + HalfEdgeIndices inner_he_; + + /** \brief Storage for addFaceImplBase. */ + HalfEdgeIndices free_he_; + + /** \brief Storage for addFaceImplBase. */ + std::vector is_new_; + + /** \brief Storage for addFaceImplBase. */ + std::vector make_adjacent_; + + /** \brief Storage for deleteFace. */ + std::vector is_boundary_; + + /** \brief Storage for deleteVertex. */ + FaceIndices delete_faces_vertex_; + + /** \brief Storage for deleteFace. */ + FaceIndices delete_faces_face_; + + public: + + template + friend class pcl::geometry::MeshIO; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } // End namespace geometry +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/mesh_circulators.h b/deps_install/include/pcl-1.10/pcl/geometry/mesh_circulators.h new file mode 100755 index 0000000..18dae53 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/mesh_circulators.h @@ -0,0 +1,912 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +// NOTE: This file has been created with 'pcl_src/geometry/include/pcl/geometry/mesh_circulators.py' + +#pragma once + +#include +#include + +//////////////////////////////////////////////////////////////////////////////// +// VertexAroundVertexCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates counter-clockwise around a vertex and returns an index to the terminating vertex of the outgoing half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getVertexAroundVertexCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class VertexAroundVertexCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::VertexAroundVertexCirculator; + + using Mesh = MeshT; + using VertexIndex = typename Mesh::VertexIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + VertexAroundVertexCirculator () + : mesh_ (nullptr), + idx_outgoing_half_edge_ () + { + } + + /** \brief Construct from the vertex around which we want to circulate. */ + VertexAroundVertexCirculator (const VertexIndex& idx_vertex, + Mesh*const mesh) + : mesh_ (mesh), + idx_outgoing_half_edge_ (mesh->getOutgoingHalfEdgeIndex (idx_vertex)) + { + } + + /** \brief Construct directly from the outgoing half-edge. */ + VertexAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_outgoing_half_edge_ (idx_outgoing_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_outgoing_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_outgoing_half_edge_)); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getPrevHalfEdgeIndex (idx_outgoing_half_edge_)); + return (*this); + } + + /** \brief Get the index to the target vertex. */ + inline VertexIndex + getTargetIndex () const + { + return (mesh_->getTerminatingVertexIndex (idx_outgoing_half_edge_)); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_outgoing_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The outgoing half-edge of the vertex around which we want to circulate. */ + HalfEdgeIndex idx_outgoing_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// OutgoingHalfEdgeAroundVertexCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates counter-clockwise around a vertex and returns an index to the outgoing half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getOutgoingHalfEdgeAroundVertexCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class OutgoingHalfEdgeAroundVertexCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::OutgoingHalfEdgeAroundVertexCirculator; + + using Mesh = MeshT; + using VertexIndex = typename Mesh::VertexIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + OutgoingHalfEdgeAroundVertexCirculator () + : mesh_ (nullptr), + idx_outgoing_half_edge_ () + { + } + + /** \brief Construct from the vertex around which we want to circulate. */ + OutgoingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex, + Mesh*const mesh) + : mesh_ (mesh), + idx_outgoing_half_edge_ (mesh->getOutgoingHalfEdgeIndex (idx_vertex)) + { + } + + /** \brief Construct directly from the outgoing half-edge. */ + OutgoingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_outgoing_half_edge_ (idx_outgoing_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_outgoing_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_outgoing_half_edge_)); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getPrevHalfEdgeIndex (idx_outgoing_half_edge_)); + return (*this); + } + + /** \brief Get the index to the outgoing half-edge. */ + inline HalfEdgeIndex + getTargetIndex () const + { + return (idx_outgoing_half_edge_); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_outgoing_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The outgoing half-edge of the vertex around which we want to circulate. */ + HalfEdgeIndex idx_outgoing_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// IncomingHalfEdgeAroundVertexCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates counter-clockwise around a vertex and returns an index to the incoming half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getIncomingHalfEdgeAroundVertexCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class IncomingHalfEdgeAroundVertexCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::IncomingHalfEdgeAroundVertexCirculator; + + using Mesh = MeshT; + using VertexIndex = typename Mesh::VertexIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + IncomingHalfEdgeAroundVertexCirculator () + : mesh_ (nullptr), + idx_incoming_half_edge_ () + { + } + + /** \brief Construct from the vertex around which we want to circulate. */ + IncomingHalfEdgeAroundVertexCirculator (const VertexIndex& idx_vertex, + Mesh*const mesh) + : mesh_ (mesh), + idx_incoming_half_edge_ (mesh->getIncomingHalfEdgeIndex (idx_vertex)) + { + } + + /** \brief Construct directly from the incoming half-edge. */ + IncomingHalfEdgeAroundVertexCirculator (const HalfEdgeIndex& idx_incoming_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_incoming_half_edge_ (idx_incoming_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_incoming_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_incoming_half_edge_ == other.idx_incoming_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_incoming_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getNextHalfEdgeIndex (idx_incoming_half_edge_)); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_incoming_half_edge_ = mesh_->getPrevHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_incoming_half_edge_)); + return (*this); + } + + /** \brief Get the index to the incoming half-edge. */ + inline HalfEdgeIndex + getTargetIndex () const + { + return (idx_incoming_half_edge_); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_incoming_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The incoming half-edge of the vertex around which we want to circulate. */ + HalfEdgeIndex idx_incoming_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// FaceAroundVertexCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates counter-clockwise around a vertex and returns an index to the face of the outgoing half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getFaceAroundVertexCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class FaceAroundVertexCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::FaceAroundVertexCirculator; + + using Mesh = MeshT; + using FaceIndex = typename Mesh::FaceIndex; + using VertexIndex = typename Mesh::VertexIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + FaceAroundVertexCirculator () + : mesh_ (nullptr), + idx_outgoing_half_edge_ () + { + } + + /** \brief Construct from the vertex around which we want to circulate. */ + FaceAroundVertexCirculator (const VertexIndex& idx_vertex, + Mesh*const mesh) + : mesh_ (mesh), + idx_outgoing_half_edge_ (mesh->getOutgoingHalfEdgeIndex (idx_vertex)) + { + } + + /** \brief Construct directly from the outgoing half-edge. */ + FaceAroundVertexCirculator (const HalfEdgeIndex& idx_outgoing_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_outgoing_half_edge_ (idx_outgoing_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_outgoing_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_outgoing_half_edge_ == other.idx_outgoing_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_outgoing_half_edge_ = mesh_->getNextHalfEdgeIndex (mesh_->getOppositeHalfEdgeIndex (idx_outgoing_half_edge_)); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_outgoing_half_edge_ = mesh_->getOppositeHalfEdgeIndex (mesh_->getPrevHalfEdgeIndex (idx_outgoing_half_edge_)); + return (*this); + } + + /** \brief Get the index to the target face. */ + inline FaceIndex + getTargetIndex () const + { + return (mesh_->getFaceIndex (idx_outgoing_half_edge_)); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_outgoing_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The outgoing half-edge of the vertex around which we want to circulate. */ + HalfEdgeIndex idx_outgoing_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// VertexAroundFaceCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates clockwise around a face and returns an index to the terminating vertex of the inner half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getVertexAroundFaceCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class VertexAroundFaceCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::VertexAroundFaceCirculator; + + using Mesh = MeshT; + using VertexIndex = typename Mesh::VertexIndex; + using FaceIndex = typename Mesh::FaceIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + VertexAroundFaceCirculator () + : mesh_ (nullptr), + idx_inner_half_edge_ () + { + } + + /** \brief Construct from the face around which we want to circulate. */ + VertexAroundFaceCirculator (const FaceIndex& idx_face, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face)) + { + } + + /** \brief Construct directly from the inner half-edge. */ + VertexAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (idx_inner_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_inner_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_inner_half_edge_ == other.idx_inner_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Get the index to the target vertex. */ + inline VertexIndex + getTargetIndex () const + { + return (mesh_->getTerminatingVertexIndex (idx_inner_half_edge_)); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_inner_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The inner half-edge of the face around which we want to circulate. */ + HalfEdgeIndex idx_inner_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// InnerHalfEdgeAroundFaceCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates clockwise around a face and returns an index to the inner half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getInnerHalfEdgeAroundFaceCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class InnerHalfEdgeAroundFaceCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::InnerHalfEdgeAroundFaceCirculator; + + using Mesh = MeshT; + using FaceIndex = typename Mesh::FaceIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + InnerHalfEdgeAroundFaceCirculator () + : mesh_ (nullptr), + idx_inner_half_edge_ () + { + } + + /** \brief Construct from the face around which we want to circulate. */ + InnerHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face)) + { + } + + /** \brief Construct directly from the inner half-edge. */ + InnerHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (idx_inner_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_inner_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_inner_half_edge_ == other.idx_inner_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Get the index to the inner half-edge. */ + inline HalfEdgeIndex + getTargetIndex () const + { + return (idx_inner_half_edge_); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_inner_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The inner half-edge of the face around which we want to circulate. */ + HalfEdgeIndex idx_inner_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// OuterHalfEdgeAroundFaceCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates clockwise around a face and returns an index to the outer half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getOuterHalfEdgeAroundFaceCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class OuterHalfEdgeAroundFaceCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::OuterHalfEdgeAroundFaceCirculator; + + using Mesh = MeshT; + using FaceIndex = typename Mesh::FaceIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + OuterHalfEdgeAroundFaceCirculator () + : mesh_ (nullptr), + idx_inner_half_edge_ () + { + } + + /** \brief Construct from the face around which we want to circulate. */ + OuterHalfEdgeAroundFaceCirculator (const FaceIndex& idx_face, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face)) + { + } + + /** \brief Construct directly from the inner half-edge. */ + OuterHalfEdgeAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (idx_inner_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_inner_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_inner_half_edge_ == other.idx_inner_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Get the index to the outer half-edge. */ + inline HalfEdgeIndex + getTargetIndex () const + { + return (mesh_->getOppositeHalfEdgeIndex (idx_inner_half_edge_)); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_inner_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The inner half-edge of the face around which we want to circulate. */ + HalfEdgeIndex idx_inner_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// FaceAroundFaceCirculator +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Circulates clockwise around a face and returns an index to the face of the outer half-edge (the target). The best way to declare the circulator is to use the method pcl::geometry::MeshBase::getFaceAroundFaceCirculator (). + * \tparam MeshT Mesh to which this circulator belongs to. + * \note The circulator can't be used to change the connectivity in the mesh (only const circulators are valid). + * \author Martin Saelzle + * \ingroup geometry + */ + template + class FaceAroundFaceCirculator + : boost::equality_comparable + , boost::unit_steppable + > > + { + public: + + using Base = boost::equality_comparable , + boost::unit_steppable > >; + using Self = pcl::geometry::FaceAroundFaceCirculator; + + using Mesh = MeshT; + using FaceIndex = typename Mesh::FaceIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + + /** \brief Constructor resulting in an invalid circulator. */ + FaceAroundFaceCirculator () + : mesh_ (nullptr), + idx_inner_half_edge_ () + { + } + + /** \brief Construct from the face around which we want to circulate. */ + FaceAroundFaceCirculator (const FaceIndex& idx_face, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (mesh->getInnerHalfEdgeIndex (idx_face)) + { + } + + /** \brief Construct directly from the inner half-edge. */ + FaceAroundFaceCirculator (const HalfEdgeIndex& idx_inner_half_edge, + Mesh*const mesh) + : mesh_ (mesh), + idx_inner_half_edge_ (idx_inner_half_edge) + { + } + + /** \brief Check if the circulator is valid. + * \warning Does NOT check if the stored mesh pointer is valid. You have to ensure this yourself when constructing the circulator. */ + inline bool + isValid () const + { + return (idx_inner_half_edge_.isValid ()); + } + + /** \brief Comparison operators (with boost::operators): == != + * \warning Does NOT check if the circulators belong to the same mesh. Please check this yourself. */ + inline bool + operator == (const Self& other) const + { + return (idx_inner_half_edge_ == other.idx_inner_half_edge_); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + idx_inner_half_edge_ = mesh_->getNextHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Decrement operators (with boost::operators): -- (pre and post) */ + inline Self& + operator -- () + { + idx_inner_half_edge_ = mesh_->getPrevHalfEdgeIndex (idx_inner_half_edge_); + return (*this); + } + + /** \brief Get the index to the target face. */ + inline FaceIndex + getTargetIndex () const + { + return (mesh_->getOppositeFaceIndex (idx_inner_half_edge_)); + } + + /** \brief Get the half-edge that is currently stored in the circulator. */ + inline HalfEdgeIndex + getCurrentHalfEdgeIndex () const + { + return (idx_inner_half_edge_); + } + + /** \brief The mesh to which this circulator belongs to. */ + Mesh* mesh_; + + /** \brief The inner half-edge of the face around which we want to circulate. */ + HalfEdgeIndex idx_inner_half_edge_; + }; + } // End namespace geometry +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/mesh_conversion.h b/deps_install/include/pcl-1.10/pcl/geometry/mesh_conversion.h new file mode 100755 index 0000000..c900709 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/mesh_conversion.h @@ -0,0 +1,130 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace geometry + { + /** \brief Convert a half-edge mesh to a face-vertex mesh. + * \param[in] half_edge_mesh The input mesh. + * \param[out] face_vertex_mesh The output mesh. + * \author Martin Saelzle + * \ingroup geometry + */ + template void + toFaceVertexMesh (const HalfEdgeMeshT& half_edge_mesh, pcl::PolygonMesh& face_vertex_mesh) + { + using HalfEdgeMesh = HalfEdgeMeshT; + using VAFC = typename HalfEdgeMesh::VertexAroundFaceCirculator; + using FaceIndex = typename HalfEdgeMesh::FaceIndex; + + pcl::Vertices polygon; + pcl::toPCLPointCloud2 (half_edge_mesh.getVertexDataCloud (), face_vertex_mesh.cloud); + + face_vertex_mesh.polygons.reserve (half_edge_mesh.sizeFaces ()); + for (std::size_t i=0; i int + toHalfEdgeMesh (const pcl::PolygonMesh& face_vertex_mesh, HalfEdgeMeshT& half_edge_mesh) + { + using HalfEdgeMesh = HalfEdgeMeshT; + using VertexDataCloud = typename HalfEdgeMesh::VertexDataCloud; + using VertexIndices = typename HalfEdgeMesh::VertexIndices; + + static_assert (HalfEdgeMesh::HasVertexData::value, "Output mesh must have data associated with the vertices!"); + + VertexDataCloud vertices; + pcl::fromPCLPointCloud2 (face_vertex_mesh.cloud, vertices); + + half_edge_mesh.reserveVertices (vertices.size ()); + half_edge_mesh.reserveEdges (3 * face_vertex_mesh.polygons.size ()); + half_edge_mesh.reserveFaces ( face_vertex_mesh.polygons.size ()); + + for (const auto &vertex : vertices) + { + half_edge_mesh.addVertex (vertex); + } + + assert (half_edge_mesh.sizeVertices () == vertices.size ()); + + int count_not_added = 0; + VertexIndices vi; + vi.reserve (3); // Minimum number (triangle) + for (const auto &polygon : face_vertex_mesh.polygons) + { + vi.clear (); + for (const unsigned int &vertex : polygon.vertices) + { + vi.emplace_back (vertex); + } + + if (!half_edge_mesh.addFace (vi).isValid ()) + { + ++count_not_added; + } + } + + return (count_not_added); + } + } // End namespace geometry +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/mesh_elements.h b/deps_install/include/pcl-1.10/pcl/geometry/mesh_elements.h new file mode 100755 index 0000000..284462e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/mesh_elements.h @@ -0,0 +1,187 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace geometry + { + template + class MeshBase; + + template + class MeshIO; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// Vertex +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief A vertex is a node in the mesh. + * \author Martin Saelzle + * \ingroup geometry + */ + class Vertex + { + private: + + using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex; + + /** \brief Constructor. + * \param[in] idx_outgoing_half_edge Index to the outgoing half-edge. Defaults to an invalid index. + */ + explicit Vertex (const HalfEdgeIndex& idx_outgoing_half_edge = HalfEdgeIndex ()) + : idx_outgoing_half_edge_ (idx_outgoing_half_edge) + {} + + /** \brief Index to the outgoing half-edge. The vertex is considered to be deleted if it stores an invalid outgoing half-edge index. */ + HalfEdgeIndex idx_outgoing_half_edge_; + + template + friend class pcl::geometry::MeshBase; + + template + friend class pcl::geometry::MeshIO; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// HalfEdge +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief An edge is a connection between two vertices. In a half-edge mesh the edge is split into two half-edges with opposite orientation. Each half-edge stores the index to the terminating vertex, the next half-edge, the previous half-edge and the face it belongs to. The opposite half-edge is accessed implicitly. + * \author Martin Saelzle + * \ingroup geometry + */ + class HalfEdge + { + private: + + using VertexIndex = pcl::geometry::VertexIndex; + using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex; + using FaceIndex = pcl::geometry::FaceIndex; + + /** \brief Constructor. + * \param[in] idx_terminating_vertex Index to the terminating vertex. Defaults to an invalid index. + * \param[in] idx_next_half_edge Index to the next half-edge. Defaults to an invalid index. + * \param[in] idx_prev_half_edge Index to the previous half-edge. Defaults to an invalid index. + * \param[in] idx_face Index to the face. Defaults to an invalid index. + */ + explicit HalfEdge (const VertexIndex& idx_terminating_vertex = VertexIndex (), + const HalfEdgeIndex& idx_next_half_edge = HalfEdgeIndex (), + const HalfEdgeIndex& idx_prev_half_edge = HalfEdgeIndex (), + const FaceIndex& idx_face = FaceIndex ()) + : idx_terminating_vertex_ (idx_terminating_vertex), + idx_next_half_edge_ (idx_next_half_edge), + idx_prev_half_edge_ (idx_prev_half_edge), + idx_face_ (idx_face) + { + } + + /** \brief Index to the terminating vertex. The half-edge is considered to be deleted if it stores an invalid terminating vertex index. */ + VertexIndex idx_terminating_vertex_; + + /** \brief Index to the next half-edge. */ + HalfEdgeIndex idx_next_half_edge_; + + /** \brief Index to the previous half-edge. */ + HalfEdgeIndex idx_prev_half_edge_; + + /** \brief Index to the face. The half-edge is considered to be on the boundary if it stores an invalid face index. */ + FaceIndex idx_face_; + + template + friend class pcl::geometry::MeshBase; + + template + friend class pcl::geometry::MeshIO; + }; + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// Face +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief A face is a closed loop of edges. + * \author Martin Saelzle + * \ingroup geometry + */ + class Face + { + private: + + using HalfEdgeIndex = pcl::geometry::HalfEdgeIndex; + + /** \brief Constructor. + * \param[in] inner_half_edge_idx Index to the outgoing half-edge. Defaults to an invalid index + */ + explicit Face (const HalfEdgeIndex& idx_inner_half_edge = HalfEdgeIndex ()) + : idx_inner_half_edge_ (idx_inner_half_edge) + {} + + /** \brief Index to the inner half-edge. The face is considered to be deleted if it stores an invalid inner half-edge index. */ + HalfEdgeIndex idx_inner_half_edge_; + + template + friend class pcl::geometry::MeshBase; + + template + friend class pcl::geometry::MeshIO; + }; + } // End namespace geometry +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/mesh_indices.h b/deps_install/include/pcl-1.10/pcl/geometry/mesh_indices.h new file mode 100755 index 0000000..8c5a10e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/mesh_indices.h @@ -0,0 +1,630 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +// NOTE: This file has been created with 'pcl_src/geometry/include/pcl/geometry/mesh_indices.py' + +#pragma once + +#include + +#include + +//////////////////////////////////////////////////////////////////////////////// +// VertexIndex +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods. + * \author Martin Saelzle + * \ingroup geometry + */ + class VertexIndex + : boost::totally_ordered <= >= == != + , boost::unit_steppable > > + { + public: + + using Base = boost::totally_ordered > >; + using Self = pcl::geometry::VertexIndex; + + /** \brief Constructor. Initializes with an invalid index. */ + VertexIndex () + : index_ (-1) + { + } + + /** \brief Constructor. + * \param[in] index The integer index. + */ + explicit VertexIndex (const int index) + : index_ (index) + { + } + + /** \brief Returns true if the index is valid. */ + inline bool + isValid () const + { + return (index_ >= 0); + } + + /** \brief Invalidate the index. */ + inline void + invalidate () + { + index_ = -1; + } + + /** \brief Get the index. */ + inline int + get () const + { + return (index_); + } + + /** \brief Set the index. */ + inline void + set (const int index) + { + index_ = index; + } + + /** \brief Comparison operators (with boost::operators): < > <= >= */ + inline bool + operator < (const Self& other) const + { + return (this->get () < other.get ()); + } + + /** \brief Comparison operators (with boost::operators): == != */ + inline bool + operator == (const Self& other) const + { + return (this->get () == other.get ()); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + ++index_; + return (*this); + } + + /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */ + inline Self& + operator -- () + { + --index_; + return (*this); + } + + /** \brief Addition operators (with boost::operators): + += */ + inline Self& + operator += (const Self& other) + { + index_ += other.get (); + return (*this); + } + + /** \brief Subtraction operators (with boost::operators): - -= */ + inline Self& + operator -= (const Self& other) + { + index_ -= other.get (); + return (*this); + } + + private: + + /** \brief Stored index. */ + int index_; + + friend std::istream& + operator >> (std::istream& is, pcl::geometry::VertexIndex& index); + }; + + /** \brief ostream operator. */ + inline std::ostream& + operator << (std::ostream& os, const pcl::geometry::VertexIndex& index) + { + return (os << index.get ()); + } + + /** \brief istream operator. */ + inline std::istream& + operator >> (std::istream& is, pcl::geometry::VertexIndex& index) + { + return (is >> index.index_); + } + + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// HalfEdgeIndex +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods. + * \author Martin Saelzle + * \ingroup geometry + */ + class HalfEdgeIndex + : boost::totally_ordered <= >= == != + , boost::unit_steppable > > + { + public: + + using Base = boost::totally_ordered > >; + using Self = pcl::geometry::HalfEdgeIndex; + + /** \brief Constructor. Initializes with an invalid index. */ + HalfEdgeIndex () + : index_ (-1) + { + } + + /** \brief Constructor. + * \param[in] index The integer index. + */ + explicit HalfEdgeIndex (const int index) + : index_ (index) + { + } + + /** \brief Returns true if the index is valid. */ + inline bool + isValid () const + { + return (index_ >= 0); + } + + /** \brief Invalidate the index. */ + inline void + invalidate () + { + index_ = -1; + } + + /** \brief Get the index. */ + inline int + get () const + { + return (index_); + } + + /** \brief Set the index. */ + inline void + set (const int index) + { + index_ = index; + } + + /** \brief Comparison operators (with boost::operators): < > <= >= */ + inline bool + operator < (const Self& other) const + { + return (this->get () < other.get ()); + } + + /** \brief Comparison operators (with boost::operators): == != */ + inline bool + operator == (const Self& other) const + { + return (this->get () == other.get ()); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + ++index_; + return (*this); + } + + /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */ + inline Self& + operator -- () + { + --index_; + return (*this); + } + + /** \brief Addition operators (with boost::operators): + += */ + inline Self& + operator += (const Self& other) + { + index_ += other.get (); + return (*this); + } + + /** \brief Subtraction operators (with boost::operators): - -= */ + inline Self& + operator -= (const Self& other) + { + index_ -= other.get (); + return (*this); + } + + private: + + /** \brief Stored index. */ + int index_; + + friend std::istream& + operator >> (std::istream& is, pcl::geometry::HalfEdgeIndex& index); + }; + + /** \brief ostream operator. */ + inline std::ostream& + operator << (std::ostream& os, const pcl::geometry::HalfEdgeIndex& index) + { + return (os << index.get ()); + } + + /** \brief istream operator. */ + inline std::istream& + operator >> (std::istream& is, pcl::geometry::HalfEdgeIndex& index) + { + return (is >> index.index_); + } + + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// EdgeIndex +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods. + * \author Martin Saelzle + * \ingroup geometry + */ + class EdgeIndex + : boost::totally_ordered <= >= == != + , boost::unit_steppable > > + { + public: + + using Base = boost::totally_ordered > >; + using Self = pcl::geometry::EdgeIndex; + + /** \brief Constructor. Initializes with an invalid index. */ + EdgeIndex () + : index_ (-1) + { + } + + /** \brief Constructor. + * \param[in] index The integer index. + */ + explicit EdgeIndex (const int index) + : index_ (index) + { + } + + /** \brief Returns true if the index is valid. */ + inline bool + isValid () const + { + return (index_ >= 0); + } + + /** \brief Invalidate the index. */ + inline void + invalidate () + { + index_ = -1; + } + + /** \brief Get the index. */ + inline int + get () const + { + return (index_); + } + + /** \brief Set the index. */ + inline void + set (const int index) + { + index_ = index; + } + + /** \brief Comparison operators (with boost::operators): < > <= >= */ + inline bool + operator < (const Self& other) const + { + return (this->get () < other.get ()); + } + + /** \brief Comparison operators (with boost::operators): == != */ + inline bool + operator == (const Self& other) const + { + return (this->get () == other.get ()); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + ++index_; + return (*this); + } + + /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */ + inline Self& + operator -- () + { + --index_; + return (*this); + } + + /** \brief Addition operators (with boost::operators): + += */ + inline Self& + operator += (const Self& other) + { + index_ += other.get (); + return (*this); + } + + /** \brief Subtraction operators (with boost::operators): - -= */ + inline Self& + operator -= (const Self& other) + { + index_ -= other.get (); + return (*this); + } + + private: + + /** \brief Stored index. */ + int index_; + + friend std::istream& + operator >> (std::istream& is, pcl::geometry::EdgeIndex& index); + }; + + /** \brief ostream operator. */ + inline std::ostream& + operator << (std::ostream& os, const pcl::geometry::EdgeIndex& index) + { + return (os << index.get ()); + } + + /** \brief istream operator. */ + inline std::istream& + operator >> (std::istream& is, pcl::geometry::EdgeIndex& index) + { + return (is >> index.index_); + } + + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// FaceIndex +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Index used to access elements in the half-edge mesh. It is basically just a wrapper around an integer with a few added methods. + * \author Martin Saelzle + * \ingroup geometry + */ + class FaceIndex + : boost::totally_ordered <= >= == != + , boost::unit_steppable > > + { + public: + + using Base = boost::totally_ordered > >; + using Self = pcl::geometry::FaceIndex; + + /** \brief Constructor. Initializes with an invalid index. */ + FaceIndex () + : index_ (-1) + { + } + + /** \brief Constructor. + * \param[in] index The integer index. + */ + explicit FaceIndex (const int index) + : index_ (index) + { + } + + /** \brief Returns true if the index is valid. */ + inline bool + isValid () const + { + return (index_ >= 0); + } + + /** \brief Invalidate the index. */ + inline void + invalidate () + { + index_ = -1; + } + + /** \brief Get the index. */ + inline int + get () const + { + return (index_); + } + + /** \brief Set the index. */ + inline void + set (const int index) + { + index_ = index; + } + + /** \brief Comparison operators (with boost::operators): < > <= >= */ + inline bool + operator < (const Self& other) const + { + return (this->get () < other.get ()); + } + + /** \brief Comparison operators (with boost::operators): == != */ + inline bool + operator == (const Self& other) const + { + return (this->get () == other.get ()); + } + + /** \brief Increment operators (with boost::operators): ++ (pre and post) */ + inline Self& + operator ++ () + { + ++index_; + return (*this); + } + + /** \brief Decrement operators (with boost::operators): \-\- (pre and post) */ + inline Self& + operator -- () + { + --index_; + return (*this); + } + + /** \brief Addition operators (with boost::operators): + += */ + inline Self& + operator += (const Self& other) + { + index_ += other.get (); + return (*this); + } + + /** \brief Subtraction operators (with boost::operators): - -= */ + inline Self& + operator -= (const Self& other) + { + index_ -= other.get (); + return (*this); + } + + private: + + /** \brief Stored index. */ + int index_; + + friend std::istream& + operator >> (std::istream& is, pcl::geometry::FaceIndex& index); + }; + + /** \brief ostream operator. */ + inline std::ostream& + operator << (std::ostream& os, const pcl::geometry::FaceIndex& index) + { + return (os << index.get ()); + } + + /** \brief istream operator. */ + inline std::istream& + operator >> (std::istream& is, pcl::geometry::FaceIndex& index) + { + return (is >> index.index_); + } + + } // End namespace geometry +} // End namespace pcl + +//////////////////////////////////////////////////////////////////////////////// +// Conversions +//////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + namespace geometry + { + /** \brief Convert the given half-edge index to an edge index. */ + inline pcl::geometry::EdgeIndex + toEdgeIndex (const HalfEdgeIndex& index) + { + return (index.isValid () ? EdgeIndex (index.get () / 2) : EdgeIndex ()); + } + + /** \brief Convert the given edge index to a half-edge index. + * \param index + * \param[in] get_first The first half-edge of the edge is returned if this variable is true; elsewise the second. + */ + inline pcl::geometry::HalfEdgeIndex + toHalfEdgeIndex (const EdgeIndex& index, const bool get_first=true) + { + return (index.isValid () ? HalfEdgeIndex (index.get () * 2 + static_cast (!get_first)) : HalfEdgeIndex ()); + } + } // End namespace geometry +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/mesh_io.h b/deps_install/include/pcl-1.10/pcl/geometry/mesh_io.h new file mode 100755 index 0000000..169b1fc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/mesh_io.h @@ -0,0 +1,262 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace geometry + { + /** \brief Read / write the half-edge mesh from / to a file. + * \tparam MeshT e.g. pcl::geometry::TriangleMesh or pcl::geometry::PolygonMesh + * \author Martin Saelzle + * \ingroup geometry + * \todo + * - Only writes the topology (not the mesh data). + * - Supports only ascii. + * - Does not consider the mesh traits (e.g. manifold or not) + */ + template + class MeshIO + { + public: + + using Mesh = MeshT; + + using Vertex = typename Mesh::Vertex; + using HalfEdge = typename Mesh::HalfEdge; + using Face = typename Mesh::Face; + + using Vertices = typename Mesh::Vertices; + using HalfEdges = typename Mesh::HalfEdges; + using Faces = typename Mesh::Faces; + + using VertexIndex = typename Mesh::VertexIndex; + using HalfEdgeIndex = typename Mesh::HalfEdgeIndex; + using FaceIndex = typename Mesh::FaceIndex; + + /** \brief Constructor. */ + MeshIO () + { + } + + /** \brief Read the mesh from a file with the given filename. + * \param[in] filename Path to the file. + * \param[out] mesh The loaded mesh. + * \return true if success. + */ + bool + read (const std::string& filename, Mesh& mesh) const + { + std::ifstream file (filename.c_str ()); + + if (!file.is_open ()) + { + std::cerr << "Error in MeshIO::read: Could not open the file '" << filename << "'\n"; + return (false); + } + + // Read the header + std::string line; + unsigned int line_number = 1; + int n_v = -1, n_he = -1, n_f = -1; + + if (!std::getline (file, line) || line != "PCL half-edge mesh") + { + std::cerr << "Error loading '" << filename << "' (line " << line_number << "): Wrong file format.\n"; + return (false); + } + ++line_number; + + if (!std::getline (file, line)) + { + std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Number of vertices / half-edges / faces not found.\n"; + return (false); + } + { + std::istringstream iss (line); + if (!(iss >> n_v >> n_he >> n_f) || iss.good ()) // Don't allow more than 3 en + { + std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the number of vertices / half-edges / faces.\n"; + return (false); + } + } + if (n_v < 0 || n_he < 0 || n_f < 0) + { + std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Invalid number of vertices / half-edges / faces.\n"; + return (false); + } + ++line_number; + + // Read the vertices. + { + mesh.vertices_.reserve (n_v); + HalfEdgeIndex idx_ohe; // Outgoing half-edge; + + for (int i=0; i> idx_ohe) || iss.good ()) + { + std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the vertex.\n"; + return (false); + } + mesh.vertices_.push_back (Vertex (idx_ohe)); + } + } + + // Read the half-edges. + { + mesh.half_edges_.reserve (n_he); + VertexIndex idx_tv; // Terminating vertex. + HalfEdgeIndex idx_nhe; // Next half-edge; + HalfEdgeIndex idx_phe; // Previous half-edge. + FaceIndex idx_f; // Face. + + for (int i=0; i> idx_tv >> idx_nhe >> idx_phe >> idx_f) || iss.good ()) + { + std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the half-edge.\n"; + return (false); + } + mesh.half_edges_.push_back (HalfEdge (idx_tv, idx_nhe, idx_phe, idx_f)); + } + } + + // Read the faces. + { + mesh.faces_.reserve (n_f); + HalfEdgeIndex idx_ihe; // Inner half-edge. + + for (int i=0; i> idx_ihe) || iss.good ()) + { + std::cerr << "Error loading '" << filename << "'' (line " << line_number << "): Could not read the face.\n"; + return (false); + } + mesh.faces_.push_back (Face (idx_ihe)); + } + } + + // Set the data + if (Mesh::HasVertexData::value) mesh.vertex_data_cloud_. resize (n_v); + if (Mesh::HasHalfEdgeData::value) mesh.half_edge_data_cloud_.resize (n_he); + if (Mesh::HasEdgeData::value) mesh.edge_data_cloud_. resize (n_he / 2); + if (Mesh::HasFaceData::value) mesh.face_data_cloud_. resize (n_f); + + return (true); + } + + /** \brief Write the mesh to a file with the given filename. + * \param[in] filename Path to the file. + * \param[in] mesh The saved mesh. + * \return true if success + */ + bool + write (const std::string& filename, const Mesh& mesh) const + { + std::ofstream file (filename.c_str ()); + + // Write the header + if (!file.is_open ()) + { + std::cerr << "Error in MeshIO::write: Could not open the file '" << filename << "'\n"; + return (false); + } + + file << "PCL half-edge mesh\n"; + file << mesh.sizeVertices () << " " + << mesh.sizeHalfEdges () << " " + << mesh.sizeFaces () << "\n"; + + // Write the vertices + for (typename Vertices::const_iterator it=mesh.vertices_.begin (); it!=mesh.vertices_.end (); ++it) + { + file << it->idx_outgoing_half_edge_ << "\n"; + } + + // Write the half-edges + for (typename HalfEdges::const_iterator it=mesh.half_edges_.begin (); it!=mesh.half_edges_.end (); ++it) + { + file << it->idx_terminating_vertex_ << " " + << it->idx_next_half_edge_ << " " + << it->idx_prev_half_edge_ << " " + << it->idx_face_ << "\n"; + } + + // Write the faces + for (typename Faces::const_iterator it=mesh.faces_.begin (); it!=mesh.faces_.end (); ++it) + { + file << it->idx_inner_half_edge_ << "\n"; + } + + return (true); + } + }; + + } // End namespace geometry +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/mesh_traits.h b/deps_install/include/pcl-1.10/pcl/geometry/mesh_traits.h new file mode 100755 index 0000000..d088d90 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/mesh_traits.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace geometry + { + /** \brief No data is associated with the vertices / half-edges / edges / faces. */ + struct NoData {}; + + /** \brief The mesh traits are used to set up compile time settings for the mesh. + * \tparam VertexDataT Data stored for each vertex. Defaults to pcl::NoData. + * \tparam HalfEdgeDataT Data stored for each half-edge. Defaults to pcl::NoData. + * \tparam EdgeDataT Data stored for each edge. Defaults to pcl::NoData. + * \tparam FaceDataT Data stored for each face. Defaults to pcl::NoData. + * \author Martin Saelzle + * \ingroup geometry + */ + template + struct DefaultMeshTraits + { + using VertexData = VertexDataT; + using HalfEdgeData = HalfEdgeDataT; + using EdgeData = EdgeDataT; + using FaceData = FaceDataT; + + /** \brief Specifies whether the mesh is manifold or not (only non-manifold vertices can be represented). */ + using IsManifold = std::false_type; + }; + } // End namespace geometry +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/organized_index_iterator.h b/deps_install/include/pcl-1.10/pcl/geometry/organized_index_iterator.h new file mode 100755 index 0000000..2047bb3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/organized_index_iterator.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +namespace pcl +{ +/** \brief base class for iterators on 2-dimensional maps like images/organized clouds etc. + * \author Suat Gedikli + * \ingroup geometry + */ +class OrganizedIndexIterator +{ + public: + /** \brief constructor + * \param[in] width the width of the image/organized cloud + */ + OrganizedIndexIterator (unsigned width); + + /** \brief virtual destructor*/ + virtual ~OrganizedIndexIterator (); + + /** \brief go to next pixel/point in image/cloud*/ + virtual void operator ++ () = 0; + + /** \brief go to next pixel/point in image/cloud*/ + virtual void operator ++ (int); + + /** \brief returns the pixel/point index in the linearized memory of the image/cloud + * \return the pixel/point index in the linearized memory of the image/cloud + */ + unsigned operator* () const; + + /** \brief returns the pixel/point index in the linearized memory of the image/cloud + * \return the pixel/point index in the linearized memory of the image/cloud + */ + virtual unsigned getIndex () const; + + /** \brief returns the row index (y-coordinate) of the current pixel/point + * \return the row index (y-coordinate) of the current pixel/point + */ + virtual unsigned getRowIndex () const; + + /** \brief returns the col index (x-coordinate) of the current pixel/point + * \return the col index (x-coordinate) of the current pixel/point + */ + virtual unsigned getColumnIndex () const; + + /** \brief return whether the current visited pixel/point is valid or not. + * \return true if the current pixel/point is within the points to be iterated over, false otherwise + */ + virtual bool isValid () const = 0; + + /** \brief resets the iterator to the beginning of the line + */ + virtual void reset () = 0; + + protected: + /** \brief the width of the image/cloud*/ + unsigned width_; + + /** \brief the index of the current pixel/point*/ + unsigned index_; +}; + +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +inline OrganizedIndexIterator::OrganizedIndexIterator (unsigned width) +: width_ (width) +, index_ (0) +{ +} + +//////////////////////////////////////////////////////////////////////////////// +inline OrganizedIndexIterator::~OrganizedIndexIterator () +{ +} + +//////////////////////////////////////////////////////////////////////////////// +inline void +OrganizedIndexIterator::operator++ (int) +{ + return operator ++(); +} + +//////////////////////////////////////////////////////////////////////////////// +inline unsigned +pcl::OrganizedIndexIterator::operator * () const +{ + return index_; +} + +//////////////////////////////////////////////////////////////////////////////// +inline unsigned +pcl::OrganizedIndexIterator::getIndex () const +{ + return index_; +} + +//////////////////////////////////////////////////////////////////////////////// +/** \brief default implementation. Should be overloaded + */ +inline unsigned +pcl::OrganizedIndexIterator::getRowIndex () const +{ + return index_ / width_; +} + +//////////////////////////////////////////////////////////////////////////////// +inline unsigned +pcl::OrganizedIndexIterator::getColumnIndex () const +{ + return index_ % width_; +} +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/planar_polygon.h b/deps_install/include/pcl-1.10/pcl/geometry/planar_polygon.h new file mode 100755 index 0000000..bd9f56f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/planar_polygon.h @@ -0,0 +1,141 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: planar_polygon.h 4696 2012-02-23 06:12:55Z rusu $ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space. + * \author Alex Trevor + */ + template + class PlanarPolygon + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor for PlanarPolygon */ + PlanarPolygon () : contour_ () + {} + + /** \brief Constructor for PlanarPolygon + * \param[in] contour a vector of points bounding the polygon + * \param[in] coefficients a vector of the plane's coefficients (a,b,c,d) + */ + PlanarPolygon (typename pcl::PointCloud::VectorType &contour, + Eigen::Vector4f& coefficients) + : contour_ (contour), coefficients_ (coefficients) + {} + + /** \brief Destructor. */ + virtual ~PlanarPolygon () {} + + /** \brief Set the internal contour + * \param[in] contour the new planar polygonal contour + */ + void + setContour (const pcl::PointCloud &contour) + { + contour_ = contour.points; + } + + /** \brief Getter for the contour / boundary */ + typename pcl::PointCloud::VectorType& + getContour () + { + return (contour_); + } + + /** \brief Getter for the contour / boundary */ + const typename pcl::PointCloud::VectorType& + getContour () const + { + return (contour_); + } + + /** \brief Setr the internal coefficients + * \param[in] coefficients the new coefficients to be set + */ + void + setCoefficients (const Eigen::Vector4f &coefficients) + { + coefficients_ = coefficients; + } + + /** \brief Set the internal coefficients + * \param[in] coefficients the new coefficients to be set + */ + void + setCoefficients (const pcl::ModelCoefficients &coefficients) + { + for (int i = 0; i < 4; i++) + coefficients_[i] = coefficients.values.at (i); + } + + /** \brief Getter for the coefficients */ + Eigen::Vector4f& + getCoefficients () + { + return (coefficients_); + } + + /** \brief Getter for the coefficients */ + const Eigen::Vector4f& + getCoefficients () const + { + return (coefficients_); + } + + protected: + /** \brief A list of points on the boundary/contour of the planar region. */ + typename pcl::PointCloud::VectorType contour_; + + /** \brief A list of model coefficients (a,b,c,d). */ + Eigen::Vector4f coefficients_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/geometry/polygon_mesh.h b/deps_install/include/pcl-1.10/pcl/geometry/polygon_mesh.h new file mode 100755 index 0000000..3c74323 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/polygon_mesh.h @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace geometry + { + /** \brief Tag describing the type of the mesh. */ + struct PolygonMeshTag {}; + + /** \brief General half-edge mesh that can store any polygon with a minimum number of vertices of 3. + * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits. + * \author Martin Saelzle + * \ingroup geometry + */ + template + class PolygonMesh : public pcl::geometry::MeshBase , MeshTraitsT, PolygonMeshTag> + { + public: + + using Base = pcl::geometry::MeshBase , MeshTraitsT, PolygonMeshTag>; + + using Self = PolygonMesh; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using VertexData = typename Base::VertexData; + using HalfEdgeData = typename Base::HalfEdgeData; + using EdgeData = typename Base::EdgeData; + using FaceData = typename Base::FaceData; + using IsManifold = typename Base::IsManifold; + using MeshTag = typename Base::MeshTag; + + using HasVertexData = typename Base::HasVertexData; + using HasHalfEdgeData = typename Base::HasHalfEdgeData; + using HasEdgeData = typename Base::HasEdgeData; + using HasFaceData = typename Base::HasFaceData; + + using VertexDataCloud = typename Base::VertexDataCloud; + using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud; + using EdgeDataCloud = typename Base::EdgeDataCloud; + using FaceDataCloud = typename Base::FaceDataCloud; + + // Indices + using VertexIndex = typename Base::VertexIndex; + using HalfEdgeIndex = typename Base::HalfEdgeIndex; + using EdgeIndex = typename Base::EdgeIndex; + using FaceIndex = typename Base::FaceIndex; + + using VertexIndices = typename Base::VertexIndices; + using HalfEdgeIndices = typename Base::HalfEdgeIndices; + using EdgeIndices = typename Base::EdgeIndices; + using FaceIndices = typename Base::FaceIndices; + + // Circulators + using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator; + using OutgoingHalfEdgeAroundVertexCirculator = typename Base::OutgoingHalfEdgeAroundVertexCirculator; + using IncomingHalfEdgeAroundVertexCirculator = typename Base::IncomingHalfEdgeAroundVertexCirculator; + using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator; + using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator; + using InnerHalfEdgeAroundFaceCirculator = typename Base::InnerHalfEdgeAroundFaceCirculator; + using OuterHalfEdgeAroundFaceCirculator = typename Base::OuterHalfEdgeAroundFaceCirculator; + using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator; + + /** \brief Constructor. */ + PolygonMesh () + : Base (), + add_triangle_ (3), + add_quad_ (4) + { + } + + /** \brief The base method of addFace is hidden because of the overloads in this class. */ + using Base::addFace; + + /** \brief Add a triangle to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one. + * \param[in] idx_v_0 Index to the first vertex. + * \param[in] idx_v_1 Index to the second vertex. + * \param[in] idx_v_2 Index to the third vertex. + * \param[in] face_data Data that is set for the face. + * \param[in] half_edge_data Data that is set for all added half-edges. + * \param[in] edge_data Data that is set for all added edges. + * \return Index to the new face. Failure is signaled by returning an invalid face index. + * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior! + */ + inline FaceIndex + addFace (const VertexIndex& idx_v_0, + const VertexIndex& idx_v_1, + const VertexIndex& idx_v_2, + const FaceData& face_data = FaceData (), + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData& half_edge_data = HalfEdgeData ()) + { + add_triangle_ [0] = idx_v_0; + add_triangle_ [1] = idx_v_1; + add_triangle_ [2] = idx_v_2; + + return (this->addFaceImplBase (add_triangle_, face_data, edge_data, half_edge_data)); + } + + /** \brief Add a quad to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one. + * \param[in] idx_v_0 Index to the first vertex. + * \param[in] idx_v_1 Index to the second vertex. + * \param[in] idx_v_2 Index to the third vertex. + * \param[in] idx_v_3 Index to the fourth vertex. + * \param[in] face_data Data that is set for the face. + * \param[in] half_edge_data Data that is set for all added half-edges. + * \param[in] edge_data Data that is set for all added edges. + * \return Index to the new face. Failure is signaled by returning an invalid face index. + * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior! + */ + inline FaceIndex + addFace (const VertexIndex& idx_v_0, + const VertexIndex& idx_v_1, + const VertexIndex& idx_v_2, + const VertexIndex& idx_v_3, + const FaceData& face_data = FaceData (), + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData& half_edge_data = HalfEdgeData ()) + { + add_quad_ [0] = idx_v_0; + add_quad_ [1] = idx_v_1; + add_quad_ [2] = idx_v_2; + add_quad_ [3] = idx_v_3; + + return (this->addFaceImplBase (add_quad_, face_data, edge_data, half_edge_data)); + } + + private: + + // NOTE: Can't use the typedef of Base as a friend. + friend class pcl::geometry::MeshBase , MeshTraitsT, pcl::geometry::PolygonMeshTag>; + + /** \brief addFace for the polygon mesh. */ + inline FaceIndex + addFaceImpl (const VertexIndices& vertices, + const FaceData& face_data, + const EdgeData& edge_data, + const HalfEdgeData& half_edge_data) + { + return (this->addFaceImplBase (vertices, face_data, edge_data, half_edge_data)); + } + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + /** \brief Storage for adding a triangle. */ + VertexIndices add_triangle_; + + /** \brief Storage for adding a quad. */ + VertexIndices add_quad_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } // End namespace geom +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/polygon_operations.h b/deps_install/include/pcl-1.10/pcl/geometry/polygon_operations.h new file mode 100755 index 0000000..f43540c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/polygon_operations.h @@ -0,0 +1,70 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: point_operators.h 4389 2012-02-10 10:10:26Z nizar $ + * + */ + +#pragma once + +#include "planar_polygon.h" +#include + +namespace pcl +{ + /** \brief see approximatePolygon2D + * \author Suat Gedikli + */ + template + void approximatePolygon (const PlanarPolygon& polygon, PlanarPolygon& approx_polygon, float threshold, bool refine = false, bool closed = true); + + /** \brief returns an approximate polygon to given 2D contour. Uses just X and Y values. + * \note if refinement is not turned on, the resulting polygon will contain points from the original contour with their original z values (if any) + * \note if refinement is turned on, the z values of the refined polygon are not valid and should be set to 0 if point contains z attribute. + * \param [in] polygon input polygon + * \param [out] approx_polygon approximate polygon + * \param [in] threshold maximum allowed distance of an input vertex to an output edge + * \param refine + * \param [in] closed whether it is a closed polygon or a polyline + * \author Suat Gedikli + */ + template + void approximatePolygon2D (const typename PointCloud::VectorType &polygon, + typename PointCloud::VectorType &approx_polygon, + float threshold, bool refine = false, bool closed = true); + +} // namespace pcl + +#include "impl/polygon_operations.hpp" diff --git a/deps_install/include/pcl-1.10/pcl/geometry/quad_mesh.h b/deps_install/include/pcl-1.10/pcl/geometry/quad_mesh.h new file mode 100755 index 0000000..76dd113 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/quad_mesh.h @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace geometry + { + /** \brief Tag describing the type of the mesh. */ + struct QuadMeshTag {}; + + /** \brief Half-edge mesh that can only store quads. + * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits. + * \author Martin Saelzle + * \ingroup geometry + */ + template + class QuadMesh : public pcl::geometry::MeshBase , MeshTraitsT, QuadMeshTag> + { + public: + + using Base = pcl::geometry::MeshBase , MeshTraitsT, QuadMeshTag>; + + using Self = QuadMesh; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using VertexData = typename Base::VertexData; + using HalfEdgeData = typename Base::HalfEdgeData; + using EdgeData = typename Base::EdgeData; + using FaceData = typename Base::FaceData; + using IsManifold = typename Base::IsManifold; + using MeshTag = typename Base::MeshTag; + + using HasVertexData = typename Base::HasVertexData; + using HasHalfEdgeData = typename Base::HasHalfEdgeData; + using HasEdgeData = typename Base::HasEdgeData; + using HasFaceData = typename Base::HasFaceData; + + using VertexDataCloud = typename Base::VertexDataCloud; + using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud; + using EdgeDataCloud = typename Base::EdgeDataCloud; + using FaceDataCloud = typename Base::FaceDataCloud; + + // Indices + using VertexIndex = typename Base::VertexIndex; + using HalfEdgeIndex = typename Base::HalfEdgeIndex; + using EdgeIndex = typename Base::EdgeIndex; + using FaceIndex = typename Base::FaceIndex; + + using VertexIndices = typename Base::VertexIndices; + using HalfEdgeIndices = typename Base::HalfEdgeIndices; + using EdgeIndices = typename Base::EdgeIndices; + using FaceIndices = typename Base::FaceIndices; + + // Circulators + using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator; + using OutgoingHalfEdgeAroundVertexCirculator = typename Base::OutgoingHalfEdgeAroundVertexCirculator; + using IncomingHalfEdgeAroundVertexCirculator = typename Base::IncomingHalfEdgeAroundVertexCirculator; + using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator; + using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator; + using InnerHalfEdgeAroundFaceCirculator = typename Base::InnerHalfEdgeAroundFaceCirculator; + using OuterHalfEdgeAroundFaceCirculator = typename Base::OuterHalfEdgeAroundFaceCirculator; + using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator; + + /** \brief Constructor. */ + QuadMesh () + : Base (), + add_quad_ (4) + { + } + + /** \brief The base method of addFace is hidden because of the overloads in this class. */ + using Base::addFace; + + /** \brief Add a quad to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one. + * \param[in] idx_v_0 Index to the first vertex. + * \param[in] idx_v_1 Index to the second vertex. + * \param[in] idx_v_2 Index to the third vertex. + * \param[in] idx_v_3 Index to the fourth vertex. + * \param[in] face_data Data that is set for the face. + * \param[in] half_edge_data Data that is set for all added half-edges. + * \param[in] edge_data Data that is set for all added edges. + * \return Index to the new face. Failure is signaled by returning an invalid face index. + * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior! + */ + inline FaceIndex + addFace (const VertexIndex& idx_v_0, + const VertexIndex& idx_v_1, + const VertexIndex& idx_v_2, + const VertexIndex& idx_v_3, + const FaceData& face_data = FaceData (), + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData& half_edge_data = HalfEdgeData ()) + { + add_quad_ [0] = idx_v_0; + add_quad_ [1] = idx_v_1; + add_quad_ [2] = idx_v_2; + add_quad_ [3] = idx_v_3; + + return (this->addFaceImplBase (add_quad_, face_data, edge_data, half_edge_data)); + } + + private: + + // NOTE: Can't use the typedef of Base as a friend. + friend class pcl::geometry::MeshBase , MeshTraitsT, pcl::geometry::QuadMeshTag>; + + /** \brief addFace for the quad mesh. */ + inline FaceIndex + addFaceImpl (const VertexIndices& vertices, + const FaceData& face_data, + const EdgeData& edge_data, + const HalfEdgeData& half_edge_data) + { + if (vertices.size () == 4) + return (this->addFaceImplBase (vertices, face_data, edge_data, half_edge_data)); + return (FaceIndex ()); + } + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + /** \brief Storage for adding a quad. */ + VertexIndices add_quad_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } // End namespace geom +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/geometry/triangle_mesh.h b/deps_install/include/pcl-1.10/pcl/geometry/triangle_mesh.h new file mode 100755 index 0000000..ceb6257 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/geometry/triangle_mesh.h @@ -0,0 +1,351 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl +{ + namespace geometry + { + /** \brief Tag describing the type of the mesh. */ + struct TriangleMeshTag {}; + + /** \brief Half-edge mesh that can only store triangles. + * \tparam MeshTraitsT Please have a look at pcl::geometry::DefaultMeshTraits. + * \author Martin Saelzle + * \ingroup geometry + */ + template + class TriangleMesh : public pcl::geometry::MeshBase , MeshTraitsT, TriangleMeshTag> + { + public: + + using Base = pcl::geometry::MeshBase , MeshTraitsT, TriangleMeshTag>; + + using Self = TriangleMesh; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using VertexData = typename Base::VertexData; + using HalfEdgeData = typename Base::HalfEdgeData; + using EdgeData = typename Base::EdgeData; + using FaceData = typename Base::FaceData; + using IsManifold = typename Base::IsManifold; + using MeshTag = typename Base::MeshTag; + + using HasVertexData = typename Base::HasVertexData; + using HasHalfEdgeData = typename Base::HasHalfEdgeData; + using HasEdgeData = typename Base::HasEdgeData; + using HasFaceData = typename Base::HasFaceData; + + using VertexDataCloud = typename Base::VertexDataCloud; + using HalfEdgeDataCloud = typename Base::HalfEdgeDataCloud; + using EdgeDataCloud = typename Base::EdgeDataCloud; + using FaceDataCloud = typename Base::FaceDataCloud; + + // Indices + using VertexIndex = typename Base::VertexIndex; + using HalfEdgeIndex = typename Base::HalfEdgeIndex; + using EdgeIndex = typename Base::EdgeIndex; + using FaceIndex = typename Base::FaceIndex; + using FaceIndexPair = std::pair ; + + using VertexIndices = typename Base::VertexIndices; + using HalfEdgeIndices = typename Base::HalfEdgeIndices; + using EdgeIndices = typename Base::EdgeIndices; + using FaceIndices = typename Base::FaceIndices; + + // Circulators + using VertexAroundVertexCirculator = typename Base::VertexAroundVertexCirculator; + using OutgoingHalfEdgeAroundVertexCirculator = typename Base::OutgoingHalfEdgeAroundVertexCirculator; + using IncomingHalfEdgeAroundVertexCirculator = typename Base::IncomingHalfEdgeAroundVertexCirculator; + using FaceAroundVertexCirculator = typename Base::FaceAroundVertexCirculator; + using VertexAroundFaceCirculator = typename Base::VertexAroundFaceCirculator; + using InnerHalfEdgeAroundFaceCirculator = typename Base::InnerHalfEdgeAroundFaceCirculator; + using OuterHalfEdgeAroundFaceCirculator = typename Base::OuterHalfEdgeAroundFaceCirculator; + using FaceAroundFaceCirculator = typename Base::FaceAroundFaceCirculator; + + /** \brief Constructor. */ + TriangleMesh () + : Base (), + add_triangle_ (3), + inner_he_atp_ (4), + is_new_atp_ (4) + { + } + + /** \brief The base method of addFace is hidden because of the overloads in this class. */ + using Base::addFace; + + /** \brief Add a triangle to the mesh. Data is only added if it is associated with the elements. The last vertex is connected with the first one. + * \param[in] idx_v_0 Index to the first vertex. + * \param[in] idx_v_1 Index to the second vertex. + * \param[in] idx_v_2 Index to the third vertex. + * \param[in] face_data Data that is set for the face. + * \param[in] half_edge_data Data that is set for all added half-edges. + * \param[in] edge_data Data that is set for all added edges. + * \return Index to the new face. Failure is signaled by returning an invalid face index. + * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior! + */ + inline FaceIndex + addFace (const VertexIndex& idx_v_0, + const VertexIndex& idx_v_1, + const VertexIndex& idx_v_2, + const FaceData& face_data = FaceData (), + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData& half_edge_data = HalfEdgeData ()) + { + add_triangle_ [0] = idx_v_0; + add_triangle_ [1] = idx_v_1; + add_triangle_ [2] = idx_v_2; + + return (this->addFaceImplBase (add_triangle_, face_data, edge_data, half_edge_data)); + } + + /** \brief Add two triangles for the four given input vertices. When using a manifold triangle mesh it is not possible to connect two bounded regions without going through a non-manifold intermediate step. This method first tries to add the triangles individually and if this fails connects the whole configuration at once (if possible). + * \param[in] vertices Indices to the vertices of the new face. (The size must be equal to four). + * \param[in] face_data Data that is set for the face. + * \param[in] half_edge_data Data that is set for all added half-edges. + * \param[in] edge_data Data that is set for all added edges. + * \return Pair of face indices. The first index is valid if one triangle was added. Both indices are valid if two triangles were added. + * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior! + */ + FaceIndexPair + addTrianglePair (const VertexIndices& vertices, + const FaceData& face_data = FaceData (), + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData& half_edge_data = HalfEdgeData ()) + { + if (vertices.size () != 4) + { + return (std::make_pair (FaceIndex (), FaceIndex ())); + } + return (this->addTrianglePair (vertices [0], vertices [1], vertices [2], vertices [3], face_data, edge_data, half_edge_data)); + } + + /** \brief Add two triangles for the four given input vertices. When using a manifold triangle mesh it is not possible to connect two bounded regions without going through a non-manifold intermediate step. This method first tries to add the triangles individually and if this fails connects the whole configuration at once (if possible). + * \param[in] idx_v_0 Index to the first vertex. + * \param[in] idx_v_1 Index to the second vertex. + * \param[in] idx_v_2 Index to the third vertex. + * \param[in] idx_v_3 Index to the fourth vertex. + * \param[in] face_data Data that is set for the face. + * \param[in] half_edge_data Data that is set for all added half-edges. + * \param[in] edge_data Data that is set for all added edges. + * \return Pair of face indices. The first index is valid if one triangle was added. Both indices are valid if two triangles were added. + * \warning The vertices must be valid and unique (each vertex may be contained only once). Not complying with this requirement results in undefined behavior! + */ + inline FaceIndexPair + addTrianglePair (const VertexIndex& idx_v_0, + const VertexIndex& idx_v_1, + const VertexIndex& idx_v_2, + const VertexIndex& idx_v_3, + const FaceData& face_data = FaceData (), + const EdgeData& edge_data = EdgeData (), + const HalfEdgeData& half_edge_data = HalfEdgeData ()) + { + // Try to add two faces + // 3 - 2 + // | / | + // 0 - 1 + FaceIndex idx_face_0 = this->addFace (idx_v_0, idx_v_1, idx_v_2, face_data); + FaceIndex idx_face_1 = this->addFace (idx_v_0, idx_v_2, idx_v_3, face_data); + + if (idx_face_0.isValid ()) + { + return (std::make_pair (idx_face_0, idx_face_1)); + } + if (idx_face_1.isValid ()) + { + idx_face_0 = this->addFace (idx_v_0, idx_v_1, idx_v_2, face_data); // might be possible to add now + return (std::make_pair (idx_face_1, idx_face_0)); + } + + // Try to add two faces + // 3 - 2 + // | \ | + // 0 - 1 + idx_face_0 = this->addFace (idx_v_1, idx_v_2, idx_v_3, face_data); + idx_face_1 = this->addFace (idx_v_0, idx_v_1, idx_v_3, face_data); + + if (idx_face_0.isValid ()) + { + return (std::make_pair (idx_face_0, idx_face_1)); + } + if (idx_face_1.isValid ()) + { + idx_face_0 = this->addFace (idx_v_1, idx_v_2, idx_v_3, face_data); // might be possible to add now + return (std::make_pair (idx_face_1, idx_face_0)); + } + + if (!IsManifold::value) + { + return (std::make_pair (FaceIndex (), FaceIndex ())); + } + + // Check manifoldness + if (!Base::checkTopology1 (idx_v_0,idx_v_1, inner_he_atp_ [0], is_new_atp_ [0], IsManifold ()) || + !Base::checkTopology1 (idx_v_1,idx_v_2, inner_he_atp_ [1], is_new_atp_ [1], IsManifold ()) || + !Base::checkTopology1 (idx_v_2,idx_v_3, inner_he_atp_ [2], is_new_atp_ [2], IsManifold ()) || + !Base::checkTopology1 (idx_v_3,idx_v_0, inner_he_atp_ [3], is_new_atp_ [3], IsManifold ())) + { + return (std::make_pair (FaceIndex (), FaceIndex ())); + } + + // Connect the triangle pair + if (!is_new_atp_ [0] && is_new_atp_ [1] && !is_new_atp_ [2] && is_new_atp_ [3]) + { + return (this->connectTrianglePair (inner_he_atp_ [0], inner_he_atp_ [2], idx_v_0, idx_v_1, idx_v_2, idx_v_3, face_data, edge_data, half_edge_data)); + } + if (is_new_atp_ [0] && !is_new_atp_ [1] && is_new_atp_ [2] && !is_new_atp_ [3]) + { + return (this->connectTrianglePair (inner_he_atp_ [1], inner_he_atp_ [3], idx_v_1, idx_v_2, idx_v_3, idx_v_0, face_data, edge_data, half_edge_data)); + } + return (std::make_pair (FaceIndex (), FaceIndex ())); + } + + private: + + // NOTE: Can't use the typedef of Base as a friend. + friend class pcl::geometry::MeshBase , MeshTraitsT, pcl::geometry::TriangleMeshTag>; + + /** \brief addFace for the triangular mesh. */ + inline FaceIndex + addFaceImpl (const VertexIndices& vertices, + const FaceData& face_data, + const EdgeData& edge_data, + const HalfEdgeData& half_edge_data) + { + if (vertices.size () == 3) + return (this->addFaceImplBase (vertices, face_data, edge_data, half_edge_data)); + return (FaceIndex ()); + } + + /** \brief Connect the triangles a-b-c and a-c-d. The edges a-b and c-d must be old and the edges b-c and d-a must be new. */ + // d - c + // | / | + // a - b + FaceIndexPair + connectTrianglePair (const HalfEdgeIndex& idx_he_ab, + const HalfEdgeIndex& idx_he_cd, + const VertexIndex& idx_v_a, + const VertexIndex& idx_v_b, + const VertexIndex& idx_v_c, + const VertexIndex& idx_v_d, + const FaceData& face_data, + const EdgeData& edge_data, + const HalfEdgeData& he_data) + { + // Add new half-edges + const HalfEdgeIndex idx_he_bc = Base::addEdge (idx_v_b, idx_v_c, he_data, edge_data); + const HalfEdgeIndex idx_he_da = Base::addEdge (idx_v_d, idx_v_a, he_data, edge_data); + const HalfEdgeIndex idx_he_ca = Base::addEdge (idx_v_c, idx_v_a, he_data, edge_data); + + const HalfEdgeIndex idx_he_cb = Base::getOppositeHalfEdgeIndex (idx_he_bc); + const HalfEdgeIndex idx_he_ad = Base::getOppositeHalfEdgeIndex (idx_he_da); + const HalfEdgeIndex idx_he_ac = Base::getOppositeHalfEdgeIndex (idx_he_ca); + + // Get the existing half-edges + const HalfEdgeIndex idx_he_ab_prev = Base::getPrevHalfEdgeIndex (idx_he_ab); // No reference! + const HalfEdgeIndex idx_he_ab_next = Base::getNextHalfEdgeIndex (idx_he_ab); // No reference! + + const HalfEdgeIndex idx_he_cd_prev = Base::getPrevHalfEdgeIndex (idx_he_cd); // No reference! + const HalfEdgeIndex idx_he_cd_next = Base::getNextHalfEdgeIndex (idx_he_cd); // No reference! + + // Connect the outer half-edges + Base::connectPrevNext (idx_he_ab_prev, idx_he_ad ); + Base::connectPrevNext (idx_he_ad , idx_he_cd_next); + Base::connectPrevNext (idx_he_cd_prev, idx_he_cb ); + Base::connectPrevNext (idx_he_cb , idx_he_ab_next); + + // Connect the inner half-edges + Base::connectPrevNext (idx_he_ab, idx_he_bc); + Base::connectPrevNext (idx_he_bc, idx_he_ca); + Base::connectPrevNext (idx_he_ca, idx_he_ab); + + Base::connectPrevNext (idx_he_ac, idx_he_cd); + Base::connectPrevNext (idx_he_cd, idx_he_da); + Base::connectPrevNext (idx_he_da, idx_he_ac); + + // Connect the vertices to the boundary half-edges + Base::setOutgoingHalfEdgeIndex (idx_v_a, idx_he_ad ); + Base::setOutgoingHalfEdgeIndex (idx_v_b, idx_he_ab_next); + Base::setOutgoingHalfEdgeIndex (idx_v_c, idx_he_cb ); + Base::setOutgoingHalfEdgeIndex (idx_v_d, idx_he_cd_next); + + // Add and connect the faces + HalfEdgeIndices inner_he_abc; inner_he_abc.reserve (3); + inner_he_abc.push_back (idx_he_ab); + inner_he_abc.push_back (idx_he_bc); + inner_he_abc.push_back (idx_he_ca); + + HalfEdgeIndices inner_he_acd; inner_he_acd.reserve (3); + inner_he_acd.push_back (idx_he_ac); + inner_he_acd.push_back (idx_he_cd); + inner_he_acd.push_back (idx_he_da); + + const FaceIndex idx_f_abc = Base::connectFace (inner_he_abc, face_data); + const FaceIndex idx_f_acd = Base::connectFace (inner_he_acd, face_data); + + return (std::make_pair (idx_f_abc, idx_f_acd)); + } + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + /** \brief Storage for adding a triangle. */ + VertexIndices add_triangle_; + + /** \brief Storage for addTrianglePair. */ + HalfEdgeIndices inner_he_atp_; + + /** \brief Storage for addTrianglePair. */ + std::vector is_new_atp_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } // End namespace geom +} // End namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/impl/cloud_iterator.hpp b/deps_install/include/pcl-1.10/pcl/impl/cloud_iterator.hpp new file mode 100755 index 0000000..86f396a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/impl/cloud_iterator.hpp @@ -0,0 +1,557 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_POINT_CLOUD_ITERATOR_HPP_ +#define PCL_POINT_CLOUD_ITERATOR_HPP_ + +#include + +namespace pcl +{ + /** \brief + * \author Suat Gedikli + */ + template + class DefaultIterator : public CloudIterator::Iterator + { + public: + DefaultIterator (PointCloud& cloud) + : cloud_ (cloud) + , iterator_ (cloud.begin ()) + { + } + + ~DefaultIterator () + { + } + + void operator ++ () + { + ++iterator_; + } + + void operator ++ (int) + { + iterator_++; + } + + PointT& operator* () const + { + return (*iterator_); + } + + PointT* operator-> () + { + return (&(*iterator_)); + } + + unsigned getCurrentPointIndex () const + { + return (iterator_ - cloud_.begin ()); + } + + unsigned getCurrentIndex () const + { + return (iterator_ - cloud_.begin ()); + } + + std::size_t size () const + { + return cloud_.size (); + } + + void reset () + { + iterator_ = cloud_.begin (); + } + + bool isValid () const + { + return (iterator_ != cloud_.end ()); + } + private: + PointCloud& cloud_; + typename PointCloud::iterator iterator_; + }; + + /** \brief + * \author Suat Gedikli + */ + template + class IteratorIdx : public CloudIterator::Iterator + { + public: + IteratorIdx (PointCloud& cloud, const std::vector& indices) + : cloud_ (cloud) + , indices_ (indices) + , iterator_ (indices_.begin ()) + { + } + + IteratorIdx (PointCloud& cloud, const PointIndices& indices) + : cloud_ (cloud) + , indices_ (indices.indices) + , iterator_ (indices_.begin ()) + { + } + + virtual ~IteratorIdx () {} + + void operator ++ () + { + ++iterator_; + } + + void operator ++ (int) + { + iterator_++; + } + + PointT& operator* () const + { + return (cloud_.points [*iterator_]); + } + + PointT* operator-> () + { + return (&(cloud_.points [*iterator_])); + } + + unsigned getCurrentPointIndex () const + { + return (*iterator_); + } + + unsigned getCurrentIndex () const + { + return (iterator_ - indices_.begin ()); + } + + std::size_t size () const + { + return indices_.size (); + } + + void reset () + { + iterator_ = indices_.begin (); + } + + bool isValid () const + { + return (iterator_ != indices_.end ()); + } + + private: + PointCloud& cloud_; + std::vector indices_; + std::vector::iterator iterator_; + }; + + /** \brief + * \author Suat Gedikli + */ + template + class ConstCloudIterator::DefaultConstIterator : public ConstCloudIterator::Iterator + { + public: + DefaultConstIterator (const PointCloud& cloud) + : cloud_ (cloud) + , iterator_ (cloud.begin ()) + { + } + + ~DefaultConstIterator () + { + } + + void operator ++ () override + { + ++iterator_; + } + + void operator ++ (int) override + { + iterator_++; + } + + const PointT& operator* () const override + { + return (*iterator_); + } + + const PointT* operator-> () const override + { + return (&(*iterator_)); + } + + unsigned getCurrentPointIndex () const override + { + return (unsigned (iterator_ - cloud_.begin ())); + } + + unsigned getCurrentIndex () const override + { + return (unsigned (iterator_ - cloud_.begin ())); + } + + std::size_t size () const override + { + return cloud_.size (); + } + + void reset () override + { + iterator_ = cloud_.begin (); + } + + bool isValid () const override + { + return (iterator_ != cloud_.end ()); + } + private: + const PointCloud& cloud_; + typename PointCloud::const_iterator iterator_; + }; + + /** \brief + * \author Suat Gedikli + */ + template + class ConstCloudIterator::ConstIteratorIdx : public ConstCloudIterator::Iterator + { + public: + ConstIteratorIdx (const PointCloud& cloud, + const std::vector& indices) + : cloud_ (cloud) + , indices_ (indices) + , iterator_ (indices_.begin ()) + { + } + + ConstIteratorIdx (const PointCloud& cloud, + const PointIndices& indices) + : cloud_ (cloud) + , indices_ (indices.indices) + , iterator_ (indices_.begin ()) + { + } + + ~ConstIteratorIdx () {} + + void operator ++ () override + { + ++iterator_; + } + + void operator ++ (int) override + { + iterator_++; + } + + const PointT& operator* () const override + { + return (cloud_.points[*iterator_]); + } + + const PointT* operator-> () const override + { + return (&(cloud_.points [*iterator_])); + } + + unsigned getCurrentPointIndex () const override + { + return (unsigned (*iterator_)); + } + + unsigned getCurrentIndex () const override + { + return (unsigned (iterator_ - indices_.begin ())); + } + + std::size_t size () const override + { + return indices_.size (); + } + + void reset () override + { + iterator_ = indices_.begin (); + } + + bool isValid () const override + { + return (iterator_ != indices_.end ()); + } + + private: + const PointCloud& cloud_; + std::vector indices_; + std::vector::iterator iterator_; + }; +} // namespace pcl + +////////////////////////////////////////////////////////////////////////////// +template +pcl::CloudIterator::CloudIterator (PointCloud& cloud) + : iterator_ (new DefaultIterator (cloud)) +{ +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::CloudIterator::CloudIterator ( + PointCloud& cloud, const std::vector& indices) + : iterator_ (new IteratorIdx (cloud, indices)) +{ +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::CloudIterator::CloudIterator ( + PointCloud& cloud, const PointIndices& indices) + : iterator_ (new IteratorIdx (cloud, indices)) +{ +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::CloudIterator::CloudIterator ( + PointCloud& cloud, const Correspondences& corrs, bool source) +{ + std::vector indices; + indices.reserve (corrs.size ()); + if (source) + { + for (const auto &corr : corrs) + indices.push_back (corr.index_query); + } + else + { + for (const auto &corr : corrs) + indices.push_back (corr.index_match); + } + iterator_ = new IteratorIdx (cloud, indices); +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::CloudIterator::~CloudIterator () +{ + delete iterator_; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::CloudIterator::operator ++ () +{ + iterator_->operator++ (); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::CloudIterator::operator ++ (int) +{ + iterator_->operator++ (0); +} + +////////////////////////////////////////////////////////////////////////////// +template PointT& +pcl::CloudIterator::operator* () const +{ + return (iterator_->operator * ()); +} + +////////////////////////////////////////////////////////////////////////////// +template PointT* +pcl::CloudIterator::operator-> () const +{ + return (iterator_->operator-> ()); +} + +////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::CloudIterator::getCurrentPointIndex () const +{ + return (iterator_->getCurrentPointIndex ()); +} + +////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::CloudIterator::getCurrentIndex () const +{ + return (iterator_->getCurrentIndex ()); +} + +////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::CloudIterator::size () const +{ + return (iterator_->size ()); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::CloudIterator::reset () +{ + iterator_->reset (); +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::CloudIterator::isValid () const +{ + return (iterator_->isValid ()); +} + + +////////////////////////////////////////////////////////////////////////////// +template +pcl::ConstCloudIterator::ConstCloudIterator (const PointCloud& cloud) + : iterator_ (new typename pcl::ConstCloudIterator::DefaultConstIterator (cloud)) +{ +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::ConstCloudIterator::ConstCloudIterator ( + const PointCloud& cloud, const std::vector& indices) + : iterator_ (new typename pcl::ConstCloudIterator::ConstIteratorIdx (cloud, indices)) +{ +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::ConstCloudIterator::ConstCloudIterator ( + const PointCloud& cloud, const PointIndices& indices) + : iterator_ (new typename pcl::ConstCloudIterator::ConstIteratorIdx (cloud, indices)) +{ +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::ConstCloudIterator::ConstCloudIterator ( + const PointCloud& cloud, const Correspondences& corrs, bool source) +{ + std::vector indices; + indices.reserve (corrs.size ()); + if (source) + { + for (const auto &corr : corrs) + indices.push_back (corr.index_query); + } + else + { + for (const auto &corr : corrs) + indices.push_back (corr.index_match); + } + iterator_ = new typename pcl::ConstCloudIterator::ConstIteratorIdx (cloud, indices); +} + +////////////////////////////////////////////////////////////////////////////// +template +pcl::ConstCloudIterator::~ConstCloudIterator () +{ + delete iterator_; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::ConstCloudIterator::operator ++ () +{ + iterator_->operator++ (); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::ConstCloudIterator::operator ++ (int) +{ + iterator_->operator++ (0); +} + +////////////////////////////////////////////////////////////////////////////// +template const PointT& +pcl::ConstCloudIterator::operator* () const +{ + return (iterator_->operator * ()); +} + +////////////////////////////////////////////////////////////////////////////// +template const PointT* +pcl::ConstCloudIterator::operator-> () const +{ + return (iterator_->operator-> ()); +} + +////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::ConstCloudIterator::getCurrentPointIndex () const +{ + return (iterator_->getCurrentPointIndex ()); +} + +////////////////////////////////////////////////////////////////////////////// +template unsigned +pcl::ConstCloudIterator::getCurrentIndex () const +{ + return (iterator_->getCurrentIndex ()); +} + +////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::ConstCloudIterator::size () const +{ + return (iterator_->size ()); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::ConstCloudIterator::reset () +{ + iterator_->reset (); +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ConstCloudIterator::isValid () const +{ + return (iterator_->isValid ()); +} + +#endif // PCL_POINT_CLOUD_ITERATOR_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/impl/instantiate.hpp b/deps_install/include/pcl-1.10/pcl/impl/instantiate.hpp new file mode 100755 index 0000000..ea73a0c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/impl/instantiate.hpp @@ -0,0 +1,114 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef PCL_IMPL_INSTANTIATE_H_ +#define PCL_IMPL_INSTANTIATE_H_ + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#include + +//#define PCL_POINT_TYPES (bool)(int)(float)(double) +//#define PCL_TEMPLATES (Type)(Othertype) + +// +// PCL_INSTANTIATE: call to instantiate template TEMPLATE for all +// POINT_TYPES +// + +#ifdef PCL_NO_PRECOMPILE + +#define PCL_INSTANTIATE_IMPL(r, TEMPLATE, POINT_TYPE) +#define PCL_INSTANTIATE(TEMPLATE, POINT_TYPES) +#define PCL_INSTANTIATE_PRODUCT_IMPL(r, product) +#define PCL_INSTANTIATE_PRODUCT(TEMPLATE, PRODUCT) + +#else + +#include +#include +#include +#include +#include + +#define PCL_INSTANTIATE_IMPL(r, TEMPLATE, POINT_TYPE) \ + BOOST_PP_CAT(PCL_INSTANTIATE_, TEMPLATE)(POINT_TYPE) + +#define PCL_INSTANTIATE(TEMPLATE, POINT_TYPES) \ + BOOST_PP_SEQ_FOR_EACH(PCL_INSTANTIATE_IMPL, TEMPLATE, POINT_TYPES) + + +// +// PCL_INSTANTIATE_PRODUCT(templatename, (seq1)(seq2)...(seqN)) +// +// instantiate templates +// +// A call to PCL_INSTANTIATE_PRODUCT(T, ((a)(b)) ((d)(e)) ) results in calls +// +// PCL_INSTANTIATE_T(a, d) +// PCL_INSTANTIATE_T(a, e) +// PCL_INSTANTIATE_T(b, d) +// PCL_INSTANTIATE_T(b, e) +// +// That is, PCL_INSTANTIATE_T is called for the cartesian product of the sequences seq1 ... seqN +// +// BE CAREFUL WITH YOUR PARENTHESIS! The argument PRODUCT is a +// sequence of sequences. e.g. if it were three sequences of, +// 1. letters, 2. numbers, and 3. our favorite transylvanians, it +// would be +// +// ((x)(y)(z))((1)(2)(3))((dracula)(radu)) +// +#ifdef _MSC_VER +#define PCL_INSTANTIATE_PRODUCT_IMPL(r, product) \ + BOOST_PP_CAT(PCL_INSTANTIATE_, BOOST_PP_SEQ_HEAD(product)) \ + BOOST_PP_EXPAND(BOOST_PP_SEQ_TO_TUPLE(BOOST_PP_SEQ_TAIL(product))) +#else +#define PCL_INSTANTIATE_PRODUCT_IMPL(r, product) \ + BOOST_PP_EXPAND(BOOST_PP_CAT(PCL_INSTANTIATE_, BOOST_PP_SEQ_HEAD(product)) \ + BOOST_PP_SEQ_TO_TUPLE(BOOST_PP_SEQ_TAIL(product))) +#endif + + +#define PCL_INSTANTIATE_PRODUCT(TEMPLATE, PRODUCT) \ + BOOST_PP_SEQ_FOR_EACH_PRODUCT(PCL_INSTANTIATE_PRODUCT_IMPL, ((TEMPLATE))PRODUCT) + +#endif + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/impl/pcl_base.hpp b/deps_install/include/pcl-1.10/pcl/impl/pcl_base.hpp new file mode 100755 index 0000000..8addab9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/impl/pcl_base.hpp @@ -0,0 +1,179 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef PCL_PCL_IMPL_BASE_HPP_ +#define PCL_PCL_IMPL_BASE_HPP_ + +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::PCLBase::PCLBase () + : input_ () + , use_indices_ (false) + , fake_indices_ (false) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::PCLBase::PCLBase (const PCLBase& base) + : input_ (base.input_) + , indices_ (base.indices_) + , use_indices_ (base.use_indices_) + , fake_indices_ (base.fake_indices_) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PCLBase::setInputCloud (const PointCloudConstPtr &cloud) +{ + input_ = cloud; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PCLBase::setIndices (const IndicesPtr &indices) +{ + indices_ = indices; + fake_indices_ = false; + use_indices_ = true; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PCLBase::setIndices (const IndicesConstPtr &indices) +{ + indices_.reset (new std::vector (*indices)); + fake_indices_ = false; + use_indices_ = true; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PCLBase::setIndices (const PointIndicesConstPtr &indices) +{ + indices_.reset (new std::vector (indices->indices)); + fake_indices_ = false; + use_indices_ = true; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PCLBase::setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols) +{ + if ((nb_rows > input_->height) || (row_start > input_->height)) + { + PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height", input_->height); + return; + } + + if ((nb_cols > input_->width) || (col_start > input_->width)) + { + PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width", input_->width); + return; + } + + std::size_t row_end = row_start + nb_rows; + if (row_end > input_->height) + { + PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height); + return; + } + + std::size_t col_end = col_start + nb_cols; + if (col_end > input_->width) + { + PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width); + return; + } + + indices_.reset (new std::vector); + indices_->reserve (nb_cols * nb_rows); + for(std::size_t i = row_start; i < row_end; i++) + for(std::size_t j = col_start; j < col_end; j++) + indices_->push_back (static_cast ((i * input_->width) + j)); + fake_indices_ = false; + use_indices_ = true; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PCLBase::initCompute () +{ + // Check if input was set + if (!input_) + return (false); + + // If no point indices have been given, construct a set of indices for the entire input point cloud + if (!indices_) + { + fake_indices_ = true; + indices_.reset (new std::vector); + } + + // If we have a set of fake indices, but they do not match the number of points in the cloud, update them + if (fake_indices_ && indices_->size () != input_->points.size ()) + { + const auto indices_size = indices_->size (); + try + { + indices_->resize (input_->points.size ()); + } + catch (const std::bad_alloc&) + { + PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", input_->points.size ()); + } + for (auto i = indices_size; i < indices_->size (); ++i) { (*indices_)[i] = static_cast(i); } + } + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PCLBase::deinitCompute () +{ + return (true); +} + +#define PCL_INSTANTIATE_PCLBase(T) template class PCL_EXPORTS pcl::PCLBase; + +#endif //#ifndef PCL_PCL_IMPL_BASE_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/impl/point_types.hpp b/deps_install/include/pcl-1.10/pcl/impl/point_types.hpp new file mode 100755 index 0000000..8f5823c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/impl/point_types.hpp @@ -0,0 +1,1832 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_IMPL_POINT_TYPES_HPP_ +#define PCL_IMPL_POINT_TYPES_HPP_ + +#include +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +#include + +#include + +// Define all PCL point types +#define PCL_POINT_TYPES \ + (pcl::PointXYZ) \ + (pcl::PointXYZI) \ + (pcl::PointXYZL) \ + (pcl::Label) \ + (pcl::PointXYZRGBA) \ + (pcl::PointXYZRGB) \ + (pcl::PointXYZRGBL) \ + (pcl::PointXYZHSV) \ + (pcl::PointXY) \ + (pcl::InterestPoint) \ + (pcl::Axis) \ + (pcl::Normal) \ + (pcl::PointNormal) \ + (pcl::PointXYZRGBNormal) \ + (pcl::PointXYZINormal) \ + (pcl::PointXYZLNormal) \ + (pcl::PointWithRange) \ + (pcl::PointWithViewpoint) \ + (pcl::MomentInvariants) \ + (pcl::PrincipalRadiiRSD) \ + (pcl::Boundary) \ + (pcl::PrincipalCurvatures) \ + (pcl::PFHSignature125) \ + (pcl::PFHRGBSignature250) \ + (pcl::PPFSignature) \ + (pcl::CPPFSignature) \ + (pcl::PPFRGBSignature) \ + (pcl::NormalBasedSignature12) \ + (pcl::FPFHSignature33) \ + (pcl::VFHSignature308) \ + (pcl::GASDSignature512) \ + (pcl::GASDSignature984) \ + (pcl::GASDSignature7992) \ + (pcl::GRSDSignature21) \ + (pcl::ESFSignature640) \ + (pcl::BRISKSignature512) \ + (pcl::Narf36) \ + (pcl::IntensityGradient) \ + (pcl::PointWithScale) \ + (pcl::PointSurfel) \ + (pcl::ShapeContext1980) \ + (pcl::UniqueShapeContext1960) \ + (pcl::SHOT352) \ + (pcl::SHOT1344) \ + (pcl::PointUV) \ + (pcl::ReferenceFrame) \ + (pcl::PointDEM) + +// Define all point types that include RGB data +#define PCL_RGB_POINT_TYPES \ + (pcl::PointXYZRGBA) \ + (pcl::PointXYZRGB) \ + (pcl::PointXYZRGBL) \ + (pcl::PointXYZRGBNormal) \ + (pcl::PointSurfel) \ + +// Define all point types that include XYZ data +#define PCL_XYZ_POINT_TYPES \ + (pcl::PointXYZ) \ + (pcl::PointXYZI) \ + (pcl::PointXYZL) \ + (pcl::PointXYZRGBA) \ + (pcl::PointXYZRGB) \ + (pcl::PointXYZRGBL) \ + (pcl::PointXYZHSV) \ + (pcl::InterestPoint) \ + (pcl::PointNormal) \ + (pcl::PointXYZRGBNormal) \ + (pcl::PointXYZINormal) \ + (pcl::PointXYZLNormal) \ + (pcl::PointWithRange) \ + (pcl::PointWithViewpoint) \ + (pcl::PointWithScale) \ + (pcl::PointSurfel) \ + (pcl::PointDEM) + +// Define all point types with XYZ and label +#define PCL_XYZL_POINT_TYPES \ + (pcl::PointXYZL) \ + (pcl::PointXYZRGBL) \ + (pcl::PointXYZLNormal) + +// Define all point types that include normal[3] data +#define PCL_NORMAL_POINT_TYPES \ + (pcl::Normal) \ + (pcl::PointNormal) \ + (pcl::PointXYZRGBNormal) \ + (pcl::PointXYZINormal) \ + (pcl::PointXYZLNormal) \ + (pcl::PointSurfel) + +// Define all point types that represent features +#define PCL_FEATURE_POINT_TYPES \ + (pcl::PFHSignature125) \ + (pcl::PFHRGBSignature250) \ + (pcl::PPFSignature) \ + (pcl::CPPFSignature) \ + (pcl::PPFRGBSignature) \ + (pcl::NormalBasedSignature12) \ + (pcl::FPFHSignature33) \ + (pcl::VFHSignature308) \ + (pcl::GASDSignature512) \ + (pcl::GASDSignature984) \ + (pcl::GASDSignature7992) \ + (pcl::GRSDSignature21) \ + (pcl::ESFSignature640) \ + (pcl::BRISKSignature512) \ + (pcl::Narf36) + +namespace pcl +{ + + using Array3fMap = Eigen::Map; + using Array3fMapConst = const Eigen::Map; + using Array4fMap = Eigen::Map; + using Array4fMapConst = const Eigen::Map; + using Vector3fMap = Eigen::Map; + using Vector3fMapConst = const Eigen::Map; + using Vector4fMap = Eigen::Map; + using Vector4fMapConst = const Eigen::Map; + + using Vector3c = Eigen::Matrix; + using Vector3cMap = Eigen::Map; + using Vector3cMapConst = const Eigen::Map; + using Vector4c = Eigen::Matrix; + using Vector4cMap = Eigen::Map; + using Vector4cMapConst = const Eigen::Map; + +#define PCL_ADD_UNION_POINT4D \ + union EIGEN_ALIGN16 { \ + float data[4]; \ + struct { \ + float x; \ + float y; \ + float z; \ + }; \ + }; + +#define PCL_ADD_EIGEN_MAPS_POINT4D \ + inline pcl::Vector3fMap getVector3fMap () { return (pcl::Vector3fMap (data)); } \ + inline pcl::Vector3fMapConst getVector3fMap () const { return (pcl::Vector3fMapConst (data)); } \ + inline pcl::Vector4fMap getVector4fMap () { return (pcl::Vector4fMap (data)); } \ + inline pcl::Vector4fMapConst getVector4fMap () const { return (pcl::Vector4fMapConst (data)); } \ + inline pcl::Array3fMap getArray3fMap () { return (pcl::Array3fMap (data)); } \ + inline pcl::Array3fMapConst getArray3fMap () const { return (pcl::Array3fMapConst (data)); } \ + inline pcl::Array4fMap getArray4fMap () { return (pcl::Array4fMap (data)); } \ + inline pcl::Array4fMapConst getArray4fMap () const { return (pcl::Array4fMapConst (data)); } + +#define PCL_ADD_POINT4D \ + PCL_ADD_UNION_POINT4D \ + PCL_ADD_EIGEN_MAPS_POINT4D + +#define PCL_ADD_UNION_NORMAL4D \ + union EIGEN_ALIGN16 { \ + float data_n[4]; \ + float normal[3]; \ + struct { \ + float normal_x; \ + float normal_y; \ + float normal_z; \ + }; \ + }; + +#define PCL_ADD_EIGEN_MAPS_NORMAL4D \ + inline pcl::Vector3fMap getNormalVector3fMap () { return (pcl::Vector3fMap (data_n)); } \ + inline pcl::Vector3fMapConst getNormalVector3fMap () const { return (pcl::Vector3fMapConst (data_n)); } \ + inline pcl::Vector4fMap getNormalVector4fMap () { return (pcl::Vector4fMap (data_n)); } \ + inline pcl::Vector4fMapConst getNormalVector4fMap () const { return (pcl::Vector4fMapConst (data_n)); } + +#define PCL_ADD_NORMAL4D \ + PCL_ADD_UNION_NORMAL4D \ + PCL_ADD_EIGEN_MAPS_NORMAL4D + +#define PCL_ADD_UNION_RGB \ + union \ + { \ + union \ + { \ + struct \ + { \ + std::uint8_t b; \ + std::uint8_t g; \ + std::uint8_t r; \ + std::uint8_t a; \ + }; \ + float rgb; \ + }; \ + std::uint32_t rgba; \ + }; + +#define PCL_ADD_EIGEN_MAPS_RGB \ + inline Eigen::Vector3i getRGBVector3i () { return (Eigen::Vector3i (r, g, b)); } \ + inline const Eigen::Vector3i getRGBVector3i () const { return (Eigen::Vector3i (r, g, b)); } \ + inline Eigen::Vector4i getRGBVector4i () { return (Eigen::Vector4i (r, g, b, a)); } \ + inline const Eigen::Vector4i getRGBVector4i () const { return (Eigen::Vector4i (r, g, b, a)); } \ + inline Eigen::Vector4i getRGBAVector4i () { return (Eigen::Vector4i (r, g, b, a)); } \ + inline const Eigen::Vector4i getRGBAVector4i () const { return (Eigen::Vector4i (r, g, b, a)); } \ + inline pcl::Vector3cMap getBGRVector3cMap () { return (pcl::Vector3cMap (reinterpret_cast (&rgba))); } \ + inline pcl::Vector3cMapConst getBGRVector3cMap () const { return (pcl::Vector3cMapConst (reinterpret_cast (&rgba))); } \ + inline pcl::Vector4cMap getBGRAVector4cMap () { return (pcl::Vector4cMap (reinterpret_cast (&rgba))); } \ + inline pcl::Vector4cMapConst getBGRAVector4cMap () const { return (pcl::Vector4cMapConst (reinterpret_cast (&rgba))); } + +#define PCL_ADD_RGB \ + PCL_ADD_UNION_RGB \ + PCL_ADD_EIGEN_MAPS_RGB + +#define PCL_ADD_INTENSITY \ + struct \ + { \ + float intensity; \ + }; \ + +#define PCL_ADD_INTENSITY_8U \ + struct \ + { \ + std::uint8_t intensity; \ + }; \ + +#define PCL_ADD_INTENSITY_32U \ + struct \ + { \ + std::uint32_t intensity; \ + }; \ + + + struct _PointXYZ + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZ& p); + /** \brief A point structure representing Euclidean xyz coordinates. (SSE friendly) + * \ingroup common + */ + struct EIGEN_ALIGN16 PointXYZ : public _PointXYZ + { + inline PointXYZ (const _PointXYZ &p): PointXYZ(p.x, p.y, p.z) {} + + inline PointXYZ (): PointXYZ(0.f, 0.f, 0.f) {} + + inline PointXYZ (float _x, float _y, float _z) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZ& p); + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + +#ifdef RGB +#undef RGB +#endif + struct _RGB + { + PCL_ADD_RGB; + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const RGB& p); + /** \brief A structure representing RGB color information. + * + * The RGBA information is available either as separate r, g, b, or as a + * packed std::uint32_t rgba value. To pack it, use: + * + * \code + * int rgb = ((int)r) << 16 | ((int)g) << 8 | ((int)b); + * \endcode + * + * To unpack it use: + * + * \code + * int rgb = ...; + * std::uint8_t r = (rgb >> 16) & 0x0000ff; + * std::uint8_t g = (rgb >> 8) & 0x0000ff; + * std::uint8_t b = (rgb) & 0x0000ff; + * \endcode + * + */ + struct RGB: public _RGB + { + inline RGB (const _RGB &p) + { + rgba = p.rgba; + } + + inline RGB (): RGB(0, 0, 0) {} + + inline RGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) + { + r = _r; g = _g; b = _b; + a = 255; + } + + friend std::ostream& operator << (std::ostream& os, const RGB& p); + }; + + struct _Intensity + { + PCL_ADD_INTENSITY; + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Intensity& p); + /** \brief A point structure representing the grayscale intensity in single-channel images. + * Intensity is represented as a float value. + * \ingroup common + */ + struct Intensity: public _Intensity + { + inline Intensity (const _Intensity &p) + { + intensity = p.intensity; + } + + inline Intensity (float _intensity = 0.f) + { + intensity = _intensity; + } + + friend std::ostream& operator << (std::ostream& os, const Intensity& p); + }; + + + struct _Intensity8u + { + PCL_ADD_INTENSITY_8U; + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Intensity8u& p); + /** \brief A point structure representing the grayscale intensity in single-channel images. + * Intensity is represented as a std::uint8_t value. + * \ingroup common + */ + struct Intensity8u: public _Intensity8u + { + inline Intensity8u (const _Intensity8u &p) + { + intensity = p.intensity; + } + + inline Intensity8u (std::uint8_t _intensity = 0) + { + intensity = _intensity; + } + +#if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 + operator unsigned char() const + { + return intensity; + } +#endif + + friend std::ostream& operator << (std::ostream& os, const Intensity8u& p); + }; + + struct _Intensity32u + { + PCL_ADD_INTENSITY_32U; + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Intensity32u& p); + /** \brief A point structure representing the grayscale intensity in single-channel images. + * Intensity is represented as a std::uint32_t value. + * \ingroup common + */ + struct Intensity32u: public _Intensity32u + { + inline Intensity32u (const _Intensity32u &p) + { + intensity = p.intensity; + } + + inline Intensity32u (std::uint32_t _intensity = 0) + { + intensity = _intensity; + } + + friend std::ostream& operator << (std::ostream& os, const Intensity32u& p); + }; + + /** \brief A point structure representing Euclidean xyz coordinates, and the intensity value. + * \ingroup common + */ + struct EIGEN_ALIGN16 _PointXYZI + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + union + { + struct + { + float intensity; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZI& p); + struct PointXYZI : public _PointXYZI + { + inline PointXYZI (const _PointXYZI &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + intensity = p.intensity; + } + + inline PointXYZI (float _intensity = 0.f): PointXYZI(0.f, 0.f, 0.f, _intensity) {} + + inline PointXYZI (float _x, float _y, float _z, float _intensity = 0.f) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + intensity = _intensity; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZI& p); + }; + + + struct EIGEN_ALIGN16 _PointXYZL + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + std::uint32_t label; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZL& p); + struct PointXYZL : public _PointXYZL + { + inline PointXYZL (const _PointXYZL &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + label = p.label; + } + + inline PointXYZL (std::uint32_t _label = 0): PointXYZL(0.f, 0.f, 0.f, _label) {} + + inline PointXYZL (float _x, float _y, float _z, std::uint32_t _label = 0) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + label = _label; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZL& p); + }; + + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Label& p); + struct Label + { + std::uint32_t label = 0; + + Label (std::uint32_t _label = 0): label(_label) {} + + friend std::ostream& operator << (std::ostream& os, const Label& p); + }; + + + struct EIGEN_ALIGN16 _PointXYZRGBA + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_RGB; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p); + /** \brief A point structure representing Euclidean xyz coordinates, and the RGBA color. + * + * The RGBA information is available either as separate r, g, b, or as a + * packed std::uint32_t rgba value. To pack it, use: + * + * \code + * int rgb = ((int)r) << 16 | ((int)g) << 8 | ((int)b); + * \endcode + * + * To unpack it use: + * + * \code + * int rgb = ...; + * std::uint8_t r = (rgb >> 16) & 0x0000ff; + * std::uint8_t g = (rgb >> 8) & 0x0000ff; + * std::uint8_t b = (rgb) & 0x0000ff; + * \endcode + * + * \ingroup common + */ + struct EIGEN_ALIGN16 PointXYZRGBA : public _PointXYZRGBA + { + inline PointXYZRGBA (const _PointXYZRGBA &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + rgba = p.rgba; + } + + inline PointXYZRGBA (): PointXYZRGBA (0, 0, 0, 0) {} + + inline PointXYZRGBA (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a): + PointXYZRGBA (0.f, 0.f, 0.f, _r, _g, _b, _a) {} + + inline PointXYZRGBA (float _x, float _y, float _z): + PointXYZRGBA (_x, _y, _z, 0, 0, 0, 0) {} + + inline PointXYZRGBA (float _x, float _y, float _z, std::uint8_t _r, + std::uint8_t _g, std::uint8_t _b, std::uint8_t _a) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + r = _r; g = _g; b = _b; a = _a; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p); + }; + + + struct EIGEN_ALIGN16 _PointXYZRGB + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_RGB; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + struct EIGEN_ALIGN16 _PointXYZRGBL + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_RGB; + std::uint32_t label; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZRGB& p); + /** \brief A point structure representing Euclidean xyz coordinates, and the RGB color. + * + * Due to historical reasons (PCL was first developed as a ROS package), the + * RGB information is packed into an integer and casted to a float. This is + * something we wish to remove in the near future, but in the meantime, the + * following code snippet should help you pack and unpack RGB colors in your + * PointXYZRGB structure: + * + * \code + * // pack r/g/b into rgb + * std::uint8_t r = 255, g = 0, b = 0; // Example: Red color + * std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b); + * p.rgb = *reinterpret_cast(&rgb); + * \endcode + * + * To unpack the data into separate values, use: + * + * \code + * PointXYZRGB p; + * // unpack rgb into r/g/b + * std::uint32_t rgb = *reinterpret_cast(&p.rgb); + * std::uint8_t r = (rgb >> 16) & 0x0000ff; + * std::uint8_t g = (rgb >> 8) & 0x0000ff; + * std::uint8_t b = (rgb) & 0x0000ff; + * \endcode + * + * + * Alternatively, from 1.1.0 onwards, you can use p.r, p.g, and p.b directly. + * + * \ingroup common + */ + struct EIGEN_ALIGN16 PointXYZRGB : public _PointXYZRGB + { + inline PointXYZRGB (const _PointXYZRGB &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + rgb = p.rgb; + } + + inline PointXYZRGB (): PointXYZRGB (0.f, 0.f, 0.f) {} + + inline PointXYZRGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGB (0.f, 0.f, 0.f, _r, _g, _b) {} + + inline PointXYZRGB (float _x, float _y, float _z): + PointXYZRGB (_x, _y, _z, 0, 0, 0) {} + + inline PointXYZRGB (float _x, float _y, float _z, + std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + r = _r; g = _g; b = _b; + a = 255; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZRGB& p); + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZRGBL& p); + struct EIGEN_ALIGN16 PointXYZRGBL : public _PointXYZRGBL + { + inline PointXYZRGBL (const _PointXYZRGBL &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + rgba = p.rgba; + label = p.label; + } + + inline PointXYZRGBL (std::uint32_t _label = 0): + PointXYZRGBL (0.f, 0.f, 0.f, 0, 0, 0, _label) {} + + inline PointXYZRGBL (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGBL (0.f, 0.f, 0.f, _r, _g, _b) {} + + inline PointXYZRGBL (float _x, float _y, float _z): + PointXYZRGBL (_x, _y, _z, 0, 0, 0) {} + + inline PointXYZRGBL (float _x, float _y, float _z, + std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, + std::uint32_t _label = 0) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + r = _r; g = _g; b = _b; + a = 255; + label = _label; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZRGBL& p); + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + struct EIGEN_ALIGN16 _PointXYZHSV + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + union + { + struct + { + float h; + float s; + float v; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZHSV& p); + struct EIGEN_ALIGN16 PointXYZHSV : public _PointXYZHSV + { + inline PointXYZHSV (const _PointXYZHSV &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + h = p.h; s = p.s; v = p.v; + } + + inline PointXYZHSV (): PointXYZHSV (0.f, 0.f, 0.f) {} + + // @TODO: Use strong types?? + // This is a dangerous type, doesn't behave like others + inline PointXYZHSV (float _h, float _s, float _v): + PointXYZHSV (0.f, 0.f, 0.f, _h, _s, _v) {} + + inline PointXYZHSV (float _x, float _y, float _z, + float _h, float _s, float _v) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + h = _h; s = _s; v = _v; + data_c[3] = 0; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZHSV& p); + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXY& p); + /** \brief A 2D point structure representing Euclidean xy coordinates. + * \ingroup common + */ + struct PointXY + { + float x = 0.f; + float y = 0.f; + + inline PointXY() = default; + + inline PointXY(float _x, float _y): x(_x), y(_y) {} + + friend std::ostream& operator << (std::ostream& os, const PointXY& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointUV& p); + /** \brief A 2D point structure representing pixel image coordinates. + * \note We use float to be able to represent subpixels. + * \ingroup common + */ + struct PointUV + { + float u = 0.f; + float v = 0.f; + + inline PointUV() = default; + + inline PointUV(float _u, float _v): u(_u), v(_v) {} + + friend std::ostream& operator << (std::ostream& os, const PointUV& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const InterestPoint& p); + /** \brief A point structure representing an interest point with Euclidean xyz coordinates, and an interest value. + * \ingroup common + */ + // @TODO: inheritance trick like on other PointTypes + struct EIGEN_ALIGN16 InterestPoint + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + union + { + struct + { + float strength; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + + friend std::ostream& operator << (std::ostream& os, const InterestPoint& p); + }; + + struct EIGEN_ALIGN16 _Normal + { + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union + { + struct + { + float curvature; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Normal& p); + /** \brief A point structure representing normal coordinates and the surface curvature estimate. (SSE friendly) + * \ingroup common + */ + struct Normal : public _Normal + { + inline Normal (const _Normal &p) + { + normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; + data_n[3] = 0.0f; + curvature = p.curvature; + } + + inline Normal (float _curvature = 0.f): Normal (0.f, 0.f, 0.f, _curvature) {} + + inline Normal (float n_x, float n_y, float n_z, float _curvature = 0.f) + { + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.0f; + curvature = _curvature; + } + + friend std::ostream& operator << (std::ostream& os, const Normal& p); + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + struct EIGEN_ALIGN16 _Axis + { + PCL_ADD_NORMAL4D; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Axis& p); + /** \brief A point structure representing an Axis using its normal coordinates. (SSE friendly) + * \ingroup common + */ + struct EIGEN_ALIGN16 Axis : public _Axis + { + inline Axis (const _Axis &p) + { + normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; + data_n[3] = 0.0f; + } + + inline Axis (): Axis (0.f, 0.f, 0.f) {} + + inline Axis (float n_x, float n_y, float n_z) + { + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.0f; + } + + friend std::ostream& operator << (std::ostream& os, const Axis& p); + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + struct EIGEN_ALIGN16 _PointNormal + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union + { + struct + { + float curvature; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointNormal& p); + /** \brief A point structure representing Euclidean xyz coordinates, together with normal coordinates and the surface curvature estimate. (SSE friendly) + * \ingroup common + */ + struct PointNormal : public _PointNormal + { + inline PointNormal (const _PointNormal &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; + curvature = p.curvature; + } + + inline PointNormal (float _curvature = 0.f): PointNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {} + + inline PointNormal (float _x, float _y, float _z): + PointNormal (_x, _y, _z, 0.f, 0.f, 0.f, 0.f) {} + + inline PointNormal (float _x, float _y, float _z, float n_x, float n_y, float n_z, float _curvature = 0.f) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.0f; + curvature = _curvature; + } + + friend std::ostream& operator << (std::ostream& os, const PointNormal& p); + }; + + + struct EIGEN_ALIGN16 _PointXYZRGBNormal + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union + { + struct + { + PCL_ADD_UNION_RGB; + float curvature; + }; + float data_c[4]; + }; + PCL_ADD_EIGEN_MAPS_RGB; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p); + /** \brief A point structure representing Euclidean xyz coordinates, and the RGB color, together with normal coordinates and the surface curvature estimate. + * Due to historical reasons (PCL was first developed as a ROS package), the + * RGB information is packed into an integer and casted to a float. This is + * something we wish to remove in the near future, but in the meantime, the + * following code snippet should help you pack and unpack RGB colors in your + * PointXYZRGB structure: + * + * \code + * // pack r/g/b into rgb + * std::uint8_t r = 255, g = 0, b = 0; // Example: Red color + * std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b); + * p.rgb = *reinterpret_cast(&rgb); + * \endcode + * + * To unpack the data into separate values, use: + * + * \code + * PointXYZRGB p; + * // unpack rgb into r/g/b + * std::uint32_t rgb = *reinterpret_cast(&p.rgb); + * std::uint8_t r = (rgb >> 16) & 0x0000ff; + * std::uint8_t g = (rgb >> 8) & 0x0000ff; + * std::uint8_t b = (rgb) & 0x0000ff; + * \endcode + * + * + * Alternatively, from 1.1.0 onwards, you can use p.r, p.g, and p.b directly. + * \ingroup common + */ + struct PointXYZRGBNormal : public _PointXYZRGBNormal + { + inline PointXYZRGBNormal (const _PointXYZRGBNormal &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; + curvature = p.curvature; + rgba = p.rgba; + } + + inline PointXYZRGBNormal (float _curvature = 0.f): + PointXYZRGBNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {} + + inline PointXYZRGBNormal (float _x, float _y, float _z): + PointXYZRGBNormal (_x, _y, _z, 0, 0, 0) {} + + inline PointXYZRGBNormal (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGBNormal (0.f, 0.f, 0.f, _r, _g, _b) {} + + inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGBNormal (_x, _y, _z, _r, _g, _b, 0.f, 0.f, 0.f) {} + + inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, + float n_x, float n_y, float n_z, float _curvature = 0.f) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + r = _r; g = _g; b = _b; + a = 255; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.f; + curvature = _curvature; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p); + }; + + struct EIGEN_ALIGN16 _PointXYZINormal + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union + { + struct + { + float intensity; + float curvature; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZINormal& p); + /** \brief A point structure representing Euclidean xyz coordinates, intensity, together with normal coordinates and the surface curvature estimate. + * \ingroup common + */ + struct PointXYZINormal : public _PointXYZINormal + { + inline PointXYZINormal (const _PointXYZINormal &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; + curvature = p.curvature; + intensity = p.intensity; + } + + inline PointXYZINormal (float _intensity = 0.f): PointXYZINormal (0.f, 0.f, 0.f, _intensity) {} + + inline PointXYZINormal (float _x, float _y, float _z, float _intensity = 0.f): + PointXYZINormal (_x, _y, _z, _intensity, 0.f, 0.f, 0.f) {} + + inline PointXYZINormal (float _x, float _y, float _z, float _intensity, + float n_x, float n_y, float n_z, float _curvature = 0.f) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + intensity = _intensity; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.f; + curvature = _curvature; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZINormal& p); + }; + +//---- + struct EIGEN_ALIGN16 _PointXYZLNormal + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union + { + struct + { + std::uint32_t label; + float curvature; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZLNormal& p); + /** \brief A point structure representing Euclidean xyz coordinates, a label, together with normal coordinates and the surface curvature estimate. + * \ingroup common + */ + struct PointXYZLNormal : public _PointXYZLNormal + { + inline PointXYZLNormal (const _PointXYZLNormal &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; + curvature = p.curvature; + label = p.label; + } + + inline PointXYZLNormal (std::uint32_t _label = 0): PointXYZLNormal (0.f, 0.f, 0.f, _label) {} + + inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label = 0.f): + PointXYZLNormal (_x, _y, _z, _label, 0.f, 0.f, 0.f) {} + + inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label, + float n_x, float n_y, float n_z, float _curvature = 0.f) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + label = _label; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.f; + curvature = _curvature; + } + + friend std::ostream& operator << (std::ostream& os, const PointXYZLNormal& p); + }; + +// --- + + + struct EIGEN_ALIGN16 _PointWithRange + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + union + { + struct + { + float range; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointWithRange& p); + /** \brief A point structure representing Euclidean xyz coordinates, padded with an extra range float. + * \ingroup common + */ + struct PointWithRange : public _PointWithRange + { + inline PointWithRange (const _PointWithRange &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + range = p.range; + } + + inline PointWithRange (float _range = 0.f): PointWithRange (0.f, 0.f, 0.f, _range) {} + + inline PointWithRange (float _x, float _y, float _z, float _range = 0.f) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + range = _range; + } + + friend std::ostream& operator << (std::ostream& os, const PointWithRange& p); + }; + + + struct EIGEN_ALIGN16 _PointWithViewpoint + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + union + { + struct + { + float vp_x; + float vp_y; + float vp_z; + }; + float data_c[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p); + /** \brief A point structure representing Euclidean xyz coordinates together with the viewpoint from which it was seen. + * \ingroup common + */ + struct EIGEN_ALIGN16 PointWithViewpoint : public _PointWithViewpoint + { + inline PointWithViewpoint (const _PointWithViewpoint &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + vp_x = p.vp_x; vp_y = p.vp_y; vp_z = p.vp_z; + } + + inline PointWithViewpoint (): PointWithViewpoint (0.f, 0.f, 0.f) {} + + PCL_DEPRECATED("Use ctor accepting all position (x, y, z) data") + inline PointWithViewpoint (float _x, float _y = 0.f): + PointWithViewpoint (_x, _y, 0.f) {} + + PCL_DEPRECATED("Use ctor accepting all viewpoint (vp_x, vp_y, vp_z) data") + inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y = 0.f): + PointWithViewpoint (_x, _y, _z, _vp_x, _vp_y, 0.f) {} + + inline PointWithViewpoint (float _x, float _y, float _z): PointWithViewpoint (_x, _y, _z, 0.f, 0.f, 0.f) {} + + inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y, float _vp_z) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + vp_x = _vp_x; vp_y = _vp_y; vp_z = _vp_z; + } + + friend std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const MomentInvariants& p); + /** \brief A point structure representing the three moment invariants. + * \ingroup common + */ + struct MomentInvariants + { + float j1 = 0.f, j2 = 0.f, j3 = 0.f; + + inline MomentInvariants () = default; + + inline MomentInvariants (float _j1, float _j2, float _j3): j1 (_j1), j2 (_j2), j3 (_j3) {} + + friend std::ostream& operator << (std::ostream& os, const MomentInvariants& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p); + /** \brief A point structure representing the minimum and maximum surface radii (in meters) computed using RSD. + * \ingroup common + */ + struct PrincipalRadiiRSD + { + float r_min = 0.f, r_max = 0.f; + + inline PrincipalRadiiRSD () = default; + + inline PrincipalRadiiRSD (float _r_min, float _r_max): r_min (_r_min), r_max (_r_max) {} + + friend std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Boundary& p); + /** \brief A point structure representing a description of whether a point is lying on a surface boundary or not. + * \ingroup common + */ + struct Boundary + { + std::uint8_t boundary_point = 0; + +#if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 + operator unsigned char() const + { + return boundary_point; + } +#endif + + inline Boundary (std::uint8_t _boundary = 0): boundary_point (_boundary) {} + + friend std::ostream& operator << (std::ostream& os, const Boundary& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p); + /** \brief A point structure representing the principal curvatures and their magnitudes. + * \ingroup common + */ + struct PrincipalCurvatures + { + union + { + float principal_curvature[3]; + struct + { + float principal_curvature_x; + float principal_curvature_y; + float principal_curvature_z; + }; + }; + float pc1 = 0.f; + float pc2 = 0.f; + + inline PrincipalCurvatures (): PrincipalCurvatures (0.f, 0.f) {} + + inline PrincipalCurvatures (float _pc1, float _pc2): PrincipalCurvatures (0.f, 0.f, 0.f, _pc1, _pc2) {} + + inline PrincipalCurvatures (float _x, float _y, float _z): PrincipalCurvatures (_x, _y, _z, 0.f, 0.f) {} + + inline PrincipalCurvatures (float _x, float _y, float _z, float _pc1, float _pc2): + principal_curvature_x (_x), principal_curvature_y (_y), principal_curvature_z (_z), pc1 (_pc1), pc2 (_pc2) {} + + friend std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PFHSignature125& p); + /** \brief A point structure representing the Point Feature Histogram (PFH). + * \ingroup common + */ + struct PFHSignature125 + { + float histogram[125] = {0.f}; + static int descriptorSize () { return 125; } + + inline PFHSignature125 () = default; + + friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p); + /** \brief A point structure representing the Point Feature Histogram with colors (PFHRGB). + * \ingroup common + */ + struct PFHRGBSignature250 + { + float histogram[250] = {0.f}; + static int descriptorSize () { return 250; } + + inline PFHRGBSignature250 () = default; + + friend std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PPFSignature& p); + /** \brief A point structure for storing the Point Pair Feature (PPF) values + * \ingroup common + */ + struct PPFSignature + { + float f1 = 0.f, f2 = 0.f, f3 = 0.f, f4 = 0.f; + float alpha_m = 0.f; + + inline PPFSignature (float _alpha = 0.f): PPFSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} + + inline PPFSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): + f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), alpha_m (_alpha) {} + + friend std::ostream& operator << (std::ostream& os, const PPFSignature& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const CPPFSignature& p); + /** \brief A point structure for storing the Point Pair Feature (CPPF) values + * \ingroup common + */ + struct CPPFSignature + { + float f1, f2, f3, f4, f5, f6, f7, f8, f9, f10; + float alpha_m; + + inline CPPFSignature (float _alpha = 0.f): + CPPFSignature (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _alpha) {} + + inline CPPFSignature (float _f1, float _f2, float _f3, float _f4, float _f5, float _f6, + float _f7, float _f8, float _f9, float _f10, float _alpha = 0.f): + f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), f5 (_f5), f6 (_f6), + f7 (_f7), f8 (_f8), f9 (_f9), f10 (_f10), alpha_m (_alpha) {} + + friend std::ostream& operator << (std::ostream& os, const CPPFSignature& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p); + /** \brief A point structure for storing the Point Pair Color Feature (PPFRGB) values + * \ingroup common + */ + struct PPFRGBSignature + { + float f1 = 0.f, f2 = 0.f, f3 = 0.f, f4 = 0.f; + float r_ratio = 0.f, g_ratio = 0.f, b_ratio = 0.f; + float alpha_m = 0.f; + + inline PPFRGBSignature (float _alpha = 0.f): PPFRGBSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} + + inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): + PPFRGBSignature (_f1, _f2, _f3, _f4, _alpha, 0.f, 0.f, 0.f) {} + + inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha, float _r, float _g, float _b): + f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), r_ratio (_r), g_ratio (_g), b_ratio (_b), alpha_m (_alpha) {} + + friend std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const NormalBasedSignature12& p); + /** \brief A point structure representing the Normal Based Signature for + * a feature matrix of 4-by-3 + * \ingroup common + */ + struct NormalBasedSignature12 + { + float values[12] = {0.f}; + + inline NormalBasedSignature12 () = default; + + friend std::ostream& operator << (std::ostream& os, const NormalBasedSignature12& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const ShapeContext1980& p); + /** \brief A point structure representing a Shape Context. + * \ingroup common + */ + struct ShapeContext1980 + { + float descriptor[1980] = {0.f}; + float rf[9] = {0.f}; + static int descriptorSize () { return 1980; } + + inline ShapeContext1980 () = default; + + friend std::ostream& operator << (std::ostream& os, const ShapeContext1980& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const UniqueShapeContext1960& p); + /** \brief A point structure representing a Unique Shape Context. + * \ingroup common + */ + struct UniqueShapeContext1960 + { + float descriptor[1960] = {0.f}; + float rf[9] = {0.f}; + static int descriptorSize () { return 1960; } + + inline UniqueShapeContext1960 () = default; + + friend std::ostream& operator << (std::ostream& os, const UniqueShapeContext1960& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const SHOT352& p); + /** \brief A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape only. + * \ingroup common + */ + struct SHOT352 + { + float descriptor[352] = {0.f}; + float rf[9] = {0.f}; + static int descriptorSize () { return 352; } + + inline SHOT352 () = default; + + friend std::ostream& operator << (std::ostream& os, const SHOT352& p); + }; + + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const SHOT1344& p); + /** \brief A point structure representing the generic Signature of Histograms of OrienTations (SHOT) - shape+color. + * \ingroup common + */ + struct SHOT1344 + { + float descriptor[1344] = {0.f}; + float rf[9] = {0.f}; + static int descriptorSize () { return 1344; } + + inline SHOT1344 () = default; + + friend std::ostream& operator << (std::ostream& os, const SHOT1344& p); + }; + + + /** \brief A structure representing the Local Reference Frame of a point. + * \ingroup common + */ + struct EIGEN_ALIGN16 _ReferenceFrame + { + union + { + float rf[9]; + struct + { + float x_axis[3]; + float y_axis[3]; + float z_axis[3]; + }; + }; + + inline Eigen::Map getXAxisVector3fMap () { return (Eigen::Vector3f::Map (x_axis)); } + inline const Eigen::Map getXAxisVector3fMap () const { return (Eigen::Vector3f::Map (x_axis)); } + inline Eigen::Map getYAxisVector3fMap () { return (Eigen::Vector3f::Map (y_axis)); } + inline const Eigen::Map getYAxisVector3fMap () const { return (Eigen::Vector3f::Map (y_axis)); } + inline Eigen::Map getZAxisVector3fMap () { return (Eigen::Vector3f::Map (z_axis)); } + inline const Eigen::Map getZAxisVector3fMap () const { return (Eigen::Vector3f::Map (z_axis)); } + inline Eigen::Map getMatrix3fMap () { return (Eigen::Matrix3f::Map (rf)); } + inline const Eigen::Map getMatrix3fMap () const { return (Eigen::Matrix3f::Map (rf)); } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const ReferenceFrame& p); + struct EIGEN_ALIGN16 ReferenceFrame : public _ReferenceFrame + { + inline ReferenceFrame (const _ReferenceFrame &p) + { + std::copy_n(p.rf, 9, rf); + } + + inline ReferenceFrame () + { + std::fill_n(x_axis, 3, 0); + std::fill_n(y_axis, 3, 0); + std::fill_n(z_axis, 3, 0); + } + + // @TODO: add other ctors + + friend std::ostream& operator << (std::ostream& os, const ReferenceFrame& p); + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const FPFHSignature33& p); + /** \brief A point structure representing the Fast Point Feature Histogram (FPFH). + * \ingroup common + */ + struct FPFHSignature33 + { + float histogram[33] = {0.f}; + static int descriptorSize () { return 33; } + + inline FPFHSignature33 () = default; + + friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const VFHSignature308& p); + /** \brief A point structure representing the Viewpoint Feature Histogram (VFH). + * \ingroup common + */ + struct VFHSignature308 + { + float histogram[308] = {0.f}; + static int descriptorSize () { return 308; } + + inline VFHSignature308 () = default; + + friend std::ostream& operator << (std::ostream& os, const VFHSignature308& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GRSDSignature21& p); + /** \brief A point structure representing the Global Radius-based Surface Descriptor (GRSD). + * \ingroup common + */ + struct GRSDSignature21 + { + float histogram[21] = {0.f}; + static int descriptorSize () { return 21; } + + inline GRSDSignature21 () = default; + + friend std::ostream& operator << (std::ostream& os, const GRSDSignature21& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const BRISKSignature512& p); + /** \brief A point structure representing the Binary Robust Invariant Scalable Keypoints (BRISK). + * \ingroup common + */ + struct BRISKSignature512 + { + float scale = 0.f; + float orientation = 0.f; + unsigned char descriptor[64] = {0}; + static int descriptorSize () { return 64; } + + inline BRISKSignature512 () = default; + + inline BRISKSignature512 (float _scale, float _orientation): scale (_scale), orientation (_orientation) {} + + friend std::ostream& operator << (std::ostream& os, const BRISKSignature512& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const ESFSignature640& p); + /** \brief A point structure representing the Ensemble of Shape Functions (ESF). + * \ingroup common + */ + struct ESFSignature640 + { + float histogram[640] = {0.f}; + static int descriptorSize () { return 640; } + + inline ESFSignature640 () = default; + + friend std::ostream& operator << (std::ostream& os, const ESFSignature640& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GASDSignature512& p); + /** \brief A point structure representing the Globally Aligned Spatial Distribution (GASD) shape descriptor. + * \ingroup common + */ + struct GASDSignature512 + { + float histogram[512] = {0.f}; + static int descriptorSize() { return 512; } + + inline GASDSignature512 () = default; + + friend std::ostream& operator << (std::ostream& os, const GASDSignature512& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GASDSignature984& p); + /** \brief A point structure representing the Globally Aligned Spatial Distribution (GASD) shape and color descriptor. + * \ingroup common + */ + struct GASDSignature984 + { + float histogram[984] = {0.f}; + static int descriptorSize() { return 984; } + + inline GASDSignature984 () = default; + + friend std::ostream& operator << (std::ostream& os, const GASDSignature984& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GASDSignature7992& p); + /** \brief A point structure representing the Globally Aligned Spatial Distribution (GASD) shape and color descriptor. + * \ingroup common + */ + struct GASDSignature7992 + { + float histogram[7992] = {0.f}; + static int descriptorSize() { return 7992; } + + inline GASDSignature7992 () = default; + + friend std::ostream& operator << (std::ostream& os, const GASDSignature7992& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GFPFHSignature16& p); + /** \brief A point structure representing the GFPFH descriptor with 16 bins. + * \ingroup common + */ + struct GFPFHSignature16 + { + float histogram[16] = {0.f}; + static int descriptorSize () { return 16; } + + inline GFPFHSignature16 () = default; + + friend std::ostream& operator << (std::ostream& os, const GFPFHSignature16& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Narf36& p); + /** \brief A point structure representing the Narf descriptor. + * \ingroup common + */ + struct Narf36 + { + float x = 0.f, y = 0.f, z = 0.f, roll = 0.f, pitch = 0.f, yaw = 0.f; + float descriptor[36] = {0.f}; + static int descriptorSize () { return 36; } + + inline Narf36 () = default; + + inline Narf36 (float _x, float _y, float _z): Narf36 (_x, _y, _z, 0.f, 0.f, 0.f) {} + + inline Narf36 (float _x, float _y, float _z, float _roll, float _pitch, float _yaw): + x (_x), y (_y), z (_z), roll (_roll), pitch (_pitch), yaw (_yaw) {} + + friend std::ostream& operator << (std::ostream& os, const Narf36& p); + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const BorderDescription& p); + /** \brief A structure to store if a point in a range image lies on a border between an obstacle and the background. + * \ingroup common + */ + struct BorderDescription + { + int x = 0.f, y = 0.f; + BorderTraits traits; + //std::vector neighbors; + + inline BorderDescription () = default; + + // TODO: provide other ctors + + friend std::ostream& operator << (std::ostream& os, const BorderDescription& p); + }; + + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const IntensityGradient& p); + /** \brief A point structure representing the intensity gradient of an XYZI point cloud. + * \ingroup common + */ + struct IntensityGradient + { + union + { + float gradient[3]; + struct + { + float gradient_x; + float gradient_y; + float gradient_z; + }; + }; + + inline IntensityGradient (): IntensityGradient (0.f, 0.f, 0.f) {} + + inline IntensityGradient (float _x, float _y, float _z): gradient_x (_x), gradient_y (_y), gradient_z (_z) {} + + friend std::ostream& operator << (std::ostream& os, const IntensityGradient& p); + }; + + // TODO: Maybe make other histogram based structs an alias for this + /** \brief A point structure representing an N-D histogram. + * \ingroup common + */ + template + struct Histogram + { + float histogram[N]; + static int descriptorSize () { return N; } + }; + + struct EIGEN_ALIGN16 _PointWithScale + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + + union + { + /** \brief Diameter of the meaningful keypoint neighborhood. */ + float scale; + float size; + }; + + /** \brief Computed orientation of the keypoint (-1 if not applicable). */ + float angle; + /** \brief The response by which the most strong keypoints have been selected. */ + float response; + /** \brief octave (pyramid layer) from which the keypoint has been extracted. */ + int octave; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointWithScale& p); + /** \brief A point structure representing a 3-D position and scale. + * \ingroup common + */ + struct PointWithScale : public _PointWithScale + { + inline PointWithScale (const _PointWithScale &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + scale = p.scale; + angle = p.angle; + response = p.response; + octave = p.octave; + } + + inline PointWithScale (): PointWithScale (0.f, 0.f, 0.f) {} + + inline PointWithScale (float _x, float _y, float _z, float _scale = 1.f, + float _angle = -1.f, float _response = 0.f, int _octave = 0) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + scale = _scale; + angle = _angle; + response = _response; + octave = _octave; + } + + friend std::ostream& operator << (std::ostream& os, const PointWithScale& p); + }; + + + struct EIGEN_ALIGN16 _PointSurfel + { + PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union + { + struct + { + PCL_ADD_UNION_RGB; + float radius; + float confidence; + float curvature; + }; + float data_c[4]; + }; + PCL_ADD_EIGEN_MAPS_RGB; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointSurfel& p); + /** \brief A surfel, that is, a point structure representing Euclidean xyz coordinates, together with normal coordinates, a RGBA color, a radius, a confidence value and the surface curvature estimate. + * \ingroup common + */ + struct PointSurfel : public _PointSurfel + { + inline PointSurfel (const _PointSurfel &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + rgba = p.rgba; + radius = p.radius; + confidence = p.confidence; + curvature = p.curvature; + } + + inline PointSurfel () + { + x = y = z = 0.0f; + data[3] = 1.0f; + normal_x = normal_y = normal_z = data_n[3] = 0.0f; + r = g = b = 0; + a = 255; + radius = confidence = curvature = 0.0f; + } + + // TODO: add other ctor to PointSurfel + + friend std::ostream& operator << (std::ostream& os, const PointSurfel& p); + }; + + struct EIGEN_ALIGN16 _PointDEM + { + PCL_ADD_POINT4D; + float intensity; + float intensity_variance; + float height_variance; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointDEM& p); + /** \brief A point structure representing Digital Elevation Map. + * \ingroup common + */ + struct PointDEM : public _PointDEM + { + inline PointDEM (const _PointDEM &p) + { + x = p.x; y = p.y; z = p.z; data[3] = 1.0f; + intensity = p.intensity; + intensity_variance = p.intensity_variance; + height_variance = p.height_variance; + } + + inline PointDEM (): PointDEM (0.f, 0.f, 0.f) {} + + inline PointDEM (float _x, float _y, float _z): PointDEM (_x, _y, _z, 0.f, 0.f, 0.f) {} + + inline PointDEM (float _x, float _y, float _z, float _intensity, + float _intensity_variance, float _height_variance) + { + x = _x; y = _y; z = _z; + data[3] = 1.0f; + intensity = _intensity; + intensity_variance = _intensity_variance; + height_variance = _height_variance; + } + + friend std::ostream& operator << (std::ostream& os, const PointDEM& p); + }; + + template std::ostream& + operator << (std::ostream& os, const Histogram& p) + { + // make constexpr + if (N > 0) + { + os << "(" << p.histogram[0]; + std::for_each(p.histogram + 1, std::end(p.histogram), + [&os](const auto& hist) { os << ", " << hist; }); + os << ")"; + } + return (os); + } +} // End namespace + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/ascii_io.h b/deps_install/include/pcl-1.10/pcl/io/ascii_io.h new file mode 100755 index 0000000..c7ebf40 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/ascii_io.h @@ -0,0 +1,167 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + + +namespace pcl +{ + /** \brief Ascii Point Cloud Reader. + * Read any ASCII file by setting the separating characters and input point fields. + * + * \author Adam Stambler (adasta@gmail.com) + * \ingroup io + */ + class PCL_EXPORTS ASCIIReader : public FileReader + { + public: + ASCIIReader (); + ~ASCIIReader (); + using FileReader::read; + + /* Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given FILE file. Useful for fast + * evaluation of the underlying data structure. + * + * Returns: + * * < 0 (-1) on error + * * > 0 on success + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only the header will be filled) + * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present) + * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7) + * \param[out] data_type the type of data (binary data=1, ascii=0, etc) + * \param[out] data_idx the offset of cloud data within the file + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) override ; + + + /** \brief Read a point cloud data from a FILE file and store it into a pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present) + * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7) + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version, + const int offset = 0) override; + + /** \brief Set the ascii file point fields. + */ + template + void setInputFields (); + + /** \brief Set the ascii file point fields using a list of fields. + * \param[in] fields is a list of point fields, in order, in the input ascii file + */ + void + setInputFields (const std::vector& fields); + + + /** \brief Set the ascii file point fields using a point type. + * \param[in] p a point type + */ + template + PCL_DEPRECATED("use parameterless setInputFields() instead") + inline void setInputFields (const PointT p) + { + (void) p; + setInputFields (); + } + + + /** \brief Set the Separating characters for the ascii point fields 2. + * \param[in] chars string of separating characters + * Sets the separating characters for the point fields. The + * default separating characters are " \n\t," + */ + void + setSepChars (const std::string &chars); + + /** \brief Set the extension of the ascii point file type. + * \param[in] ext extension (example : ".txt" or ".xyz" ) + */ + void + setExtension (const std::string &ext) { extension_ = ext; } + + protected: + std::string sep_chars_; + std::string extension_; + std::vector fields_; + std::string name_; + + + /** \brief Parses token based on field type. + * \param[in] token string representation of point fields + * \param[in] field token point field type + * \param[out] data_target address that the point field data should be assigned to + * returns the size of the parsed point field in bytes + */ + int + parse (const std::string& token, const pcl::PCLPointField& field, std::uint8_t* data_target); + + /** \brief Returns the size in bytes of a point field type. + * \param[in] type point field type + * returns the size of the type in bytes + */ + std::uint32_t + typeSize (int type); + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/io/auto_io.h b/deps_install/include/pcl-1.10/pcl/io/auto_io.h new file mode 100755 index 0000000..2b04f66 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/auto_io.h @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + + namespace io + { + /** \brief Load a file into a PointCloud2 according to extension. + * \param[in] file_name the name of the file to load + * \param[out] blob the resultant pcl::PointCloud2 blob + * \ingroup io + */ + PCL_EXPORTS int + load (const std::string& file_name, pcl::PCLPointCloud2& blob); + + /** \brief Load a file into a template PointCloud type according to extension. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + template int + load (const std::string& file_name, pcl::PointCloud& cloud); + + /** \brief Load a file into a PolygonMesh according to extension. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant pcl::PolygonMesh + * \ingroup io + */ + PCL_EXPORTS int + load (const std::string& file_name, pcl::PolygonMesh& mesh); + + /** \brief Load a file into a TextureMesh according to extension. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant pcl::TextureMesh + * \ingroup io + */ + PCL_EXPORTS int + load (const std::string& file_name, pcl::TextureMesh& mesh); + + /** \brief Save point cloud data to a binary file when available else to ASCII. + * \param[in] file_name the output file name + * \param[in] blob the point cloud data message + * \param[in] precision float precision when saving to ASCII files + * \ingroup io + */ + PCL_EXPORTS int + save (const std::string& file_name, const pcl::PCLPointCloud2& blob, unsigned precision = 5); + + /** \brief Save point cloud to a binary file when available else to ASCII. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * \ingroup io + */ + template int + save (const std::string& file_name, const pcl::PointCloud& cloud); + + /** \brief Saves a TextureMesh to a binary file when available else to ASCII. + * \param[in] file_name the name of the file to write to disk + * \param[in] tex_mesh the texture mesh to save + * \param[in] precision float precision when saving to ASCII files + * \ingroup io + */ + PCL_EXPORTS int + save (const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision = 5); + + /** \brief Saves a PolygonMesh to a binary file when available else to ASCII. + * \param[in] file_name the name of the file to write to disk + * \param[in] mesh the polygonal mesh to save + * \param[in] precision float precision when saving to ASCII files + * \ingroup io + */ + PCL_EXPORTS int + save (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5); + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/io/boost.h b/deps_install/include/pcl-1.10/pcl/io/boost.h new file mode 100755 index 0000000..395f194 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/boost.h @@ -0,0 +1,69 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif +//https://bugreports.qt-project.org/browse/QTBUG-22829 +#ifndef Q_MOC_RUN +#ifndef __CUDACC__ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define BOOST_PARAMETER_MAX_ARITY 7 +#include +#include +#endif +#include +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/debayer.h b/deps_install/include/pcl-1.10/pcl/io/debayer.h new file mode 100755 index 0000000..f118c5c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/debayer.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace io + { + /** \brief Various debayering methods. + * \author Suat Gedikli + * \ingroup io + */ + class PCL_EXPORTS DeBayer + { + public: + // Debayering methods + void + debayerBilinear ( + const unsigned char *bayer_pixel, unsigned char *rgb_buffer, + unsigned width, unsigned height, + int bayer_line_step = 0, + int bayer_line_step2 = 0, + unsigned rgb_line_step = 0) const; + + void + debayerEdgeAware ( + const unsigned char *bayer_pixel, unsigned char *rgb_buffer, + unsigned width, unsigned height, + int bayer_line_step = 0, + int bayer_line_step2 = 0, + unsigned rgb_line_step = 0) const; + + void + debayerEdgeAwareWeighted ( + const unsigned char *bayer_pixel, unsigned char *rgb_buffer, + unsigned width, unsigned height, + int bayer_line_step = 0, + int bayer_line_step2 = 0, + unsigned rgb_line_step = 0) const; + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/dinast_grabber.h b/deps_install/include/pcl-1.10/pcl/io/dinast_grabber.h new file mode 100755 index 0000000..7ee5cad --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/dinast_grabber.h @@ -0,0 +1,217 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace pcl +{ + /** \brief Grabber for DINAST devices (i.e., IPA-1002, IPA-1110, IPA-2001) + * \author Marco A. Gutierrez + * \ingroup io + */ + class PCL_EXPORTS DinastGrabber: public Grabber + { + // Define callback signature typedefs + using sig_cb_dinast_point_cloud = void (const pcl::PointCloud::ConstPtr &); + + public: + /** \brief Constructor that sets up the grabber constants. + * \param[in] device_position Number corresponding the device to grab + */ + DinastGrabber (const int device_position=1); + + /** \brief Destructor. It never throws. */ + ~DinastGrabber () noexcept; + + /** \brief Check if the grabber is running + * \return true if grabber is running / streaming. False otherwise. + */ + bool + isRunning () const override; + + /** \brief Returns the name of the concrete subclass, DinastGrabber. + * \return DinastGrabber. + */ + std::string + getName () const override + { return (std::string ("DinastGrabber")); } + + /** \brief Start the data acquisition process. + */ + void + start () override; + + /** \brief Stop the data acquisition process. + */ + void + stop () override; + + /** \brief Obtain the number of frames per second (FPS). */ + float + getFramesPerSecond () const override; + + /** \brief Get the version number of the currently opened device + */ + std::string + getDeviceVersion (); + + protected: + + /** \brief On initialization processing. */ + void + onInit (const int device_id); + + /** \brief Setup a Dinast 3D camera device + * \param[in] device_position Number corresponding the device to grab + * \param[in] id_vendor The ID of the camera vendor (should be 0x18d1) + * \param[in] id_product The ID of the product (should be 0x1402) + */ + void + setupDevice (int device_position, + const int id_vendor = 0x18d1, + const int id_product = 0x1402); + + /** \brief Send a RX data packet request + * \param[in] req_code the request to send (the request field for the setup packet) + * \param buffer + * \param[in] length the length field for the setup packet. The data buffer should be at least this size. + */ + bool + USBRxControlData (const unsigned char req_code, + unsigned char *buffer, + int length); + + /** \brief Send a TX data packet request + * \param[in] req_code the request to send (the request field for the setup packet) + * \param buffer + * \param[in] length the length field for the setup packet. The data buffer should be at least this size. + */ + bool + USBTxControlData (const unsigned char req_code, + unsigned char *buffer, + int length); + + /** \brief Check if we have a header in the global buffer, and return the position of the next valid image. + * \note If the image in the buffer is partial, return -1, as we have to wait until we add more data to it. + * \return the position of the next valid image (i.e., right after a valid header) or -1 in case the buffer + * either doesn't have an image or has a partial image + */ + int + checkHeader (); + + /** \brief Read image data and leaves it on image_ + */ + void + readImage (); + + /** \brief Obtains XYZI Point Cloud from the image of the camera + * \return the point cloud from the image data + */ + pcl::PointCloud::Ptr + getXYZIPointCloud (); + + /** \brief The function in charge of getting the data from the camera + */ + void + captureThreadFunction (); + + /** \brief Width of image */ + int image_width_; + + /** \brief Height of image */ + int image_height_; + + /** \brief Total size of image */ + int image_size_; + + /** \brief Length of a sync packet */ + int sync_packet_size_; + + double dist_max_2d_; + + /** \brief diagonal Field of View*/ + double fov_; + + /** \brief Size of pixel */ + enum pixel_size { RAW8=1, RGB16=2, RGB24=3, RGB32=4 }; + + /** \brief The libusb context*/ + libusb_context *context_; + + /** \brief the actual device_handle for the camera */ + struct libusb_device_handle *device_handle_; + + /** \brief Temporary USB read buffer, since we read two RGB16 images at a time size is the double of two images + * plus a sync packet. + */ + unsigned char *raw_buffer_ ; + + /** \brief Global circular buffer */ + boost::circular_buffer g_buffer_; + + /** \brief Bulk endpoint address value */ + unsigned char bulk_ep_; + + /** \brief Device command values */ + enum { CMD_READ_START=0xC7, CMD_READ_STOP=0xC8, CMD_GET_VERSION=0xDC, CMD_SEND_DATA=0xDE }; + + unsigned char *image_; + + /** \brief Since there is no header after the first image, we need to save the state */ + bool second_image_; + + bool running_; + + std::thread capture_thread_; + + mutable std::mutex capture_mutex_; + boost::signals2::signal* point_cloud_signal_; + }; +} //namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/io/eigen.h b/deps_install/include/pcl-1.10/pcl/io/eigen.h new file mode 100755 index 0000000..12d234a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/eigen.h @@ -0,0 +1,44 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include diff --git a/deps_install/include/pcl-1.10/pcl/io/file_grabber.h b/deps_install/include/pcl-1.10/pcl/io/file_grabber.h new file mode 100755 index 0000000..400e2b9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/file_grabber.h @@ -0,0 +1,85 @@ + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: file_grabber.h 8413 2013-01-15 08:37:39Z sdmiller $ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief FileGrabber provides a container-style interface for grabbers which operate on fixed-size input + * \author Stephen Miller + * \ingroup io + */ + template + class PCL_EXPORTS FileGrabber + { + public: + + /** \brief Empty destructor */ + virtual ~FileGrabber () {} + + /** \brief operator[] Returns the idx-th cloud in the dataset, without bounds checking. + * Note that in the future, this could easily be modified to do caching + * \param[in] idx The frame to load + */ + virtual const typename pcl::PointCloud::ConstPtr + operator[] (std::size_t idx) const = 0; + + /** \brief size Returns the number of clouds currently loaded by the grabber */ + virtual std::size_t + size () const = 0; + + /** \brief at Returns the idx-th cloud in the dataset, with bounds checking + * \param[in] idx The frame to load + */ + virtual const typename pcl::PointCloud::ConstPtr + at (std::size_t idx) const + { + if (idx >= size ()) + { + // Throw error + throw pcl::IOException ("[pcl::FileGrabber] Attempted to access element which is out of bounds!"); + } + return (operator[] (idx)); + } + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/io/file_io.h b/deps_install/include/pcl-1.10/pcl/io/file_io.h new file mode 100755 index 0000000..9dce0af --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/file_io.h @@ -0,0 +1,413 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: file_io.h 827 2011-05-04 02:00:04Z nizar $ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Point Cloud Data (FILE) file format reader interface. + * Any (FILE) format file reader should implement its virtual methods. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS FileReader + { + public: + /** \brief empty constructor */ + FileReader() {} + /** \brief empty destructor */ + virtual ~FileReader() {} + /** \brief Read a point cloud data header from a FILE file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given FILE file. Useful for fast + * evaluation of the underlying data structure. + * + * Returns: + * * < 0 (-1) on error + * * > 0 on success + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only the header will be filled) + * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present) + * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7) + * \param[out] data_type the type of data (binary data=1, ascii=0, etc) + * \param[out] data_idx the offset of cloud data within the file + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + virtual int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, int &data_type, unsigned int &data_idx, const int offset = 0) = 0; + + /** \brief Read a point cloud data from a FILE file and store it into a pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] origin the sensor acquisition origin (only for > FILE_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > FILE_V7 - identity if not present) + * \param[out] file_version the FILE version of the file (either FILE_V6 or FILE_V7) + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + virtual int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &file_version, + const int offset = 0) = 0; + + /** \brief Read a point cloud data from a FILE file (FILE_V6 only!) and store it into a pcl/PCLPointCloud2. + * + * \note This function is provided for backwards compatibility only and + * it can only read FILE_V6 files correctly, as pcl::PCLPointCloud2 + * does not contain a sensor origin/orientation. Reading any file + * > FILE_V6 will generate a warning. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0) + { + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + int file_version; + return (read (file_name, cloud, origin, orientation, file_version, offset)); + } + + /** \brief Read a point cloud data from any FILE file, and convert it to the given template format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + template inline int + read (const std::string &file_name, pcl::PointCloud &cloud, const int offset =0) + { + pcl::PCLPointCloud2 blob; + int file_version; + int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, + file_version, offset); + + // Exit in case of error + if (res < 0) + return res; + pcl::fromPCLPointCloud2 (blob, cloud); + return (0); + } + }; + + /** \brief Point Cloud Data (FILE) file format writer. + * Any (FILE) format file reader should implement its virtual methods + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS FileWriter + { + public: + /** \brief Empty constructor */ + FileWriter () {} + + /** \brief Empty destructor */ + virtual ~FileWriter () {} + + /** \brief Save point cloud data to a FILE file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \param[in] binary set to true if the file is to be written in a binary + * FILE format, false (default) for ASCII + */ + virtual int + write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary = false) = 0; + + /** \brief Save point cloud data to a FILE file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message (boost shared pointer) + * \param[in] binary set to true if the file is to be written in a binary + * FILE format, false (default) for ASCII + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + */ + inline int + write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary = false) + { + return (write (file_name, *cloud, origin, orientation, binary)); + } + + /** \brief Save point cloud data to a FILE file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the pcl::PointCloud data + * \param[in] binary set to true if the file is to be written in a binary + * FILE format, false (default) for ASCII + */ + template inline int + write (const std::string &file_name, + const pcl::PointCloud &cloud, + const bool binary = false) + { + Eigen::Vector4f origin = cloud.sensor_origin_; + Eigen::Quaternionf orientation = cloud.sensor_orientation_; + + pcl::PCLPointCloud2 blob; + pcl::toPCLPointCloud2 (cloud, blob); + + // Save the data + return (write (file_name, blob, origin, orientation, binary)); + } + }; + + /** \brief inserts a value of type Type (uchar, char, uint, int, float, double, ...) into a stringstream. + * + * If the value is NaN, it inserts "nan". + * + * \param[in] cloud the cloud to copy from + * \param[in] point_index the index of the point + * \param[in] point_size the size of the point in the cloud + * \param[in] field_idx the index of the dimension/field + * \param[in] fields_count the current fields count + * \param[out] stream the ostringstream to copy into + */ + template inline + std::enable_if_t::value> + copyValueString (const pcl::PCLPointCloud2 &cloud, + const unsigned int point_index, + const int point_size, + const unsigned int field_idx, + const unsigned int fields_count, + std::ostream &stream) + { + Type value; + memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type)); + if (std::isnan (value)) + stream << "nan"; + else + stream << value; + } + + template inline + std::enable_if_t::value> + copyValueString (const pcl::PCLPointCloud2 &cloud, + const unsigned int point_index, + const int point_size, + const unsigned int field_idx, + const unsigned int fields_count, + std::ostream &stream) + { + Type value; + memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type)); + stream << value; + } + + template <> inline void + copyValueString (const pcl::PCLPointCloud2 &cloud, + const unsigned int point_index, + const int point_size, + const unsigned int field_idx, + const unsigned int fields_count, + std::ostream &stream) + { + std::int8_t value; + memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (std::int8_t)], sizeof (std::int8_t)); + //Cast to int to prevent value is handled as char + stream << boost::numeric_cast(value); + } + + template <> inline void + copyValueString (const pcl::PCLPointCloud2 &cloud, + const unsigned int point_index, + const int point_size, + const unsigned int field_idx, + const unsigned int fields_count, + std::ostream &stream) + { + std::uint8_t value; + memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (std::uint8_t)], sizeof (std::uint8_t)); + //Cast to unsigned int to prevent value is handled as char + stream << boost::numeric_cast(value); + } + + /** \brief Check whether a given value of type Type (uchar, char, uint, int, float, double, ...) is finite or not + * + * \param[in] cloud the cloud that contains the data + * \param[in] point_index the index of the point + * \param[in] point_size the size of the point in the cloud + * \param[in] field_idx the index of the dimension/field + * \param[in] fields_count the current fields count + * + * \return true if the value is finite, false otherwise + */ + template inline + std::enable_if_t::value, bool> + isValueFinite (const pcl::PCLPointCloud2 &cloud, + const unsigned int point_index, + const int point_size, + const unsigned int field_idx, + const unsigned int fields_count) + { + Type value; + memcpy (&value, &cloud.data[point_index * point_size + cloud.fields[field_idx].offset + fields_count * sizeof (Type)], sizeof (Type)); + return std::isfinite (value); + } + + template inline + std::enable_if_t::value, bool> + isValueFinite (const pcl::PCLPointCloud2& /* cloud */, + const unsigned int /* point_index */, + const int /* point_size */, + const unsigned int /* field_idx */, + const unsigned int /* fields_count */) + { + return true; + } + + /** \brief Copy one single value of type T (uchar, char, uint, int, float, double, ...) from a string + * + * Uses aoti/atof to do the conversion. + * Checks if the st is "nan" and converts it accordingly. + * + * \param[in] st the string containing the value to convert and copy + * \param[out] cloud the cloud to copy it to + * \param[in] point_index the index of the point + * \param[in] field_idx the index of the dimension/field + * \param[in] fields_count the current fields count + */ + template inline void + copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud, + unsigned int point_index, unsigned int field_idx, unsigned int fields_count) + { + Type value; + if (boost::iequals(st, "nan")) + { + value = std::numeric_limits::quiet_NaN (); + cloud.is_dense = false; + } + else + { + std::istringstream is (st); + is.imbue (std::locale::classic ()); + if (!(is >> value)) + value = static_cast (atof (st.c_str ())); + } + + memcpy (&cloud.data[point_index * cloud.point_step + + cloud.fields[field_idx].offset + + fields_count * sizeof (Type)], reinterpret_cast (&value), sizeof (Type)); + } + + template <> inline void + copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud, + unsigned int point_index, unsigned int field_idx, unsigned int fields_count) + { + std::int8_t value; + if (boost::iequals(st, "nan")) + { + value = static_cast (std::numeric_limits::quiet_NaN ()); + cloud.is_dense = false; + } + else + { + int val; + std::istringstream is (st); + is.imbue (std::locale::classic ()); + //is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS + if (!(is >> val)) + val = static_cast (atof (st.c_str ())); + value = static_cast (val); + } + + memcpy (&cloud.data[point_index * cloud.point_step + + cloud.fields[field_idx].offset + + fields_count * sizeof (std::int8_t)], reinterpret_cast (&value), sizeof (std::int8_t)); + } + + template <> inline void + copyStringValue (const std::string &st, pcl::PCLPointCloud2 &cloud, + unsigned int point_index, unsigned int field_idx, unsigned int fields_count) + { + std::uint8_t value; + if (boost::iequals(st, "nan")) + { + value = static_cast (std::numeric_limits::quiet_NaN ()); + cloud.is_dense = false; + } + else + { + int val; + std::istringstream is (st); + is.imbue (std::locale::classic ()); + //is >> val; -- unfortunately this fails on older GCC versions and CLANG on MacOS + if (!(is >> val)) + val = static_cast (atof (st.c_str ())); + value = static_cast (val); + } + + memcpy (&cloud.data[point_index * cloud.point_step + + cloud.fields[field_idx].offset + + fields_count * sizeof (std::uint8_t)], reinterpret_cast (&value), sizeof (std::uint8_t)); + } + +} diff --git a/deps_install/include/pcl-1.10/pcl/io/fotonic_grabber.h b/deps_install/include/pcl-1.10/pcl/io/fotonic_grabber.h new file mode 100755 index 0000000..04d276a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/fotonic_grabber.h @@ -0,0 +1,40 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#error PCL does not support FZ-API anymore. See https://github.com/PointCloudLibrary/pcl/issues/2701 for more details. diff --git a/deps_install/include/pcl-1.10/pcl/io/grabber.h b/deps_install/include/pcl-1.10/pcl/io/grabber.h new file mode 100755 index 0000000..b94d42d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/grabber.h @@ -0,0 +1,291 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + +// needed for the grabber interface / observers +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + + /** \brief Grabber interface for PCL 1.x device drivers + * \author Suat Gedikli + * \ingroup io + */ + class PCL_EXPORTS Grabber + { + public: + /** \brief virtual destructor. */ + virtual inline ~Grabber () noexcept; + + /** \brief registers a callback function/method to a signal with the corresponding signature + * \param[in] callback: the callback function/method + * \return Connection object, that can be used to disconnect the callback method from the signal again. + */ + template boost::signals2::connection + registerCallback (const std::function& callback); + + /** \brief registers a callback function/method to a signal with the corresponding signature + * \param[in] callback: the callback function/method + * \return Connection object, that can be used to disconnect the callback method from the signal again. + */ + template class FunctionT> + PCL_DEPRECATED ("please assign the callback to a std::function.") + boost::signals2::connection + registerCallback (const FunctionT& callback) + { + return registerCallback (std::function (callback)); + } + + /** \brief indicates whether a signal with given parameter-type exists or not + * \return true if signal exists, false otherwise + */ + template bool + providesCallback () const; + + /** \brief For devices that are streaming, the streams are started by calling this method. + * Trigger-based devices, just trigger the device once for each call of start. + */ + virtual void + start () = 0; + + /** \brief For devices that are streaming, the streams are stopped. + * This method has no effect for triggered devices. + */ + virtual void + stop () = 0; + + /** \brief For devices that are streaming, stopped streams are started and running stream are stopped. + * For triggered devices, the behavior is not defined. + * \return true if grabber is running / streaming. False otherwise. + */ + inline bool + toggle (); + + /** \brief returns the name of the concrete subclass. + * \return the name of the concrete driver. + */ + virtual std::string + getName () const = 0; + + /** \brief Indicates whether the grabber is streaming or not. This value is not defined for triggered devices. + * \return true if grabber is running / streaming. False otherwise. + */ + virtual bool + isRunning () const = 0; + + /** \brief returns fps. 0 if trigger based. */ + virtual float + getFramesPerSecond () const = 0; + + protected: + + virtual void + signalsChanged () { } + + template boost::signals2::signal* + find_signal () const; + + template int + num_slots () const; + + template void + disconnect_all_slots (); + + template void + block_signal (); + + template void + unblock_signal (); + + inline void + block_signals (); + + inline void + unblock_signals (); + + template boost::signals2::signal* + createSignal (); + + std::map signals_; + std::map > connections_; + std::map > shared_connections_; + } ; + + Grabber::~Grabber () noexcept + { + for (auto &signal : signals_) + delete signal.second; + } + + bool + Grabber::toggle () + { + if (isRunning ()) + { + stop (); + } else + { + start (); + } + return isRunning (); + } + + template boost::signals2::signal* + Grabber::find_signal () const + { + using Signal = boost::signals2::signal; + + std::map::const_iterator signal_it = signals_.find (typeid (T).name ()); + if (signal_it != signals_.end ()) + return (dynamic_cast (signal_it->second)); + + return (NULL); + } + + template void + Grabber::disconnect_all_slots () + { + using Signal = boost::signals2::signal; + + if (signals_.find (typeid (T).name ()) != signals_.end ()) + { + Signal* signal = dynamic_cast (signals_[typeid (T).name ()]); + signal->disconnect_all_slots (); + } + } + + template void + Grabber::block_signal () + { + if (connections_.find (typeid (T).name ()) != connections_.end ()) + for (auto &connection : shared_connections_[typeid (T).name ()]) + connection.block (); + } + + template void + Grabber::unblock_signal () + { + if (connections_.find (typeid (T).name ()) != connections_.end ()) + for (auto &connection : shared_connections_[typeid (T).name ()]) + connection.unblock (); + } + + void + Grabber::block_signals () + { + for (const auto &signal : signals_) + for (auto &connection : shared_connections_[signal.first]) + connection.block (); + } + + void + Grabber::unblock_signals () + { + for (const auto &signal : signals_) + for (auto &connection : shared_connections_[signal.first]) + connection.unblock (); + } + + template int + Grabber::num_slots () const + { + using Signal = boost::signals2::signal; + + // see if we have a signal for this type + std::map::const_iterator signal_it = signals_.find (typeid (T).name ()); + if (signal_it != signals_.end ()) + { + Signal* signal = dynamic_cast (signal_it->second); + return (static_cast (signal->num_slots ())); + } + return (0); + } + + template boost::signals2::signal* + Grabber::createSignal () + { + using Signal = boost::signals2::signal; + + if (signals_.find (typeid (T).name ()) == signals_.end ()) + { + Signal* signal = new Signal (); + signals_[typeid (T).name ()] = signal; + return (signal); + } + return (nullptr); + } + + template boost::signals2::connection + Grabber::registerCallback (const std::function & callback) + { + using Signal = boost::signals2::signal; + if (signals_.find (typeid (T).name ()) == signals_.end ()) + { + std::stringstream sstream; + + sstream << "no callback for type:" << typeid (T).name (); + + PCL_THROW_EXCEPTION (pcl::IOException, "[" << getName () << "] " << sstream.str ()); + //return (boost::signals2::connection ()); + } + Signal* signal = dynamic_cast (signals_[typeid (T).name ()]); + boost::signals2::connection ret = signal->connect (callback); + + connections_[typeid (T).name ()].push_back (ret); + shared_connections_[typeid (T).name ()].push_back (boost::signals2::shared_connection_block (connections_[typeid (T).name ()].back (), false)); + signalsChanged (); + return (ret); + } + + template bool + Grabber::providesCallback () const + { + if (signals_.find (typeid (T).name ()) == signals_.end ()) + return (false); + return (true); + } + +} // namespace diff --git a/deps_install/include/pcl-1.10/pcl/io/hdl_grabber.h b/deps_install/include/pcl-1.10/pcl/io/hdl_grabber.h new file mode 100755 index 0000000..d33352e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/hdl_grabber.h @@ -0,0 +1,328 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2012,2015 The MITRE Corporation + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "pcl/pcl_config.h" +#include + +#include +#include +#include +#include +#include +#include +#include + +#define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0) + +namespace pcl +{ + + /** \brief Grabber for the Velodyne High-Definition-Laser (HDL) + * \author Keven Ring + * \ingroup io + */ + class PCL_EXPORTS HDLGrabber : public Grabber + { + public: + /** \brief Signal used for a single sector + * Represents 1 corrected packet from the HDL Velodyne + */ + using sig_cb_velodyne_hdl_scan_point_cloud_xyz = void (const pcl::PointCloud::ConstPtr &, float, float); + + /** \brief Signal used for a single sector + * Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB + */ + using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba = void (const pcl::PointCloud::ConstPtr &, float, float); + + using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead") + = sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba; + + /** \brief Signal used for a single sector + * Represents 1 corrected packet from the HDL Velodyne with the returned intensity. + */ + using sig_cb_velodyne_hdl_scan_point_cloud_xyzi = void (const pcl::PointCloud::ConstPtr &, float, float); + + /** \brief Signal used for a 360 degree sweep + * Represents multiple corrected packets from the HDL Velodyne + * This signal is sent when the Velodyne passes angle "0" + */ + using sig_cb_velodyne_hdl_sweep_point_cloud_xyz = void (const pcl::PointCloud::ConstPtr &); + + /** \brief Signal used for a 360 degree sweep + * Represents multiple corrected packets from the HDL Velodyne with the returned intensity + * This signal is sent when the Velodyne passes angle "0" + */ + using sig_cb_velodyne_hdl_sweep_point_cloud_xyzi = void (const pcl::PointCloud::ConstPtr &); + + /** \brief Signal used for a 360 degree sweep + * Represents multiple corrected packets from the HDL Velodyne + * This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB + */ + using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba = void (const pcl::PointCloud::ConstPtr &); + + using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead") + = sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba; + + /** \brief Constructor taking an optional path to an HDL corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368] + * \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This parameter is mandatory for the HDL-64, optional for the HDL-32 + * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional + */ + HDLGrabber (const std::string& correctionsFile = "", + const std::string& pcapFile = ""); + + /** \brief Constructor taking a specified IP/port and an optional path to an HDL corrections file. + * \param[in] ipAddress IP Address that should be used to listen for HDL packets + * \param[in] port UDP Port that should be used to listen for HDL packets + * \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This field is mandatory for the HDL-64, optional for the HDL-32 + */ + HDLGrabber (const boost::asio::ip::address& ipAddress, + const std::uint16_t port, + const std::string& correctionsFile = ""); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + + ~HDLGrabber () noexcept; + + /** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */ + void + start () override; + + /** \brief Stops processing the Velodyne packets, either from the network or PCAP file */ + void + stop () override; + + /** \brief Obtains the name of this I/O Grabber + * \return The name of the grabber + */ + std::string + getName () const override; + + /** \brief Check if the grabber is still running. + * \return TRUE if the grabber is running, FALSE otherwise + */ + bool + isRunning () const override; + + /** \brief Returns the number of frames per second. + */ + float + getFramesPerSecond () const override; + + /** \brief Allows one to filter packets based on the SOURCE IP address and PORT + * This can be used, for instance, if multiple HDL LIDARs are on the same network + */ + void + filterPackets (const boost::asio::ip::address& ipAddress, + const std::uint16_t port = 443); + + /** \brief Allows one to customize the colors used by each laser. + * \param[in] color RGB color to set + * \param[in] laserNumber Number of laser to set color + */ + void + setLaserColorRGB (const pcl::RGB& color, + const std::uint8_t laserNumber); + + /** \brief Allows one to customize the colors used for each of the lasers. + * \param[in] begin begin iterator of RGB color array + * \param[in] end end iterator of RGB color array + */ + template void + setLaserColorRGB (const IterT& begin, const IterT& end) + { + std::copy (begin, end, laser_rgb_mapping_); + } + + /** \brief Any returns from the HDL with a distance less than this are discarded. + * This value is in meters + * Default: 0.0 + */ + void + setMinimumDistanceThreshold (float & minThreshold); + + /** \brief Any returns from the HDL with a distance greater than this are discarded. + * This value is in meters + * Default: 10000.0 + */ + void + setMaximumDistanceThreshold (float & maxThreshold); + + /** \brief Returns the current minimum distance threshold, in meters + */ + + float + getMinimumDistanceThreshold (); + + /** \brief Returns the current maximum distance threshold, in meters + */ + float + getMaximumDistanceThreshold (); + + /** \brief Returns the maximum number of lasers + */ + virtual std::uint8_t + getMaximumNumberOfLasers () const; + + protected: + static const std::uint16_t HDL_DATA_PORT = 2368; + static const std::uint16_t HDL_NUM_ROT_ANGLES = 36001; + static const std::uint8_t HDL_LASER_PER_FIRING = 32; + static const std::uint8_t HDL_MAX_NUM_LASERS = 64; + static const std::uint8_t HDL_FIRING_PER_PKT = 12; + + enum HDLBlock + { + BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff + }; + +#pragma pack(push, 1) + struct HDLLaserReturn + { + std::uint16_t distance; + std::uint8_t intensity; + }; +#pragma pack(pop) + + struct HDLFiringData + { + std::uint16_t blockIdentifier; + std::uint16_t rotationalPosition; + HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING]; + }; + + struct HDLDataPacket + { + HDLFiringData firingData[HDL_FIRING_PER_PKT]; + std::uint32_t gpsTimestamp; + std::uint8_t mode; + std::uint8_t sensorType; + }; + + struct HDLLaserCorrection + { + double azimuthCorrection; + double verticalCorrection; + double distanceCorrection; + double verticalOffsetCorrection; + double horizontalOffsetCorrection; + double sinVertCorrection; + double cosVertCorrection; + double sinVertOffsetCorrection; + double cosVertOffsetCorrection; + }; + + HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS]; + std::uint16_t last_azimuth_; + pcl::PointCloud::Ptr current_scan_xyz_, current_sweep_xyz_; + pcl::PointCloud::Ptr current_scan_xyzi_, current_sweep_xyzi_; + pcl::PointCloud::Ptr current_scan_xyzrgba_, current_sweep_xyzrgba_; + boost::signals2::signal* sweep_xyz_signal_; + boost::signals2::signal* sweep_xyzrgba_signal_; + boost::signals2::signal* sweep_xyzi_signal_; + boost::signals2::signal* scan_xyz_signal_; + boost::signals2::signal* scan_xyzrgba_signal_; + boost::signals2::signal* scan_xyzi_signal_; + + void + fireCurrentSweep (); + + void + fireCurrentScan (const std::uint16_t startAngle, + const std::uint16_t endAngle); + void + computeXYZI (pcl::PointXYZI& pointXYZI, + std::uint16_t azimuth, + HDLLaserReturn laserReturn, + HDLLaserCorrection correction); + + + private: + static double *cos_lookup_table_; + static double *sin_lookup_table_; + pcl::SynchronizedQueue hdl_data_; + boost::asio::ip::udp::endpoint udp_listener_endpoint_; + boost::asio::ip::address source_address_filter_; + std::uint16_t source_port_filter_; + boost::asio::io_service hdl_read_socket_service_; + boost::asio::ip::udp::socket *hdl_read_socket_; + std::string pcap_file_name_; + std::thread *queue_consumer_thread_; + std::thread *hdl_read_packet_thread_; + bool terminate_read_packet_thread_; + pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS]; + float min_distance_threshold_; + float max_distance_threshold_; + + virtual void + toPointClouds (HDLDataPacket *dataPacket); + + virtual boost::asio::ip::address + getDefaultNetworkAddress (); + + void + initialize (const std::string& correctionsFile = ""); + + void + processVelodynePackets (); + + void + enqueueHDLPacket (const std::uint8_t *data, + std::size_t bytesReceived); + + void + loadCorrectionsFile (const std::string& correctionsFile); + + void + loadHDL32Corrections (); + + void + readPacketsFromSocket (); + +#ifdef HAVE_PCAP + void + readPacketsFromPcap(); + +#endif //#ifdef HAVE_PCAP + + bool + isAddressUnspecified (const boost::asio::ip::address& ip_address); + + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/io/ifs_io.h b/deps_install/include/pcl-1.10/pcl/io/ifs_io.h new file mode 100755 index 0000000..4069474 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/ifs_io.h @@ -0,0 +1,255 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Indexed Face set (IFS) file format reader. This file format is used for + * the Brown Mesh Set for instance. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS IFSReader + { + public: + /** Empty constructor */ + IFSReader () {} + /** Empty destructor */ + ~IFSReader () {} + + /** \brief we support two versions + * 1.0 classic + * 1.1 with texture coordinates addon + */ + enum + { + IFS_V1_0 = 0, + IFS_V1_1 = 1 + }; + + /** \brief Read a point cloud data header from an IFS file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given IFS file. Useful for fast + * evaluation of the underlying data structure. + * + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only header will be filled) + * \param[out] ifs_version the IFS version of the file (IFS_V1_0 or IFS_V1_1) + * \param[out] data_idx the offset of cloud data within the file + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + int &ifs_version, unsigned int &data_idx); + + /** \brief Read a point cloud data from an IFS file and store it into a pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PCLPointCloud2 blob read from disk + * \param[out] ifs_version the IFS version of the file (either IFS_V1_0 or IFS_V1_1) + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, int &ifs_version); + + /** \brief Read a point cloud data from an IFS file and store it into a PolygonMesh. + * \param[in] file_name the name of the file containing the mesh data + * \param[out] mesh the resultant PolygonMesh + * \param[out] ifs_version the IFS version of the file (either IFS_V1_0 or IFS_V1_1) + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, int &ifs_version); + + /** \brief Read a point cloud data from an IFS file, and convert it to the + * given template pcl::PointCloud format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + template int + read (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::PCLPointCloud2 blob; + int ifs_version; + cloud.sensor_origin_ = Eigen::Vector4f::Zero (); + cloud.sensor_orientation_ = Eigen::Quaternionf::Identity (); + int res = read (file_name, blob, ifs_version); + + // If no error, convert the data + if (res == 0) + pcl::fromPCLPointCloud2 (blob, cloud); + return (res); + } + }; + + /** \brief Point Cloud Data (IFS) file format writer. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS IFSWriter + { + public: + IFSWriter() {} + ~IFSWriter() {} + + /** \brief Save point cloud data to an IFS file containing 3D points. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data + * \param[in] cloud_name the point cloud name to be stored inside the IFS file. + * + * \return + * * 0 on success + * * < 0 on error + */ + int + write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const std::string &cloud_name = "cloud"); + + /** \brief Save point cloud data to an IFS file containing 3D points. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * \param[in] cloud_name the point cloud name to be stored inside the IFS file. + * + * \return + * * 0 on success + * * < 0 on error + */ + template int + write (const std::string &file_name, const pcl::PointCloud &cloud, + const std::string &cloud_name = "cloud") + { + pcl::PCLPointCloud2 blob; + pcl::toPCLPointCloud2 (cloud, blob); + return (write (file_name, blob, cloud_name)); + } + }; + + namespace io + { + /** \brief Load an IFS file into a PCLPointCloud2 blob type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadIFSFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) + { + pcl::IFSReader p; + int ifs_version; + return (p.read (file_name, cloud, ifs_version)); + } + + /** \brief Load any IFS file into a templated PointCloud type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + template inline int + loadIFSFile (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::IFSReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any IFS file into a PolygonMesh type. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant mesh + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadIFSFile (const std::string &file_name, pcl::PolygonMesh &mesh) + { + pcl::IFSReader p; + int ifs_version; + return (p.read (file_name, mesh, ifs_version)); + } + + /** \brief Save point cloud data to an IFS file containing 3D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + saveIFSFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud) + { + pcl::IFSWriter w; + return (w.write (file_name, cloud)); + } + + /** \brief Save point cloud data to an IFS file containing 3D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + template int + saveIFSFile (const std::string &file_name, const pcl::PointCloud &cloud) + { + pcl::IFSWriter w; + return (w.write (file_name, cloud)); + } + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/image.h b/deps_install/include/pcl-1.10/pcl/io/image.h new file mode 100755 index 0000000..4cb43ee --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/image.h @@ -0,0 +1,214 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Image interface class providing an interface to fill a RGB or Grayscale image buffer. + * @param[in] image_metadata + * @ingroup io + */ + class PCL_EXPORTS Image + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using Clock = std::chrono::high_resolution_clock; + using Timestamp = std::chrono::high_resolution_clock::time_point; + + enum Encoding + { + BAYER_GRBG, + YUV422, + RGB + }; + + Image (FrameWrapper::Ptr image_metadata) + : wrapper_ (std::move(image_metadata)) + , timestamp_ (Clock::now ()) + {} + + Image (FrameWrapper::Ptr image_metadata, Timestamp time) + : wrapper_ (std::move(image_metadata)) + , timestamp_ (time) + {} + + /** + * @brief virtual Destructor that never throws an exception. + */ + inline virtual ~Image () + {} + + /** + * @param[in] input_width width of input image + * @param[in] input_height height of input image + * @param[in] output_width width of desired output image + * @param[in] output_height height of desired output image + * @return whether the resizing is supported or not. + */ + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, + unsigned output_width, unsigned output_height) const = 0; + + /** + * @brief fills a user given buffer with the RGB values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] rgb_buffer the output RGB buffer. + * @param[in] rgb_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const = 0; + + /** + * @brief returns the encoding of the native data. + * @return encoding + */ + virtual Encoding + getEncoding () const = 0; + + /** + * @brief fills a user given buffer with the raw values. + * @param[in,out] rgb_buffer + */ + virtual void + fillRaw (unsigned char* rgb_buffer) const + { + memcpy (rgb_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + } + + /** + * @brief fills a user given buffer with the gray values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] gray_buffer the output gray buffer. + * @param[in] gray_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, + unsigned gray_line_step = 0) const = 0; + + /** + * @return width of the image + */ + unsigned + getWidth () const + { + return (wrapper_->getWidth ()); + } + + /** + * @return height of the image + */ + unsigned + getHeight () const + { + return (wrapper_->getHeight ()); + } + + /** + * @return frame id of the image. + * @note frame ids are ascending, but not necessarily synchronized with other streams + */ + unsigned + getFrameID () const + { + return (wrapper_->getFrameID ()); + } + + /** + * @return the timestamp of the image + * @note the time value is not synchronized with the system time + */ + std::uint64_t + getTimestamp () const + { + return (wrapper_->getTimestamp ()); + } + + + /** + * @return the timestamp of the image + * @note the time value *is* synchronized with the system time. + */ + Timestamp + getSystemTimestamp () const + { + return (timestamp_); + } + + // Get a const pointer to the raw depth buffer + const void* + getData () + { + return (wrapper_->getData ()); + } + + // Data buffer size in bytes + int + getDataSize () const + { + return (wrapper_->getDataSize ()); + } + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/image_depth.h b/deps_install/include/pcl-1.10/pcl/io/image_depth.h new file mode 100755 index 0000000..2e16ebd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/image_depth.h @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** \brief This class provides methods to fill a depth or disparity image. + */ + class PCL_EXPORTS DepthImage + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using Clock = std::chrono::high_resolution_clock; + using Timestamp = std::chrono::high_resolution_clock::time_point; + + /** \brief Constructor + * \param[in] depth_metadata the actual data from the OpenNI library + * \param[in] baseline the baseline of the "stereo" camera, i.e. the distance between the projector and the IR camera for + * Primesense like cameras. e.g. 7.5cm for PSDK5 and PSDK6 reference design. + * \param[in] focal_length focal length of the "stereo" frame. + * \param[in] shadow_value defines which values in the depth data are indicating shadow (resulting from the parallax between projector and IR camera) + * \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined . + * \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not. + */ + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, std::uint64_t shadow_value, std::uint64_t no_sample_value); + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, std::uint64_t shadow_value, std::uint64_t no_sample_value, Timestamp time); + + /** \brief Destructor. Never throws an exception. */ + ~DepthImage (); + + /** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method + * \return the actual depth data of type openni::VideoFrameRef. + */ + const FrameWrapper::Ptr + getMetaData () const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width the width of the desired disparity image. + * \param[in] height the height of the desired disparity image. + * \param[in,out] disparity_buffer the float pointer to the actual memory buffer to be filled with the disparity values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired depth image. + * \param[in] height height the height of the desired depth image. + * \param[in,out] depth_buffer the float pointer to the actual memory buffer to be filled with the depth values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the raw values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired raw image. + * \param[in] height height the height of the desired raw image. + * \param[in,out] depth_buffer the unsigned short pointer to the actual memory buffer to be filled with the raw values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step = 0) const; + + /** \brief method to access the baseline of the "stereo" frame that was used to retrieve the depth image. + * \return baseline in meters + */ + float + getBaseline () const; + + /** \brief method to access the focal length of the "stereo" frame that was used to retrieve the depth image. + * \return focal length in pixels + */ + float + getFocalLength () const; + + /** \brief method to access the shadow value, that indicates pixels lying in shadow in the depth image. + * \return shadow value + */ + std::uint64_t + getShadowValue () const; + + /** \brief method to access the no-sample value, that indicates pixels where no disparity could be determined for the depth image. + * \return no-sample value + */ + std::uint64_t + getNoSampleValue () const; + + /** \return the width of the depth image */ + unsigned + getWidth () const; + + /** \return the height of the depth image */ + unsigned + getHeight () const; + + /** \return an ascending id for the depth frame + * \attention not necessarily synchronized with other streams + */ + unsigned + getFrameID () const; + + /** \return a ascending timestamp for the depth frame + * \attention its not the system time, thus can not be used directly to synchronize different sensors. + * But definitely synchronized with other streams + */ + std::uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + pcl::io::FrameWrapper::Ptr wrapper_; + + float baseline_; + float focal_length_; + std::uint64_t shadow_value_; + std::uint64_t no_sample_value_; + Timestamp timestamp_; + }; + +}} // namespace diff --git a/deps_install/include/pcl-1.10/pcl/io/image_grabber.h b/deps_install/include/pcl-1.10/pcl/io/image_grabber.h new file mode 100755 index 0000000..0b505c6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/image_grabber.h @@ -0,0 +1,321 @@ + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace pcl +{ + /** \brief Base class for Image file grabber. + * \ingroup io + */ + class PCL_EXPORTS ImageGrabberBase : public Grabber + { + public: + /** \brief Constructor taking a folder of depth+[rgb] images. + * \param[in] directory Directory which contains an ordered set of images corresponding to an [RGB]D video, stored as TIFF, PNG, JPG, or PPM files. The naming convention is: frame_[timestamp]_["depth"/"rgb"].[extension] + * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. + * \param[in] repeat whether to play PCD file in an endless loop or not. + * \param pclzf_mode + */ + ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode); + + ImageGrabberBase (const std::string& depth_directory, const std::string& rgb_directory, float frames_per_second, bool repeat); + /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list. + * \param[in] depth_image_files Path to the depth image files files. + * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. + * \param[in] repeat whether to play PCD file in an endless loop or not. + */ + ImageGrabberBase (const std::vector& depth_image_files, float frames_per_second, bool repeat); + + /** \brief Copy constructor. + * \param[in] src the Image Grabber base object to copy into this + */ + ImageGrabberBase (const ImageGrabberBase &src) : impl_ () + { + *this = src; + } + + /** \brief Copy operator. + * \param[in] src the Image Grabber base object to copy into this + */ + ImageGrabberBase& + operator = (const ImageGrabberBase &src) + { + impl_ = src.impl_; + return (*this); + } + + /** \brief Virtual destructor. */ + ~ImageGrabberBase () noexcept; + + /** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */ + void + start () override; + + /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */ + void + stop () override; + + /** \brief Triggers a callback with new data */ + virtual void + trigger (); + + /** \brief whether the grabber is started (publishing) or not. + * \return true only if publishing. + */ + bool + isRunning () const override; + + /** \return The name of the grabber */ + std::string + getName () const override; + + /** \brief Rewinds to the first PCD file in the list.*/ + virtual void + rewind (); + + /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */ + float + getFramesPerSecond () const override; + + /** \brief Returns whether the repeat flag is on */ + bool + isRepeatOn () const; + + /** \brief Returns if the last frame is reached */ + bool + atLastFrame () const; + + /** \brief Returns the filename of the current indexed file */ + std::string + getCurrentDepthFileName () const; + + /** \brief Returns the filename of the previous indexed file + * SDM: adding this back in, but is this useful, or confusing? */ + std::string + getPrevDepthFileName () const; + + /** \brief Get the depth filename at a particular index */ + std::string + getDepthFileNameAtIndex (std::size_t idx) const; + + /** \brief Query only the timestamp of an index, if it exists */ + bool + getTimestampAtIndex (std::size_t idx, std::uint64_t ×tamp) const; + + /** \brief Manually set RGB image files. + * \param[in] rgb_image_files A vector of [tiff/png/jpg/ppm] files to use as input. There must be a 1-to-1 correspondence between these and the depth images you set + */ + void + setRGBImageFiles (const std::vector& rgb_image_files); + + /** \brief Define custom focal length and center pixel. This will override ANY other setting of parameters for the duration of the grabber's life, whether by factory defaults or explicitly read from a frame_[timestamp].xml file. + * \param[in] focal_length_x Horizontal focal length (fx) + * \param[in] focal_length_y Vertical focal length (fy) + * \param[in] principal_point_x Horizontal coordinates of the principal point (cx) + * \param[in] principal_point_y Vertical coordinates of the principal point (cy) + */ + virtual void + setCameraIntrinsics (const double focal_length_x, + const double focal_length_y, + const double principal_point_x, + const double principal_point_y); + + /** \brief Get the current focal length and center pixel. If the intrinsics have been manually set with setCameraIntrinsics, this will return those values. Else, if start () has been called and the grabber has found a frame_[timestamp].xml file, this will return the most recent values read. Else, returns factory defaults. + * \param[out] focal_length_x Horizontal focal length (fx) + * \param[out] focal_length_y Vertical focal length (fy) + * \param[out] principal_point_x Horizontal coordinates of the principal point (cx) + * \param[out] principal_point_y Vertical coordinates of the principal point (cy) + */ + virtual void + getCameraIntrinsics (double &focal_length_x, + double &focal_length_y, + double &principal_point_x, + double &principal_point_y) const; + + /** \brief Define the units the depth data is stored in. + * Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/ + void + setDepthImageUnits (float units); + + /** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population. + * Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + protected: + /** \brief Convenience function to see how many frames this consists of + */ + std::size_t + numFrames () const; + + /** \brief Gets the cloud in ROS form at location idx */ + bool + getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const; + + + private: + virtual void + publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0; + + + // to separate and hide the implementation from interface: PIMPL + struct ImageGrabberImpl; + ImageGrabberImpl* impl_; + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template class PointCloud; + template class ImageGrabber : public ImageGrabberBase, public FileGrabber + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + ImageGrabber (const std::string& dir, + float frames_per_second = 0, + bool repeat = false, + bool pclzf_mode = false); + + ImageGrabber (const std::string& depth_dir, + const std::string& rgb_dir, + float frames_per_second = 0, + bool repeat = false); + + ImageGrabber (const std::vector& depth_image_files, + float frames_per_second = 0, + bool repeat = false); + + /** \brief Empty destructor */ + ~ImageGrabber () noexcept {} + + // Inherited from FileGrabber + const typename pcl::PointCloud::ConstPtr + operator[] (std::size_t idx) const override; + + // Inherited from FileGrabber + std::size_t + size () const override; + + protected: + void + publish (const pcl::PCLPointCloud2& blob, + const Eigen::Vector4f& origin, + const Eigen::Quaternionf& orientation) const override; + boost::signals2::signal::ConstPtr&)>* signal_; + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + ImageGrabber::ImageGrabber (const std::string& dir, + float frames_per_second, + bool repeat, + bool pclzf_mode) + : ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode) + { + signal_ = createSignal::ConstPtr&)>(); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + ImageGrabber::ImageGrabber (const std::string& depth_dir, + const std::string& rgb_dir, + float frames_per_second, + bool repeat) + : ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat) + { + signal_ = createSignal::ConstPtr&)>(); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + ImageGrabber::ImageGrabber (const std::vector& depth_image_files, + float frames_per_second, + bool repeat) + : ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ () + { + signal_ = createSignal::ConstPtr&)>(); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template const typename pcl::PointCloud::ConstPtr + ImageGrabber::operator[] (std::size_t idx) const + { + pcl::PCLPointCloud2 blob; + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + getCloudAt (idx, blob, origin, orientation); + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::fromPCLPointCloud2 (blob, *cloud); + cloud->sensor_origin_ = origin; + cloud->sensor_orientation_ = orientation; + return (cloud); + } + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template std::size_t + ImageGrabber::size () const + { + return (numFrames ()); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void + ImageGrabber::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const + { + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::fromPCLPointCloud2 (blob, *cloud); + cloud->sensor_origin_ = origin; + cloud->sensor_orientation_ = orientation; + + signal_->operator () (cloud); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/image_ir.h b/deps_install/include/pcl-1.10/pcl/io/image_ir.h new file mode 100755 index 0000000..ad6c275 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/image_ir.h @@ -0,0 +1,114 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + +#pragma once + +#include + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Class containing just a reference to IR meta data. + */ + class PCL_EXPORTS IRImage + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using Clock = std::chrono::high_resolution_clock; + using Timestamp = std::chrono::high_resolution_clock::time_point; + + IRImage (FrameWrapper::Ptr ir_metadata); + IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time); + + ~IRImage () noexcept + {} + + void + fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step = 0) const; + + unsigned + getWidth () const; + + unsigned + getHeight () const; + + unsigned + getFrameID () const; + + std::uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer. If the data is accessed just read-only, then this method is faster than a fillXXX method + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + /** \brief method to access the internal data structure wrapper, which needs to be cast to an + * approperate subclass before the getMetadata(..) function is available to access the native data type. + */ + const FrameWrapper::Ptr + getMetaData () const; + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/image_metadata_wrapper.h b/deps_install/include/pcl-1.10/pcl/io/image_metadata_wrapper.h new file mode 100755 index 0000000..fcc290f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/image_metadata_wrapper.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace io + { + + /** + * Pure abstract interface to wrap native frame data types. + */ + class FrameWrapper + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + virtual + ~FrameWrapper() = default; + + virtual const void* + getData () const = 0; + + virtual unsigned + getDataSize () const = 0; + + virtual unsigned + getWidth () const = 0; + + virtual unsigned + getHeight () const = 0; + + virtual unsigned + getFrameID () const = 0; + + // Microseconds from some arbitrary start point + virtual std::uint64_t + getTimestamp () const = 0; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/image_rgb24.h b/deps_install/include/pcl-1.10/pcl/io/image_rgb24.h new file mode 100755 index 0000000..36d5e03 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/image_rgb24.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image. + * @ingroup io + */ + class PCL_EXPORTS ImageRGB24 : public pcl::io::Image + { + public: + + ImageRGB24 (FrameWrapper::Ptr image_metadata); + ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + ~ImageRGB24 () noexcept; + + inline Encoding + getEncoding () const override + { + return (RGB); + } + + void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; + + void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; + + bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; + + private: + + // Struct used for type conversion + struct RGB888Pixel + { + std::uint8_t r; + std::uint8_t g; + std::uint8_t b; + }; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/image_yuv422.h b/deps_install/include/pcl-1.10/pcl/io/image_yuv422.h new file mode 100755 index 0000000..08f7d22 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/image_yuv422.h @@ -0,0 +1,77 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + +#pragma once + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief Concrete implementation of the interface Image for a YUV 422 image used by Primesense devices. + * @ingroup io + */ + class PCL_EXPORTS ImageYUV422 : public pcl::io::Image + { + public: + ImageYUV422 (FrameWrapper::Ptr image_metadata); + ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + + ~ImageYUV422 () noexcept; + + inline Encoding + getEncoding () const override + { + return (YUV422); + } + + void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; + + void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; + + bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/ascii_io.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/ascii_io.hpp new file mode 100755 index 0000000..ba31414 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/ascii_io.hpp @@ -0,0 +1,59 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_IO_ASCII_IO_HPP_ +#define PCL_IO_ASCII_IO_HPP_ + +template void +pcl::ASCIIReader::setInputFields () +{ + fields_ = pcl::getFields (); + + // Remove empty fields and adjust offset + int offset =0; + for (std::vector::iterator field_iter = fields_.begin (); + field_iter != fields_.end (); ++field_iter) + { + if (field_iter->name == "_") + field_iter = fields_.erase (field_iter); + field_iter->offset = offset; + offset += typeSize (field_iter->datatype); + } +} + + +#endif //PCL_IO_ASCII_IO_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/auto_io.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/auto_io.hpp new file mode 100755 index 0000000..ab82de9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/auto_io.hpp @@ -0,0 +1,96 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_IO_AUTO_IO_IMPL_H_ +#define PCL_IO_AUTO_IO_IMPL_H_ + +// #include +// #include +#include +#include +#include +// #include + +namespace pcl +{ + namespace io + { + template int + load (const std::string& file_name, pcl::PointCloud& cloud) + { + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".pcd") + result = pcl::io::loadPCDFile (file_name, cloud); + else if (extension == ".ply") + result = pcl::io::loadPLYFile (file_name, cloud); + else if (extension == ".ifs") + result = pcl::io::loadIFSFile (file_name, cloud); + else + { + PCL_ERROR ("[pcl::io::load] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); + } + + template int + save (const std::string& file_name, const pcl::PointCloud& cloud) + { + boost::filesystem::path p (file_name.c_str ()); + std::string extension = p.extension ().string (); + int result = -1; + if (extension == ".pcd") + result = pcl::io::savePCDFile (file_name, cloud, true); + else if (extension == ".ply") + result = pcl::io::savePLYFile (file_name, cloud, true); + else if (extension == ".ifs") + result = pcl::io::saveIFSFile (file_name, cloud); + else + { + PCL_ERROR ("[pcl::io::save] Don't know how to handle file with extension %s", extension.c_str ()); + result = -1; + } + return (result); + } + } +} + +#endif //PCL_IO_AUTO_IO_IMPL_H_ \ No newline at end of file diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/entropy_range_coder.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/entropy_range_coder.hpp new file mode 100755 index 0000000..d348b4d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/entropy_range_coder.hpp @@ -0,0 +1,626 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * range coder based on Dmitry Subbotin's carry-less implementation (http://www.compression.ru/ds/) + * Added optimized symbol lookup and fixed/static frequency tables + * + */ + +#ifndef __PCL_IO_RANGECODING__HPP +#define __PCL_IO_RANGECODING__HPP + +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +unsigned long +pcl::AdaptiveRangeCoder::encodeCharVectorToStream (const std::vector& inputByteVector_arg, + std::ostream& outputByteStream_arg) +{ + DWord freq[257]; + + // define limits + const DWord top = static_cast (1) << 24; + const DWord bottom = static_cast (1) << 16; + const DWord maxRange = static_cast (1) << 16; + + unsigned int input_size = static_cast (inputByteVector_arg.size ()); + + // init output vector + outputCharVector_.clear (); + outputCharVector_.reserve (sizeof(char) * input_size); + + unsigned int readPos = 0; + + DWord low = 0; + DWord range = static_cast (-1); + + // initialize cumulative frequency table + for (unsigned int i = 0; i < 257; i++) + freq[i] = i; + + // scan input + while (readPos < input_size) + { + // read byte + std::uint8_t ch = inputByteVector_arg[readPos++]; + + // map range + low += freq[ch] * (range /= freq[256]); + range *= freq[ch + 1] - freq[ch]; + + // check range limits + while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1))) + { + char out = static_cast (low >> 24); + range <<= 8; + low <<= 8; + outputCharVector_.push_back (out); + } + + // update frequency table + for (unsigned int j = ch + 1; j < 257; j++) + freq[j]++; + + // detect overflow + if (freq[256] >= maxRange) + { + // rescale + for (unsigned int f = 1; f <= 256; f++) + { + freq[f] /= 2; + if (freq[f] <= freq[f - 1]) + freq[f] = freq[f - 1] + 1; + } + } + + } + + // flush remaining data + for (unsigned int i = 0; i < 4; i++) + { + char out = static_cast (low >> 24); + outputCharVector_.push_back (out); + low <<= 8; + } + + // write to stream + outputByteStream_arg.write (&outputCharVector_[0], outputCharVector_.size ()); + + return (static_cast (outputCharVector_.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +unsigned long +pcl::AdaptiveRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg, + std::vector& outputByteVector_arg) +{ + DWord freq[257]; + + // define limits + const DWord top = static_cast (1) << 24; + const DWord bottom = static_cast (1) << 16; + const DWord maxRange = static_cast (1) << 16; + + unsigned int output_size = static_cast (outputByteVector_arg.size ()); + + unsigned long streamByteCount = 0; + + unsigned int outputBufPos = 0; + + DWord code = 0; + DWord low = 0; + DWord range = static_cast (-1); + + // init decoding + for (unsigned int i = 0; i < 4; i++) + { + std::uint8_t ch; + inputByteStream_arg.read (reinterpret_cast (&ch), sizeof(char)); + streamByteCount += sizeof(char); + code = (code << 8) | ch; + } + + // init cumulative frequency table + for (unsigned int i = 0; i <= 256; i++) + freq[i] = i; + + // decoding loop + for (unsigned int i = 0; i < output_size; i++) + { + std::uint8_t symbol = 0; + std::uint8_t sSize = 256 / 2; + + // map code to range + DWord count = (code - low) / (range /= freq[256]); + + // find corresponding symbol + while (sSize > 0) + { + if (freq[symbol + sSize] <= count) + { + symbol = static_cast (symbol + sSize); + } + sSize /= 2; + } + + // output symbol + outputByteVector_arg[outputBufPos++] = symbol; + + // update range limits + low += freq[symbol] * range; + range *= freq[symbol + 1] - freq[symbol]; + + // decode range limits + while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1))) + { + std::uint8_t ch; + inputByteStream_arg.read (reinterpret_cast (&ch), sizeof(char)); + streamByteCount += sizeof(char); + code = code << 8 | ch; + range <<= 8; + low <<= 8; + } + + // update cumulative frequency table + for (unsigned int j = symbol + 1; j < 257; j++) + freq[j]++; + + // detect overflow + if (freq[256] >= maxRange) + { + // rescale + for (unsigned int f = 1; f <= 256; f++) + { + freq[f] /= 2; + if (freq[f] <= freq[f - 1]) + freq[f] = freq[f - 1] + 1; + } + } + } + + return (streamByteCount); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +unsigned long +pcl::StaticRangeCoder::encodeIntVectorToStream (std::vector& inputIntVector_arg, + std::ostream& outputByteStream_arg) +{ + // define numerical limits + const std::uint64_t top = static_cast (1) << 56; + const std::uint64_t bottom = static_cast (1) << 48; + const std::uint64_t maxRange = static_cast (1) << 48; + + unsigned long input_size = static_cast (inputIntVector_arg.size ()); + + // init output vector + outputCharVector_.clear (); + outputCharVector_.reserve ((sizeof(char) * input_size * 2)); + + std::uint64_t frequencyTableSize = 1; + + unsigned int readPos = 0; + + // calculate frequency table + cFreqTable_[0] = cFreqTable_[1] = 0; + while (readPos < input_size) + { + unsigned int inputSymbol = inputIntVector_arg[readPos++]; + + if (inputSymbol + 1 >= frequencyTableSize) + { + // frequency table is to small -> adaptively extend it + std::uint64_t oldfrequencyTableSize; + oldfrequencyTableSize = frequencyTableSize; + + do + { + // increase frequency table size by factor 2 + frequencyTableSize <<= 1; + } while (inputSymbol + 1 > frequencyTableSize); + + if (cFreqTable_.size () < frequencyTableSize + 1) + { + // resize frequency vector + cFreqTable_.resize (static_cast (frequencyTableSize + 1)); + } + + // init new frequency range with zero + memset (&cFreqTable_[static_cast (oldfrequencyTableSize + 1)], 0, + sizeof(std::uint64_t) * static_cast (frequencyTableSize - oldfrequencyTableSize)); + } + cFreqTable_[inputSymbol + 1]++; + } + frequencyTableSize++; + + // convert to cumulative frequency table + for (std::uint64_t f = 1; f < frequencyTableSize; f++) + { + cFreqTable_[f] = cFreqTable_[f - 1] + cFreqTable_[f]; + if (cFreqTable_[f] <= cFreqTable_[f - 1]) + cFreqTable_[f] = cFreqTable_[f - 1] + 1; + } + + // rescale if numerical limits are reached + while (cFreqTable_[static_cast (frequencyTableSize - 1)] >= maxRange) + { + for (std::size_t f = 1; f < cFreqTable_.size (); f++) + { + cFreqTable_[f] /= 2; + ; + if (cFreqTable_[f] <= cFreqTable_[f - 1]) + cFreqTable_[f] = cFreqTable_[f - 1] + 1; + } + } + + // calculate amount of bytes per frequency table entry + std::uint8_t frequencyTableByteSize = static_cast (std::ceil ( + std::log2 (static_cast (cFreqTable_[static_cast (frequencyTableSize - 1)] + 1)) / 8.0)); + + // write size of frequency table to output stream + outputByteStream_arg.write (reinterpret_cast (&frequencyTableSize), sizeof(frequencyTableSize)); + outputByteStream_arg.write (reinterpret_cast (&frequencyTableByteSize), sizeof(frequencyTableByteSize)); + + unsigned long streamByteCount = sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize); + + // write cumulative frequency table to output stream + for (std::uint64_t f = 1; f < frequencyTableSize; f++) + { + outputByteStream_arg.write (reinterpret_cast (&cFreqTable_[f]), frequencyTableByteSize); + streamByteCount += frequencyTableByteSize; + } + + readPos = 0; + std::uint64_t low = 0; + std::uint64_t range = static_cast (-1); + + // start encoding + while (readPos < input_size) + { + + // read symol + unsigned int inputsymbol = inputIntVector_arg[readPos++]; + + // map to range + low += cFreqTable_[inputsymbol] * (range /= cFreqTable_[static_cast (frequencyTableSize - 1)]); + range *= cFreqTable_[inputsymbol + 1] - cFreqTable_[inputsymbol]; + + // check range limits + while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1))) + { + char out = static_cast (low >> 56); + range <<= 8; + low <<= 8; + outputCharVector_.push_back (out); + } + + } + + // flush remaining data + for (unsigned int i = 0; i < 8; i++) + { + char out = static_cast (low >> 56); + outputCharVector_.push_back (out); + low <<= 8; + } + + // write encoded data to stream + outputByteStream_arg.write (&outputCharVector_[0], outputCharVector_.size ()); + + streamByteCount += static_cast (outputCharVector_.size ()); + + return (streamByteCount); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +unsigned long +pcl::StaticRangeCoder::decodeStreamToIntVector (std::istream& inputByteStream_arg, + std::vector& outputIntVector_arg) +{ + // define range limits + const std::uint64_t top = static_cast (1) << 56; + const std::uint64_t bottom = static_cast (1) << 48; + + std::uint64_t frequencyTableSize; + unsigned char frequencyTableByteSize; + + unsigned int outputBufPos = 0; + std::size_t output_size = outputIntVector_arg.size (); + + // read size of cumulative frequency table from stream + inputByteStream_arg.read (reinterpret_cast (&frequencyTableSize), sizeof(frequencyTableSize)); + inputByteStream_arg.read (reinterpret_cast (&frequencyTableByteSize), sizeof(frequencyTableByteSize)); + + unsigned long streamByteCount = sizeof(frequencyTableSize) + sizeof(frequencyTableByteSize); + + // check size of frequency table vector + if (cFreqTable_.size () < frequencyTableSize) + { + cFreqTable_.resize (static_cast (frequencyTableSize)); + } + + // init with zero + memset (&cFreqTable_[0], 0, sizeof(std::uint64_t) * static_cast (frequencyTableSize)); + + // read cumulative frequency table + for (std::uint64_t f = 1; f < frequencyTableSize; f++) + { + inputByteStream_arg.read (reinterpret_cast (&cFreqTable_[f]), frequencyTableByteSize); + streamByteCount += frequencyTableByteSize; + } + + // initialize range & code + std::uint64_t code = 0; + std::uint64_t low = 0; + std::uint64_t range = static_cast (-1); + + // init code vector + for (unsigned int i = 0; i < 8; i++) + { + std::uint8_t ch; + inputByteStream_arg.read (reinterpret_cast (&ch), sizeof(char)); + streamByteCount += sizeof(char); + code = (code << 8) | ch; + } + + // decoding + for (std::size_t i = 0; i < output_size; i++) + { + std::uint64_t count = (code - low) / (range /= cFreqTable_[static_cast (frequencyTableSize - 1)]); + + // symbol lookup in cumulative frequency table + std::uint64_t symbol = 0; + std::uint64_t sSize = (frequencyTableSize - 1) / 2; + while (sSize > 0) + { + if (cFreqTable_[static_cast (symbol + sSize)] <= count) + { + symbol += sSize; + } + sSize /= 2; + } + + // write symbol to output stream + outputIntVector_arg[outputBufPos++] = static_cast (symbol); + + // map to range + low += cFreqTable_[static_cast (symbol)] * range; + range *= cFreqTable_[static_cast (symbol + 1)] - cFreqTable_[static_cast (symbol)]; + + // check range limits + while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -low & (bottom - 1)), 1))) + { + std::uint8_t ch; + inputByteStream_arg.read (reinterpret_cast (&ch), sizeof(char)); + streamByteCount += sizeof(char); + code = code << 8 | ch; + range <<= 8; + low <<= 8; + } + } + + return streamByteCount; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +unsigned long +pcl::StaticRangeCoder::encodeCharVectorToStream (const std::vector& inputByteVector_arg, + std::ostream& outputByteStream_arg) +{ + DWord freq[257]; + + // define numerical limits + const DWord top = static_cast (1) << 24; + const DWord bottom = static_cast (1) << 16; + const DWord maxRange = static_cast (1) << 16; + + DWord low, range; + + unsigned int input_size; + input_size = static_cast (inputByteVector_arg.size ()); + + // init output vector + outputCharVector_.clear (); + outputCharVector_.reserve (sizeof(char) * input_size); + + std::uint64_t FreqHist[257]; + + // calculate frequency table + memset (FreqHist, 0, sizeof(FreqHist)); + unsigned int readPos = 0; + while (readPos < input_size) + { + std::uint8_t symbol = static_cast (inputByteVector_arg[readPos++]); + FreqHist[symbol + 1]++; + } + + // convert to cumulative frequency table + freq[0] = 0; + for (int f = 1; f <= 256; f++) + { + freq[f] = freq[f - 1] + static_cast (FreqHist[f]); + if (freq[f] <= freq[f - 1]) + freq[f] = freq[f - 1] + 1; + } + + // rescale if numerical limits are reached + while (freq[256] >= maxRange) + { + for (int f = 1; f <= 256; f++) + { + freq[f] /= 2; + ; + if (freq[f] <= freq[f - 1]) + freq[f] = freq[f - 1] + 1; + } + } + + // write cumulative frequency table to output stream + outputByteStream_arg.write (reinterpret_cast (&freq[0]), sizeof(freq)); + unsigned long streamByteCount = sizeof(freq); + + readPos = 0; + + low = 0; + range = static_cast (-1); + + // start encoding + while (readPos < input_size) + { + // read symol + std::uint8_t ch = inputByteVector_arg[readPos++]; + + // map to range + low += freq[ch] * (range /= freq[256]); + range *= freq[ch + 1] - freq[ch]; + + // check range limits + while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1))) + { + char out = static_cast (low >> 24); + range <<= 8; + low <<= 8; + outputCharVector_.push_back (out); + } + + } + + // flush remaining data + for (int i = 0; i < 4; i++) + { + char out = static_cast (low >> 24); + outputCharVector_.push_back (out); + low <<= 8; + } + + // write encoded data to stream + outputByteStream_arg.write (&outputCharVector_[0], outputCharVector_.size ()); + + streamByteCount += static_cast (outputCharVector_.size ()); + + return (streamByteCount); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +unsigned long +pcl::StaticRangeCoder::decodeStreamToCharVector (std::istream& inputByteStream_arg, + std::vector& outputByteVector_arg) +{ + DWord freq[257]; + + // define range limits + const DWord top = static_cast (1) << 24; + const DWord bottom = static_cast (1) << 16; + + DWord low, range; + DWord code; + + unsigned int outputBufPos; + unsigned int output_size; + + unsigned long streamByteCount; + + streamByteCount = 0; + + output_size = static_cast (outputByteVector_arg.size ()); + + outputBufPos = 0; + + // read cumulative frequency table + inputByteStream_arg.read (reinterpret_cast (&freq[0]), sizeof(freq)); + streamByteCount += sizeof(freq); + + code = 0; + low = 0; + range = static_cast (-1); + + // init code + for (unsigned int i = 0; i < 4; i++) + { + std::uint8_t ch; + inputByteStream_arg.read (reinterpret_cast (&ch), sizeof(char)); + streamByteCount += sizeof(char); + code = (code << 8) | ch; + } + + // decoding + for (unsigned int i = 0; i < output_size; i++) + { + // symbol lookup in cumulative frequency table + std::uint8_t symbol = 0; + std::uint8_t sSize = 256 / 2; + + DWord count = (code - low) / (range /= freq[256]); + + while (sSize > 0) + { + if (freq[symbol + sSize] <= count) + { + symbol = static_cast (symbol + sSize); + } + sSize /= 2; + } + + // write symbol to output stream + outputByteVector_arg[outputBufPos++] = symbol; + + low += freq[symbol] * range; + range *= freq[symbol + 1] - freq[symbol]; + + // check range limits + while ((low ^ (low + range)) < top || ((range < bottom) && ((range = -int (low) & (bottom - 1)), 1))) + { + std::uint8_t ch; + inputByteStream_arg.read (reinterpret_cast (&ch), sizeof(char)); + streamByteCount += sizeof(char); + code = code << 8 | ch; + range <<= 8; + low <<= 8; + } + + } + + return (streamByteCount); +} + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/lzf_image_io.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/lzf_image_io.hpp new file mode 100755 index 0000000..fe8360d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/lzf_image_io.hpp @@ -0,0 +1,526 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_LZF_IMAGE_IO_HPP_ +#define PCL_LZF_IMAGE_IO_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#define CLIP_CHAR(c) static_cast ((c)>255?255:(c)<0?0:(c)) + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFDepth16ImageReader::read ( + const std::string &filename, pcl::PointCloud &cloud) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight () * 2) + { + PCL_DEBUG ("[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 2, filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.is_dense = true; + cloud.resize (getWidth () * getHeight ()); + int depth_idx = 0, point_idx = 0; + double constant_x = 1.0 / parameters_.focal_length_x, + constant_y = 1.0 / parameters_.focal_length_y; + for (std::uint32_t v = 0; v < cloud.height; ++v) + { + for (std::uint32_t u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2) + { + PointT &pt = cloud.points[point_idx]; + unsigned short val; + memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short)); + if (val == 0) + { + pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); + cloud.is_dense = false; + continue; + } + + pt.z = static_cast (val * z_multiplication_factor_); + pt.x = (static_cast (u) - static_cast (parameters_.principal_point_x)) + * pt.z * static_cast (constant_x); + pt.y = (static_cast (v) - static_cast (parameters_.principal_point_y)) + * pt.z * static_cast (constant_y); + } + } + cloud.sensor_origin_.setZero (); + cloud.sensor_orientation_.w () = 1.0f; + cloud.sensor_orientation_.x () = 0.0f; + cloud.sensor_orientation_.y () = 0.0f; + cloud.sensor_orientation_.z () = 0.0f; + return (true); +} + +/////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename, + pcl::PointCloud &cloud, + unsigned int num_threads) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight () * 2) + { + PCL_DEBUG ("[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 2, filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.is_dense = true; + cloud.resize (getWidth () * getHeight ()); + double constant_x = 1.0 / parameters_.focal_length_x, + constant_y = 1.0 / parameters_.focal_length_y; +#ifdef _OPENMP +#pragma omp parallel for num_threads (num_threads) +#else + (void) num_threads; // suppress warning if OMP is not present +#endif + for (int i = 0; i < static_cast< int> (cloud.size ()); ++i) + { + int u = i % cloud.width; + int v = i / cloud.width; + PointT &pt = cloud.points[i]; + int depth_idx = 2*i; + unsigned short val; + memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short)); + if (val == 0) + { + pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); + if (cloud.is_dense) + { +#ifdef _OPENMP +#pragma omp critical +#endif + { + if (cloud.is_dense) + cloud.is_dense = false; + } + } + continue; + } + + pt.z = static_cast (val * z_multiplication_factor_); + pt.x = (static_cast (u) - static_cast (parameters_.principal_point_x)) + * pt.z * static_cast (constant_x); + pt.y = (static_cast (v) - static_cast (parameters_.principal_point_y)) + * pt.z * static_cast (constant_y); + + } + cloud.sensor_origin_.setZero (); + cloud.sensor_orientation_.w () = 1.0f; + cloud.sensor_orientation_.x () = 0.0f; + cloud.sensor_orientation_.y () = 0.0f; + cloud.sensor_orientation_.z () = 0.0f; + return (true); + +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFRGB24ImageReader::read ( + const std::string &filename, pcl::PointCloud &cloud) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight () * 3) + { + PCL_DEBUG ("[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 3, filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.resize (getWidth () * getHeight ()); + + int rgb_idx = 0; + unsigned char *color_r = reinterpret_cast (&uncompressed_data[0]); + unsigned char *color_g = reinterpret_cast (&uncompressed_data[getWidth () * getHeight ()]); + unsigned char *color_b = reinterpret_cast (&uncompressed_data[2 * getWidth () * getHeight ()]); + + for (std::size_t i = 0; i < cloud.size (); ++i, ++rgb_idx) + { + PointT &pt = cloud.points[i]; + + pt.b = color_b[rgb_idx]; + pt.g = color_g[rgb_idx]; + pt.r = color_r[rgb_idx]; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFRGB24ImageReader::readOMP ( + const std::string &filename, pcl::PointCloud &cloud, unsigned int num_threads) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight () * 3) + { + PCL_DEBUG ("[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 3, filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.resize (getWidth () * getHeight ()); + + unsigned char *color_r = reinterpret_cast (&uncompressed_data[0]); + unsigned char *color_g = reinterpret_cast (&uncompressed_data[getWidth () * getHeight ()]); + unsigned char *color_b = reinterpret_cast (&uncompressed_data[2 * getWidth () * getHeight ()]); + +#ifdef _OPENMP +#pragma omp parallel for num_threads (num_threads) +#else + (void) num_threads; // suppress warning if OMP is not present +#endif//_OPENMP + for (long int i = 0; i < cloud.size (); ++i) + { + PointT &pt = cloud.points[i]; + + pt.b = color_b[i]; + pt.g = color_g[i]; + pt.r = color_r[i]; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFYUV422ImageReader::read ( + const std::string &filename, pcl::PointCloud &cloud) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight () * 2) + { + PCL_DEBUG ("[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Convert YUV422 to RGB24 and copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.resize (getWidth () * getHeight ()); + + int wh2 = getWidth () * getHeight () / 2; + unsigned char *color_u = reinterpret_cast (&uncompressed_data[0]); + unsigned char *color_y = reinterpret_cast (&uncompressed_data[wh2]); + unsigned char *color_v = reinterpret_cast (&uncompressed_data[wh2 + getWidth () * getHeight ()]); + + int y_idx = 0; + for (int i = 0; i < wh2; ++i, y_idx += 2) + { + int v = color_v[i] - 128; + int u = color_u[i] - 128; + + PointT &pt1 = cloud.points[y_idx + 0]; + pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14)); + pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14)); + pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14)); + + PointT &pt2 = cloud.points[y_idx + 1]; + pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14)); + pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14)); + pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14)); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFYUV422ImageReader::readOMP ( + const std::string &filename, pcl::PointCloud &cloud, unsigned int num_threads) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight () * 2) + { + PCL_DEBUG ("[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Convert YUV422 to RGB24 and copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.resize (getWidth () * getHeight ()); + + int wh2 = getWidth () * getHeight () / 2; + unsigned char *color_u = reinterpret_cast (&uncompressed_data[0]); + unsigned char *color_y = reinterpret_cast (&uncompressed_data[wh2]); + unsigned char *color_v = reinterpret_cast (&uncompressed_data[wh2 + getWidth () * getHeight ()]); + +#ifdef _OPENMP +#pragma omp parallel for num_threads (num_threads) +#else + (void) num_threads; //suppress warning if OMP is not present +#endif//_OPENMP + for (int i = 0; i < wh2; ++i) + { + int y_idx = 2*i; + int v = color_v[i] - 128; + int u = color_u[i] - 128; + + PointT &pt1 = cloud.points[y_idx + 0]; + pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14)); + pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14)); + pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14)); + + PointT &pt2 = cloud.points[y_idx + 1]; + pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14)); + pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14)); + pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14)); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFBayer8ImageReader::read ( + const std::string &filename, pcl::PointCloud &cloud) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight ()) + { + PCL_DEBUG ("[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Convert Bayer8 to RGB24 + std::vector rgb_buffer (getWidth () * getHeight () * 3); + pcl::io::DeBayer i; + i.debayerEdgeAware (reinterpret_cast (&uncompressed_data[0]), + static_cast (&rgb_buffer[0]), + getWidth (), getHeight ()); + // Copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.resize (getWidth () * getHeight ()); + int rgb_idx = 0; + for (std::size_t i = 0; i < cloud.size (); ++i, rgb_idx += 3) + { + PointT &pt = cloud.points[i]; + + pt.b = rgb_buffer[rgb_idx + 2]; + pt.g = rgb_buffer[rgb_idx + 1]; + pt.r = rgb_buffer[rgb_idx + 0]; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::LZFBayer8ImageReader::readOMP ( + const std::string &filename, pcl::PointCloud &cloud, unsigned int num_threads) +{ + std::uint32_t uncompressed_size; + std::vector compressed_data; + if (!loadImageBlob (filename, compressed_data, uncompressed_size)) + { + PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ()); + return (false); + } + + if (uncompressed_size != getWidth () * getHeight ()) + { + PCL_DEBUG ("[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ()); + return (false); + } + + std::vector uncompressed_data (uncompressed_size); + decompress (compressed_data, uncompressed_data); + + if (uncompressed_data.empty ()) + { + PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ()); + return (false); + } + + // Convert Bayer8 to RGB24 + std::vector rgb_buffer (getWidth () * getHeight () * 3); + pcl::io::DeBayer i; + i.debayerEdgeAware (reinterpret_cast (&uncompressed_data[0]), + static_cast (&rgb_buffer[0]), + getWidth (), getHeight ()); + // Copy to PointT + cloud.width = getWidth (); + cloud.height = getHeight (); + cloud.resize (getWidth () * getHeight ()); +#ifdef _OPENMP +#pragma omp parallel for num_threads (num_threads) +#else + (void) num_threads; //suppress warning if OMP is not present +#endif//_OPENMP + for (long int i = 0; i < cloud.size (); ++i) + { + PointT &pt = cloud.points[i]; + long int rgb_idx = 3*i; + pt.b = rgb_buffer[rgb_idx + 2]; + pt.g = rgb_buffer[rgb_idx + 1]; + pt.r = rgb_buffer[rgb_idx + 0]; + } + return (true); +} + +#endif //#ifndef PCL_LZF_IMAGE_IO_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/octree_pointcloud_compression.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/octree_pointcloud_compression.hpp new file mode 100755 index 0000000..4374738 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/octree_pointcloud_compression.hpp @@ -0,0 +1,565 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef OCTREE_COMPRESSION_HPP +#define OCTREE_COMPRESSION_HPP + +#include + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace io + { + ////////////////////////////////////////////////////////////////////////////////////////////// + template void OctreePointCloudCompression< + PointT, LeafT, BranchT, OctreeT>::encodePointCloud ( + const PointCloudConstPtr &cloud_arg, + std::ostream& compressed_tree_data_out_arg) + { + unsigned char recent_tree_depth = + static_cast (this->getTreeDepth ()); + + // initialize octree + this->setInputCloud (cloud_arg); + + // add point to octree + this->addPointsFromInputCloud (); + + // make sure cloud contains points + if (this->leaf_count_>0) { + + + // color field analysis + cloud_with_color_ = false; + std::vector fields; + int rgba_index = -1; + rgba_index = pcl::getFieldIndex ("rgb", fields); + if (rgba_index == -1) + { + rgba_index = pcl::getFieldIndex ("rgba", fields); + } + if (rgba_index >= 0) + { + point_color_offset_ = static_cast (fields[rgba_index].offset); + cloud_with_color_ = true; + } + + // apply encoding configuration + cloud_with_color_ &= do_color_encoding_; + + + // if octree depth changed, we enforce I-frame encoding + i_frame_ |= (recent_tree_depth != this->getTreeDepth ());// | !(iFrameCounter%10); + + // enable I-frame rate + if (i_frame_counter_++==i_frame_rate_) + { + i_frame_counter_ =0; + i_frame_ = true; + } + + // increase frameID + frame_ID_++; + + // do octree encoding + if (!do_voxel_grid_enDecoding_) + { + point_count_data_vector_.clear (); + point_count_data_vector_.reserve (cloud_arg->points.size ()); + } + + // initialize color encoding + color_coder_.initializeEncoding (); + color_coder_.setPointCount (static_cast (cloud_arg->points.size ())); + color_coder_.setVoxelCount (static_cast (this->leaf_count_)); + + // initialize point encoding + point_coder_.initializeEncoding (); + point_coder_.setPointCount (static_cast (cloud_arg->points.size ())); + + // serialize octree + if (i_frame_) + // i-frame encoding - encode tree structure without referencing previous buffer + this->serializeTree (binary_tree_data_vector_, false); + else + // p-frame encoding - XOR encoded tree structure + this->serializeTree (binary_tree_data_vector_, true); + + + // write frame header information to stream + this->writeFrameHeader (compressed_tree_data_out_arg); + + // apply entropy coding to the content of all data vectors and send data to output stream + this->entropyEncoding (compressed_tree_data_out_arg); + + // prepare for next frame + this->switchBuffers (); + + // reset object count + object_count_ = 0; + + if (b_show_statistics_) + { + float bytes_per_XYZ = static_cast (compressed_point_data_len_) / static_cast (point_count_); + float bytes_per_color = static_cast (compressed_color_data_len_) / static_cast (point_count_); + + PCL_INFO ("*** POINTCLOUD ENCODING ***\n"); + PCL_INFO ("Frame ID: %d\n", frame_ID_); + if (i_frame_) + PCL_INFO ("Encoding Frame: Intra frame\n"); + else + PCL_INFO ("Encoding Frame: Prediction frame\n"); + PCL_INFO ("Number of encoded points: %ld\n", point_count_); + PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof (float)) * 100.0f); + PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ); + PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f); + PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color); + PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f); + PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast (compressed_point_data_len_ + compressed_color_data_len_) / 1024.0f); + PCL_INFO ("Total bytes per point: %f bytes\n", bytes_per_XYZ + bytes_per_color); + PCL_INFO ("Total compression percentage: %f%%\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f); + PCL_INFO ("Compression ratio: %f\n\n", static_cast (sizeof (int) + 3.0f * sizeof (float)) / static_cast (bytes_per_XYZ + bytes_per_color)); + } + + i_frame_ = false; + } else { + if (b_show_statistics_) + PCL_INFO ("Info: Dropping empty point cloud\n"); + this->deleteTree(); + i_frame_counter_ = 0; + i_frame_ = true; + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::decodePointCloud ( + std::istream& compressed_tree_data_in_arg, + PointCloudPtr &cloud_arg) + { + + // synchronize to frame header + syncToHeader(compressed_tree_data_in_arg); + + // initialize octree + this->switchBuffers (); + this->setOutputCloud (cloud_arg); + + // color field analysis + cloud_with_color_ = false; + std::vector fields; + int rgba_index = -1; + rgba_index = pcl::getFieldIndex ("rgb", fields); + if (rgba_index == -1) + rgba_index = pcl::getFieldIndex ("rgba", fields); + if (rgba_index >= 0) + { + point_color_offset_ = static_cast (fields[rgba_index].offset); + cloud_with_color_ = true; + } + + // read header from input stream + this->readFrameHeader (compressed_tree_data_in_arg); + + // decode data vectors from stream + this->entropyDecoding (compressed_tree_data_in_arg); + + // initialize color and point encoding + color_coder_.initializeDecoding (); + point_coder_.initializeDecoding (); + + // initialize output cloud + output_->points.clear (); + output_->points.reserve (static_cast (point_count_)); + + if (i_frame_) + // i-frame decoding - decode tree structure without referencing previous buffer + this->deserializeTree (binary_tree_data_vector_, false); + else + // p-frame decoding - decode XOR encoded tree structure + this->deserializeTree (binary_tree_data_vector_, true); + + // assign point cloud properties + output_->height = 1; + output_->width = static_cast (cloud_arg->points.size ()); + output_->is_dense = false; + + if (b_show_statistics_) + { + float bytes_per_XYZ = static_cast (compressed_point_data_len_) / static_cast (point_count_); + float bytes_per_color = static_cast (compressed_color_data_len_) / static_cast (point_count_); + + PCL_INFO ("*** POINTCLOUD DECODING ***\n"); + PCL_INFO ("Frame ID: %d\n", frame_ID_); + if (i_frame_) + PCL_INFO ("Decoding Frame: Intra frame\n"); + else + PCL_INFO ("Decoding Frame: Prediction frame\n"); + PCL_INFO ("Number of decoded points: %ld\n", point_count_); + PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof (float)) * 100.0f); + PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ); + PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f); + PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color); + PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f); + PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast (compressed_point_data_len_ + compressed_color_data_len_) / 1024.0f); + PCL_INFO ("Total bytes per point: %f bytes\n", bytes_per_XYZ + bytes_per_color); + PCL_INFO ("Total compression percentage: %f%%\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f); + PCL_INFO ("Compression ratio: %f\n\n", static_cast (sizeof (int) + 3.0f * sizeof (float)) / static_cast (bytes_per_XYZ + bytes_per_color)); + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::entropyEncoding (std::ostream& compressed_tree_data_out_arg) + { + std::uint64_t binary_tree_data_vector_size; + std::uint64_t point_avg_color_data_vector_size; + + compressed_point_data_len_ = 0; + compressed_color_data_len_ = 0; + + // encode binary octree structure + binary_tree_data_vector_size = binary_tree_data_vector_.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (binary_tree_data_vector_, + compressed_tree_data_out_arg); + + if (cloud_with_color_) + { + // encode averaged voxel color information + std::vector& pointAvgColorDataVector = color_coder_.getAverageDataVector (); + point_avg_color_data_vector_size = pointAvgColorDataVector.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_avg_color_data_vector_size), + sizeof (point_avg_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (pointAvgColorDataVector, + compressed_tree_data_out_arg); + } + + if (!do_voxel_grid_enDecoding_) + { + std::uint64_t pointCountDataVector_size; + std::uint64_t point_diff_data_vector_size; + std::uint64_t point_diff_color_data_vector_size; + + // encode amount of points per voxel + pointCountDataVector_size = point_count_data_vector_.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&pointCountDataVector_size), sizeof (pointCountDataVector_size)); + compressed_point_data_len_ += entropy_coder_.encodeIntVectorToStream (point_count_data_vector_, + compressed_tree_data_out_arg); + + // encode differential point information + std::vector& point_diff_data_vector = point_coder_.getDifferentialDataVector (); + point_diff_data_vector_size = point_diff_data_vector.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_data_vector, + compressed_tree_data_out_arg); + if (cloud_with_color_) + { + // encode differential color information + std::vector& point_diff_color_data_vector = color_coder_.getDifferentialDataVector (); + point_diff_color_data_vector_size = point_diff_color_data_vector.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_diff_color_data_vector_size), + sizeof (point_diff_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_color_data_vector, + compressed_tree_data_out_arg); + } + } + // flush output stream + compressed_tree_data_out_arg.flush (); + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::entropyDecoding (std::istream& compressed_tree_data_in_arg) + { + std::uint64_t binary_tree_data_vector_size; + std::uint64_t point_avg_color_data_vector_size; + + compressed_point_data_len_ = 0; + compressed_color_data_len_ = 0; + + // decode binary octree structure + compressed_tree_data_in_arg.read (reinterpret_cast (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size)); + binary_tree_data_vector_.resize (static_cast (binary_tree_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, + binary_tree_data_vector_); + + if (data_with_color_) + { + // decode averaged voxel color information + std::vector& point_avg_color_data_vector = color_coder_.getAverageDataVector (); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_avg_color_data_vector_size), sizeof (point_avg_color_data_vector_size)); + point_avg_color_data_vector.resize (static_cast (point_avg_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, + point_avg_color_data_vector); + } + + if (!do_voxel_grid_enDecoding_) + { + std::uint64_t point_count_data_vector_size; + std::uint64_t point_diff_data_vector_size; + std::uint64_t point_diff_color_data_vector_size; + + // decode amount of points per voxel + compressed_tree_data_in_arg.read (reinterpret_cast (&point_count_data_vector_size), sizeof (point_count_data_vector_size)); + point_count_data_vector_.resize (static_cast (point_count_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.decodeStreamToIntVector (compressed_tree_data_in_arg, point_count_data_vector_); + point_count_data_vector_iterator_ = point_count_data_vector_.begin (); + + // decode differential point information + std::vector& pointDiffDataVector = point_coder_.getDifferentialDataVector (); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size)); + pointDiffDataVector.resize (static_cast (point_diff_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, + pointDiffDataVector); + + if (data_with_color_) + { + // decode differential color information + std::vector& pointDiffColorDataVector = color_coder_.getDifferentialDataVector (); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_diff_color_data_vector_size), sizeof (point_diff_color_data_vector_size)); + pointDiffColorDataVector.resize (static_cast (point_diff_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, + pointDiffColorDataVector); + } + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::writeFrameHeader (std::ostream& compressed_tree_data_out_arg) + { + // encode header identifier + compressed_tree_data_out_arg.write (reinterpret_cast (frame_header_identifier_), strlen (frame_header_identifier_)); + // encode point cloud header id + compressed_tree_data_out_arg.write (reinterpret_cast (&frame_ID_), sizeof (frame_ID_)); + // encode frame type (I/P-frame) + compressed_tree_data_out_arg.write (reinterpret_cast (&i_frame_), sizeof (i_frame_)); + if (i_frame_) + { + double min_x, min_y, min_z, max_x, max_y, max_z; + double octree_resolution; + unsigned char color_bit_depth; + double point_resolution; + + // get current configuration + octree_resolution = this->getResolution (); + color_bit_depth = color_coder_.getBitDepth (); + point_resolution= point_coder_.getPrecision (); + this->getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); + + // encode amount of points + if (do_voxel_grid_enDecoding_) + point_count_ = this->leaf_count_; + else + point_count_ = this->object_count_; + + // encode coding configuration + compressed_tree_data_out_arg.write (reinterpret_cast (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_)); + compressed_tree_data_out_arg.write (reinterpret_cast (&cloud_with_color_), sizeof (cloud_with_color_)); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_count_), sizeof (point_count_)); + compressed_tree_data_out_arg.write (reinterpret_cast (&octree_resolution), sizeof (octree_resolution)); + compressed_tree_data_out_arg.write (reinterpret_cast (&color_bit_depth), sizeof (color_bit_depth)); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_resolution), sizeof (point_resolution)); + + // encode octree bounding box + compressed_tree_data_out_arg.write (reinterpret_cast (&min_x), sizeof (min_x)); + compressed_tree_data_out_arg.write (reinterpret_cast (&min_y), sizeof (min_y)); + compressed_tree_data_out_arg.write (reinterpret_cast (&min_z), sizeof (min_z)); + compressed_tree_data_out_arg.write (reinterpret_cast (&max_x), sizeof (max_x)); + compressed_tree_data_out_arg.write (reinterpret_cast (&max_y), sizeof (max_y)); + compressed_tree_data_out_arg.write (reinterpret_cast (&max_z), sizeof (max_z)); + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::syncToHeader ( std::istream& compressed_tree_data_in_arg) + { + // sync to frame header + unsigned int header_id_pos = 0; + while (header_id_pos < strlen (frame_header_identifier_)) + { + char readChar; + compressed_tree_data_in_arg.read (static_cast (&readChar), sizeof (readChar)); + if (readChar != frame_header_identifier_[header_id_pos++]) + header_id_pos = (frame_header_identifier_[0]==readChar)?1:0; + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::readFrameHeader ( std::istream& compressed_tree_data_in_arg) + { + // read header + compressed_tree_data_in_arg.read (reinterpret_cast (&frame_ID_), sizeof (frame_ID_)); + compressed_tree_data_in_arg.read (reinterpret_cast(&i_frame_), sizeof (i_frame_)); + if (i_frame_) + { + double min_x, min_y, min_z, max_x, max_y, max_z; + double octree_resolution; + unsigned char color_bit_depth; + double point_resolution; + + // read coder configuration + compressed_tree_data_in_arg.read (reinterpret_cast (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_)); + compressed_tree_data_in_arg.read (reinterpret_cast (&data_with_color_), sizeof (data_with_color_)); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_count_), sizeof (point_count_)); + compressed_tree_data_in_arg.read (reinterpret_cast (&octree_resolution), sizeof (octree_resolution)); + compressed_tree_data_in_arg.read (reinterpret_cast (&color_bit_depth), sizeof (color_bit_depth)); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_resolution), sizeof (point_resolution)); + + // read octree bounding box + compressed_tree_data_in_arg.read (reinterpret_cast (&min_x), sizeof (min_x)); + compressed_tree_data_in_arg.read (reinterpret_cast (&min_y), sizeof (min_y)); + compressed_tree_data_in_arg.read (reinterpret_cast (&min_z), sizeof (min_z)); + compressed_tree_data_in_arg.read (reinterpret_cast (&max_x), sizeof (max_x)); + compressed_tree_data_in_arg.read (reinterpret_cast (&max_y), sizeof (max_y)); + compressed_tree_data_in_arg.read (reinterpret_cast (&max_z), sizeof (max_z)); + + // reset octree and assign new bounding box & resolution + this->deleteTree (); + this->setResolution (octree_resolution); + this->defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); + + // configure color & point coding + color_coder_.setBitDepth (color_bit_depth); + point_coder_.setPrecision (static_cast (point_resolution)); + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::serializeTreeCallback ( + LeafT &leaf_arg, const OctreeKey & key_arg) + { + // reference to point indices vector stored within octree leaf + const std::vector& leafIdx = leaf_arg.getPointIndicesVector(); + + if (!do_voxel_grid_enDecoding_) + { + double lowerVoxelCorner[3]; + + // encode amount of points within voxel + point_count_data_vector_.push_back (static_cast (leafIdx.size ())); + + // calculate lower voxel corner based on octree key + lowerVoxelCorner[0] = static_cast (key_arg.x) * this->resolution_ + this->min_x_; + lowerVoxelCorner[1] = static_cast (key_arg.y) * this->resolution_ + this->min_y_; + lowerVoxelCorner[2] = static_cast (key_arg.z) * this->resolution_ + this->min_z_; + + // differentially encode points to lower voxel corner + point_coder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_); + + if (cloud_with_color_) + // encode color of points + color_coder_.encodePoints (leafIdx, point_color_offset_, this->input_); + } + else + { + if (cloud_with_color_) + // encode average color of all points within voxel + color_coder_.encodeAverageOfPoints (leafIdx, point_color_offset_, this->input_); + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OctreePointCloudCompression::deserializeTreeCallback (LeafT&, + const OctreeKey& key_arg) + { + PointT newPoint; + + std::size_t pointCount = 1; + + if (!do_voxel_grid_enDecoding_) + { + // get current cloud size + std::size_t cloudSize = output_->points.size (); + + // get amount of point to be decoded + pointCount = *point_count_data_vector_iterator_; + point_count_data_vector_iterator_++; + + // increase point cloud by amount of voxel points + for (std::size_t i = 0; i < pointCount; i++) + output_->points.push_back (newPoint); + + // calculcate position of lower voxel corner + double lowerVoxelCorner[3]; + lowerVoxelCorner[0] = static_cast (key_arg.x) * this->resolution_ + this->min_x_; + lowerVoxelCorner[1] = static_cast (key_arg.y) * this->resolution_ + this->min_y_; + lowerVoxelCorner[2] = static_cast (key_arg.z) * this->resolution_ + this->min_z_; + + // decode differentially encoded points + point_coder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount); + } + else + { + // calculate center of lower voxel corner + newPoint.x = static_cast ((static_cast (key_arg.x) + 0.5) * this->resolution_ + this->min_x_); + newPoint.y = static_cast ((static_cast (key_arg.y) + 0.5) * this->resolution_ + this->min_y_); + newPoint.z = static_cast ((static_cast (key_arg.z) + 0.5) * this->resolution_ + this->min_z_); + + // add point to point cloud + output_->points.push_back (newPoint); + } + + if (cloud_with_color_) + { + if (data_with_color_) + // decode color information + color_coder_.decodePoints (output_, output_->points.size () - pointCount, + output_->points.size (), point_color_offset_); + else + // set default color information + color_coder_.setDefaultColor (output_, output_->points.size () - pointCount, + output_->points.size (), point_color_offset_); + } + } + } +} + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/organized_pointcloud_compression.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/organized_pointcloud_compression.hpp new file mode 100755 index 0000000..b09bbea --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/organized_pointcloud_compression.hpp @@ -0,0 +1,448 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef ORGANIZED_COMPRESSION_HPP +#define ORGANIZED_COMPRESSION_HPP + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace pcl +{ + namespace io + { + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OrganizedPointCloudCompression::encodePointCloud (const PointCloudConstPtr &cloud_arg, + std::ostream& compressedDataOut_arg, + bool doColorEncoding, + bool convertToMono, + bool bShowStatistics_arg, + int pngLevel_arg) + { + std::uint32_t cloud_width = cloud_arg->width; + std::uint32_t cloud_height = cloud_arg->height; + + float maxDepth, focalLength, disparityShift, disparityScale; + + // no disparity scaling/shifting required during decoding + disparityScale = 1.0f; + disparityShift = 0.0f; + + analyzeOrganizedCloud (cloud_arg, maxDepth, focalLength); + + // encode header identifier + compressedDataOut_arg.write (reinterpret_cast (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_)); + // encode point cloud width + compressedDataOut_arg.write (reinterpret_cast (&cloud_width), sizeof (cloud_width)); + // encode frame type height + compressedDataOut_arg.write (reinterpret_cast (&cloud_height), sizeof (cloud_height)); + // encode frame max depth + compressedDataOut_arg.write (reinterpret_cast (&maxDepth), sizeof (maxDepth)); + // encode frame focal length + compressedDataOut_arg.write (reinterpret_cast (&focalLength), sizeof (focalLength)); + // encode frame disparity scale + compressedDataOut_arg.write (reinterpret_cast (&disparityScale), sizeof (disparityScale)); + // encode frame disparity shift + compressedDataOut_arg.write (reinterpret_cast (&disparityShift), sizeof (disparityShift)); + + // disparity and rgb image data + std::vector disparityData; + std::vector colorData; + + // compressed disparity and rgb image data + std::vector compressedDisparity; + std::vector compressedColor; + + std::uint32_t compressedDisparitySize = 0; + std::uint32_t compressedColorSize = 0; + + // Convert point cloud to disparity and rgb image + OrganizedConversion::convert (*cloud_arg, focalLength, disparityShift, disparityScale, convertToMono, disparityData, colorData); + + // Compress disparity information + encodeMonoImageToPNG (disparityData, cloud_width, cloud_height, compressedDisparity, pngLevel_arg); + + compressedDisparitySize = static_cast(compressedDisparity.size()); + // Encode size of compressed disparity image data + compressedDataOut_arg.write (reinterpret_cast (&compressedDisparitySize), sizeof (compressedDisparitySize)); + // Output compressed disparity to ostream + compressedDataOut_arg.write (reinterpret_cast (&compressedDisparity[0]), compressedDisparity.size () * sizeof(std::uint8_t)); + + // Compress color information + if (CompressionPointTraits::hasColor && doColorEncoding) + { + if (convertToMono) + { + encodeMonoImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/); + } else + { + encodeRGBImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/); + } + } + + compressedColorSize = static_cast(compressedColor.size ()); + // Encode size of compressed Color image data + compressedDataOut_arg.write (reinterpret_cast (&compressedColorSize), sizeof (compressedColorSize)); + // Output compressed disparity to ostream + compressedDataOut_arg.write (reinterpret_cast (&compressedColor[0]), compressedColor.size () * sizeof(std::uint8_t)); + + if (bShowStatistics_arg) + { + std::uint64_t pointCount = cloud_width * cloud_height; + float bytesPerPoint = static_cast (compressedDisparitySize+compressedColorSize) / static_cast (pointCount); + + PCL_INFO("*** POINTCLOUD ENCODING ***\n"); + PCL_INFO("Number of encoded points: %ld\n", pointCount); + PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast (pointCount) * CompressionPointTraits::bytesPerPoint) / 1024.0f); + PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast (compressedDisparitySize+compressedColorSize) / 1024.0f); + PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast (bytesPerPoint)); + PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits::bytesPerPoint) * 100.0f); + PCL_INFO("Compression ratio: %.2f\n\n", static_cast (CompressionPointTraits::bytesPerPoint) / bytesPerPoint); + } + + // flush output stream + compressedDataOut_arg.flush(); + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OrganizedPointCloudCompression::encodeRawDisparityMapWithColorImage ( std::vector& disparityMap_arg, + std::vector& colorImage_arg, + std::uint32_t width_arg, + std::uint32_t height_arg, + std::ostream& compressedDataOut_arg, + bool doColorEncoding, + bool convertToMono, + bool bShowStatistics_arg, + int pngLevel_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg) + { + float maxDepth = -1; + + std::size_t cloud_size = width_arg*height_arg; + assert (disparityMap_arg.size()==cloud_size); + if (!colorImage_arg.empty ()) + { + assert (colorImage_arg.size()==cloud_size*3); + } + + // encode header identifier + compressedDataOut_arg.write (reinterpret_cast (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_)); + // encode point cloud width + compressedDataOut_arg.write (reinterpret_cast (&width_arg), sizeof (width_arg)); + // encode frame type height + compressedDataOut_arg.write (reinterpret_cast (&height_arg), sizeof (height_arg)); + // encode frame max depth + compressedDataOut_arg.write (reinterpret_cast (&maxDepth), sizeof (maxDepth)); + // encode frame focal length + compressedDataOut_arg.write (reinterpret_cast (&focalLength_arg), sizeof (focalLength_arg)); + // encode frame disparity scale + compressedDataOut_arg.write (reinterpret_cast (&disparityScale_arg), sizeof (disparityScale_arg)); + // encode frame disparity shift + compressedDataOut_arg.write (reinterpret_cast (&disparityShift_arg), sizeof (disparityShift_arg)); + + // compressed disparity and rgb image data + std::vector compressedDisparity; + std::vector compressedColor; + + std::uint32_t compressedDisparitySize = 0; + std::uint32_t compressedColorSize = 0; + + // Remove color information of invalid points + std::uint16_t* depth_ptr = &disparityMap_arg[0]; + std::uint8_t* color_ptr = &colorImage_arg[0]; + + for (std::size_t i = 0; i < cloud_size; ++i, ++depth_ptr, color_ptr += sizeof(std::uint8_t) * 3) + { + if (!(*depth_ptr) || (*depth_ptr==0x7FF)) + memset(color_ptr, 0, sizeof(std::uint8_t)*3); + } + + // Compress disparity information + encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg); + + compressedDisparitySize = static_cast(compressedDisparity.size()); + // Encode size of compressed disparity image data + compressedDataOut_arg.write (reinterpret_cast (&compressedDisparitySize), sizeof (compressedDisparitySize)); + // Output compressed disparity to ostream + compressedDataOut_arg.write (reinterpret_cast (&compressedDisparity[0]), compressedDisparity.size () * sizeof(std::uint8_t)); + + // Compress color information + if (!colorImage_arg.empty () && doColorEncoding) + { + if (convertToMono) + { + std::vector monoImage; + std::size_t size = width_arg*height_arg; + + monoImage.reserve(size); + + // grayscale conversion + for (std::size_t i = 0; i < size; ++i) + { + std::uint8_t grayvalue = static_cast(0.2989 * static_cast(colorImage_arg[i*3+0]) + + 0.5870 * static_cast(colorImage_arg[i*3+1]) + + 0.1140 * static_cast(colorImage_arg[i*3+2])); + monoImage.push_back(grayvalue); + } + encodeMonoImageToPNG (monoImage, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/); + + } else + { + encodeRGBImageToPNG (colorImage_arg, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/); + } + + } + + compressedColorSize = static_cast(compressedColor.size ()); + // Encode size of compressed Color image data + compressedDataOut_arg.write (reinterpret_cast (&compressedColorSize), sizeof (compressedColorSize)); + // Output compressed disparity to ostream + compressedDataOut_arg.write (reinterpret_cast (&compressedColor[0]), compressedColor.size () * sizeof(std::uint8_t)); + + if (bShowStatistics_arg) + { + std::uint64_t pointCount = width_arg * height_arg; + float bytesPerPoint = static_cast (compressedDisparitySize+compressedColorSize) / static_cast (pointCount); + + PCL_INFO("*** POINTCLOUD ENCODING ***\n"); + PCL_INFO("Number of encoded points: %ld\n", pointCount); + PCL_INFO("Size of uncompressed disparity map+color image: %.2f kBytes\n", (static_cast (pointCount) * (sizeof(std::uint8_t)*3+sizeof(std::uint16_t))) / 1024.0f); + PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast (compressedDisparitySize+compressedColorSize) / 1024.0f); + PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast (bytesPerPoint)); + PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (sizeof(std::uint8_t)*3+sizeof(std::uint16_t)) * 100.0f); + PCL_INFO("Compression ratio: %.2f\n\n", static_cast (CompressionPointTraits::bytesPerPoint) / bytesPerPoint); + } + + // flush output stream + compressedDataOut_arg.flush(); + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template bool + OrganizedPointCloudCompression::decodePointCloud (std::istream& compressedDataIn_arg, + PointCloudPtr &cloud_arg, + bool bShowStatistics_arg) + { + std::uint32_t cloud_width; + std::uint32_t cloud_height; + float maxDepth; + float focalLength; + float disparityShift = 0.0f; + float disparityScale; + + // disparity and rgb image data + std::vector disparityData; + std::vector colorData; + + // compressed disparity and rgb image data + std::vector compressedDisparity; + std::vector compressedColor; + + std::uint32_t compressedDisparitySize; + std::uint32_t compressedColorSize; + + // PNG decoded parameters + std::size_t png_width = 0; + std::size_t png_height = 0; + unsigned int png_channels = 1; + + // sync to frame header + unsigned int headerIdPos = 0; + bool valid_stream = true; + while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_))) + { + char readChar; + compressedDataIn_arg.read (static_cast (&readChar), sizeof (readChar)); + if (compressedDataIn_arg.gcount()!= sizeof (readChar)) + valid_stream = false; + if (readChar != frameHeaderIdentifier_[headerIdPos++]) + headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0; + + valid_stream &= compressedDataIn_arg.good (); + } + + if (valid_stream) { + + ////////////// + // reading frame header + compressedDataIn_arg.read (reinterpret_cast (&cloud_width), sizeof (cloud_width)); + compressedDataIn_arg.read (reinterpret_cast (&cloud_height), sizeof (cloud_height)); + compressedDataIn_arg.read (reinterpret_cast (&maxDepth), sizeof (maxDepth)); + compressedDataIn_arg.read (reinterpret_cast (&focalLength), sizeof (focalLength)); + compressedDataIn_arg.read (reinterpret_cast (&disparityScale), sizeof (disparityScale)); + compressedDataIn_arg.read (reinterpret_cast (&disparityShift), sizeof (disparityShift)); + + // reading compressed disparity data + compressedDataIn_arg.read (reinterpret_cast (&compressedDisparitySize), sizeof (compressedDisparitySize)); + compressedDisparity.resize (compressedDisparitySize); + compressedDataIn_arg.read (reinterpret_cast (&compressedDisparity[0]), compressedDisparitySize * sizeof(std::uint8_t)); + + // reading compressed rgb data + compressedDataIn_arg.read (reinterpret_cast (&compressedColorSize), sizeof (compressedColorSize)); + compressedColor.resize (compressedColorSize); + compressedDataIn_arg.read (reinterpret_cast (&compressedColor[0]), compressedColorSize * sizeof(std::uint8_t)); + + // decode PNG compressed disparity data + decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels); + + // decode PNG compressed rgb data + decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels); + } + + if (disparityShift==0.0f) + { + // reconstruct point cloud + OrganizedConversion::convert (disparityData, + colorData, + (png_channels == 1), + cloud_width, + cloud_height, + focalLength, + disparityShift, + disparityScale, + *cloud_arg); + } else + { + + // we need to decode a raw shift image + std::size_t size = disparityData.size(); + std::vector depthData; + depthData.resize(size); + + // initialize shift-to-depth converter + if (!sd_converter_.isInitialized()) + sd_converter_.generateLookupTable(); + + // convert shift to depth image + for (std::size_t i=0; i::convert (depthData, + colorData, + static_cast(png_channels==1), + cloud_width, + cloud_height, + focalLength, + *cloud_arg); + } + + if (bShowStatistics_arg) + { + std::uint64_t pointCount = cloud_width * cloud_height; + float bytesPerPoint = static_cast (compressedDisparitySize+compressedColorSize) / static_cast (pointCount); + + PCL_INFO("*** POINTCLOUD DECODING ***\n"); + PCL_INFO("Number of encoded points: %ld\n", pointCount); + PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast (pointCount) * CompressionPointTraits::bytesPerPoint) / 1024.0f); + PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast (compressedDisparitySize+compressedColorSize) / 1024.0f); + PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast (bytesPerPoint)); + PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits::bytesPerPoint) * 100.0f); + PCL_INFO("Compression ratio: %.2f\n\n", static_cast (CompressionPointTraits::bytesPerPoint) / bytesPerPoint); + } + + return valid_stream; + } + + ////////////////////////////////////////////////////////////////////////////////////////////// + template void + OrganizedPointCloudCompression::analyzeOrganizedCloud (PointCloudConstPtr cloud_arg, + float& maxDepth_arg, + float& focalLength_arg) const + { + std::size_t width = cloud_arg->width; + std::size_t height = cloud_arg->height; + + // Center of organized point cloud + int centerX = static_cast (width / 2); + int centerY = static_cast (height / 2); + + // Ensure we have an organized point cloud + assert((width>1) && (height>1)); + assert(width*height == cloud_arg->points.size()); + + float maxDepth = 0; + float focalLength = 0; + + std::size_t it = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + const PointT& point = cloud_arg->points[it++]; + + if (pcl::isFinite (point)) + { + if (maxDepth < point.z) + { + // Update maximum depth + maxDepth = point.z; + + // Calculate focal length + focalLength = 2.0f / (point.x / (static_cast (x) * point.z) + point.y / (static_cast (y) * point.z)); + } + } + } + + // Update return values + maxDepth_arg = maxDepth; + focalLength_arg = focalLength; + } + + } +} + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/pcd_io.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/pcd_io.hpp new file mode 100755 index 0000000..6a78597 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/pcd_io.hpp @@ -0,0 +1,870 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_IO_PCD_IO_IMPL_H_ +#define PCL_IO_PCD_IO_IMPL_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::string +pcl::PCDWriter::generateHeader (const pcl::PointCloud &cloud, const int nr_points) +{ + std::ostringstream oss; + oss.imbue (std::locale::classic ()); + + oss << "# .PCD v0.7 - Point Cloud Data file format" + "\nVERSION 0.7" + "\nFIELDS"; + + const auto fields = pcl::getFields (); + + std::stringstream field_names, field_types, field_sizes, field_counts; + for (const auto &field : fields) + { + if (field.name == "_") + continue; + // Add the regular dimension + field_names << " " << field.name; + field_sizes << " " << pcl::getFieldSize (field.datatype); + if ("rgb" == field.name) + field_types << " " << "U"; + else + field_types << " " << pcl::getFieldType (field.datatype); + int count = std::abs (static_cast (field.count)); + if (count == 0) count = 1; // check for 0 counts (coming from older converter code) + field_counts << " " << count; + } + oss << field_names.str (); + oss << "\nSIZE" << field_sizes.str () + << "\nTYPE" << field_types.str () + << "\nCOUNT" << field_counts.str (); + // If the user passes in a number of points value, use that instead + if (nr_points != std::numeric_limits::max ()) + oss << "\nWIDTH " << nr_points << "\nHEIGHT " << 1 << "\n"; + else + oss << "\nWIDTH " << cloud.width << "\nHEIGHT " << cloud.height << "\n"; + + oss << "VIEWPOINT " << cloud.sensor_origin_[0] << " " << cloud.sensor_origin_[1] << " " << cloud.sensor_origin_[2] << " " << + cloud.sensor_orientation_.w () << " " << + cloud.sensor_orientation_.x () << " " << + cloud.sensor_orientation_.y () << " " << + cloud.sensor_orientation_.z () << "\n"; + + // If the user passes in a number of points value, use that instead + if (nr_points != std::numeric_limits::max ()) + oss << "POINTS " << nr_points << "\n"; + else + oss << "POINTS " << cloud.points.size () << "\n"; + + return (oss.str ()); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::PCDWriter::writeBinary (const std::string &file_name, + const pcl::PointCloud &cloud) +{ + if (cloud.empty ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!"); + return (-1); + } + int data_idx = 0; + std::ostringstream oss; + oss << generateHeader (cloud) << "DATA binary\n"; + oss.flush (); + data_idx = static_cast (oss.tellp ()); + +#ifdef _WIN32 + HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); + if (h_native_file == INVALID_HANDLE_VALUE) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!"); + return (-1); + } +#else + int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); + if (fd < 0) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!"); + return (-1); + } +#endif + // Mandatory lock file + boost::interprocess::file_lock file_lock; + setLockingPermissions (file_name, file_lock); + + auto fields = pcl::getFields (); + std::vector fields_sizes; + std::size_t fsize = 0; + std::size_t data_size = 0; + std::size_t nri = 0; + // Compute the total size of the fields + for (const auto &field : fields) + { + if (field.name == "_") + continue; + + int fs = field.count * getFieldSize (field.datatype); + fsize += fs; + fields_sizes.push_back (fs); + fields[nri++] = field; + } + fields.resize (nri); + + data_size = cloud.points.size () * fsize; + + // Prepare the map +#ifdef _WIN32 + HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL); + if (fm == NULL) + { + throw pcl::IOException("[pcl::PCDWriter::writeBinary] Error during memory map creation ()!"); + return (-1); + } + char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size)); + CloseHandle (fm); + +#else + // Allocate disk space for the entire file to prevent bus errors. + if (io::raw_fallocate (fd, data_idx + data_size) != 0) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + PCL_ERROR ("[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno)); + + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!"); + return (-1); + } + + char *map = static_cast (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0)); + if (map == reinterpret_cast (-1)) //MAP_FAILED) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!"); + return (-1); + } +#endif + + // Copy the header + memcpy (&map[0], oss.str ().c_str (), data_idx); + + // Copy the data + char *out = &map[0] + data_idx; + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + int nrj = 0; + for (const auto &field : fields) + { + memcpy (out, reinterpret_cast (&cloud.points[i]) + field.offset, fields_sizes[nrj]); + out += fields_sizes[nrj++]; + } + } + + // If the user set the synchronization flag on, call msync +#ifndef _WIN32 + if (map_synchronization_) + msync (map, data_idx + data_size, MS_SYNC); +#endif + + // Unmap the pages of memory +#ifdef _WIN32 + UnmapViewOfFile (map); +#else + if (::munmap (map, (data_idx + data_size)) == -1) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!"); + return (-1); + } +#endif + // Close file +#ifdef _WIN32 + CloseHandle (h_native_file); +#else + io::raw_close (fd); +#endif + resetLockingPermissions (file_name, file_lock); + return (0); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, + const pcl::PointCloud &cloud) +{ + if (cloud.points.empty ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Input point cloud has no data!"); + return (-1); + } + int data_idx = 0; + std::ostringstream oss; + oss << generateHeader (cloud) << "DATA binary_compressed\n"; + oss.flush (); + data_idx = static_cast (oss.tellp ()); + +#ifdef _WIN32 + HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); + if (h_native_file == INVALID_HANDLE_VALUE) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during CreateFile!"); + return (-1); + } +#else + int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); + if (fd < 0) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during open!"); + return (-1); + } +#endif + + // Mandatory lock file + boost::interprocess::file_lock file_lock; + setLockingPermissions (file_name, file_lock); + + auto fields = pcl::getFields (); + std::size_t fsize = 0; + std::size_t data_size = 0; + std::size_t nri = 0; + std::vector fields_sizes (fields.size ()); + // Compute the total size of the fields + for (const auto &field : fields) + { + if (field.name == "_") + continue; + + fields_sizes[nri] = field.count * pcl::getFieldSize (field.datatype); + fsize += fields_sizes[nri]; + fields[nri] = field; + ++nri; + } + fields_sizes.resize (nri); + fields.resize (nri); + + // Compute the size of data + data_size = cloud.points.size () * fsize; + + // If the data is to large the two 32 bit integers used to store the + // compressed and uncompressed size will overflow. + if (data_size * 3 / 2 > std::numeric_limits::max ()) + { + PCL_ERROR ("[pcl::PCDWriter::writeBinaryCompressed] The input data exceeds the maximum size for compressed version 0.7 pcds of %l bytes.\n", + static_cast (std::numeric_limits::max ()) * 2 / 3); + return (-2); + } + + ////////////////////////////////////////////////////////////////////// + // Empty array holding only the valid data + // data_size = nr_points * point_size + // = nr_points * (sizeof_field_1 + sizeof_field_2 + ... sizeof_field_n) + // = sizeof_field_1 * nr_points + sizeof_field_2 * nr_points + ... sizeof_field_n * nr_points + char *only_valid_data = static_cast (malloc (data_size)); + + // Convert the XYZRGBXYZRGB structure to XXYYZZRGBRGB to aid compression. For + // this, we need a vector of fields.size () (4 in this case), which points to + // each individual plane: + // pters[0] = &only_valid_data[offset_of_plane_x]; + // pters[1] = &only_valid_data[offset_of_plane_y]; + // pters[2] = &only_valid_data[offset_of_plane_z]; + // pters[3] = &only_valid_data[offset_of_plane_RGB]; + // + std::vector pters (fields.size ()); + std::size_t toff = 0; + for (std::size_t i = 0; i < pters.size (); ++i) + { + pters[i] = &only_valid_data[toff]; + toff += static_cast(fields_sizes[i]) * cloud.points.size(); + } + + // Go over all the points, and copy the data in the appropriate places + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + for (std::size_t j = 0; j < fields.size (); ++j) + { + memcpy (pters[j], reinterpret_cast (&cloud.points[i]) + fields[j].offset, fields_sizes[j]); + // Increment the pointer + pters[j] += fields_sizes[j]; + } + } + + char* temp_buf = static_cast (malloc (static_cast (static_cast (data_size) * 1.5f + 8.0f))); + // Compress the valid data + unsigned int compressed_size = pcl::lzfCompress (only_valid_data, + static_cast (data_size), + &temp_buf[8], + static_cast (static_cast(data_size) * 1.5f)); + unsigned int compressed_final_size = 0; + // Was the compression successful? + if (compressed_size) + { + char *header = &temp_buf[0]; + memcpy (&header[0], &compressed_size, sizeof (unsigned int)); + memcpy (&header[4], &data_size, sizeof (unsigned int)); + data_size = compressed_size + 8; + compressed_final_size = static_cast (data_size) + data_idx; + } + else + { +#ifndef _WIN32 + io::raw_close (fd); +#endif + resetLockingPermissions (file_name, file_lock); + throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during compression!"); + return (-1); + } + + // Prepare the map +#ifdef _WIN32 + HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL); + char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size)); + CloseHandle (fm); + +#else + // Allocate disk space for the entire file to prevent bus errors. + if (io::raw_fallocate (fd, compressed_final_size) != 0) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + PCL_ERROR ("[pcl::PCDWriter::writeBinaryCompressed] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno)); + + throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during posix_fallocate ()!"); + return (-1); + } + + char *map = static_cast (::mmap (nullptr, compressed_final_size, PROT_WRITE, MAP_SHARED, fd, 0)); + if (map == reinterpret_cast (-1)) //MAP_FAILED) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during mmap ()!"); + return (-1); + } +#endif + + // Copy the header + memcpy (&map[0], oss.str ().c_str (), data_idx); + // Copy the compressed data + memcpy (&map[data_idx], temp_buf, data_size); + +#ifndef _WIN32 + // If the user set the synchronization flag on, call msync + if (map_synchronization_) + msync (map, compressed_final_size, MS_SYNC); +#endif + + // Unmap the pages of memory +#ifdef _WIN32 + UnmapViewOfFile (map); +#else + if (::munmap (map, (compressed_final_size)) == -1) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + throw pcl::IOException ("[pcl::PCDWriter::writeBinaryCompressed] Error during munmap ()!"); + return (-1); + } +#endif + + // Close file +#ifdef _WIN32 + CloseHandle (h_native_file); +#else + io::raw_close (fd); +#endif + resetLockingPermissions (file_name, file_lock); + + free (only_valid_data); + free (temp_buf); + return (0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PointCloud &cloud, + const int precision) +{ + if (cloud.empty ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data!"); + return (-1); + } + + if (cloud.width * cloud.height != cloud.points.size ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!"); + return (-1); + } + + std::ofstream fs; + fs.open (file_name.c_str ()); // Open file + + if (!fs.is_open () || fs.fail ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!"); + return (-1); + } + + // Mandatory lock file + boost::interprocess::file_lock file_lock; + setLockingPermissions (file_name, file_lock); + + fs.precision (precision); + fs.imbue (std::locale::classic ()); + + const auto fields = pcl::getFields (); + + // Write the header information + fs << generateHeader (cloud) << "DATA ascii\n"; + + std::ostringstream stream; + stream.precision (precision); + stream.imbue (std::locale::classic ()); + // Iterate through the points + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + for (std::size_t d = 0; d < fields.size (); ++d) + { + // Ignore invalid padded dimensions that are inherited from binary data + if (fields[d].name == "_") + continue; + + int count = fields[d].count; + if (count == 0) + count = 1; // we simply cannot tolerate 0 counts (coming from older converter code) + + for (int c = 0; c < count; ++c) + { + switch (fields[d].datatype) + { + case pcl::PCLPointField::INT8: + { + std::int8_t value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT8: + { + std::uint8_t value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::INT16: + { + std::int16_t value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT16: + { + std::uint16_t value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::INT32: + { + std::int32_t value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT32: + { + std::uint32_t value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::FLOAT32: + { + /* + * Despite the float type, store the rgb field as uint32 + * because several fully opaque color values are mapped to + * nan. + */ + if ("rgb" == fields[d].name) + { + std::uint32_t value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float)); + stream << boost::numeric_cast(value); + break; + } + float value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (float), sizeof (float)); + if (std::isnan (value)) + stream << "nan"; + else + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::FLOAT64: + { + double value; + memcpy (&value, reinterpret_cast (&cloud.points[i]) + fields[d].offset + c * sizeof (double), sizeof (double)); + if (std::isnan (value)) + stream << "nan"; + else + stream << boost::numeric_cast(value); + break; + } + default: + PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype); + break; + } + + if (d < fields.size () - 1 || c < static_cast (fields[d].count - 1)) + stream << " "; + } + } + // Copy the stream, trim it, and write it to disk + std::string result = stream.str (); + boost::trim (result); + stream.str (""); + fs << result << "\n"; + } + fs.close (); // Close file + resetLockingPermissions (file_name, file_lock); + return (0); +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::PCDWriter::writeBinary (const std::string &file_name, + const pcl::PointCloud &cloud, + const std::vector &indices) +{ + if (cloud.points.empty () || indices.empty ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Input point cloud has no data or empty indices given!"); + return (-1); + } + int data_idx = 0; + std::ostringstream oss; + oss << generateHeader (cloud, static_cast (indices.size ())) << "DATA binary\n"; + oss.flush (); + data_idx = static_cast (oss.tellp ()); + +#ifdef _WIN32 + HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); + if (h_native_file == INVALID_HANDLE_VALUE) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during CreateFile!"); + return (-1); + } +#else + int fd = io::raw_open (file_name.c_str (), O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); + if (fd < 0) + { + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during open!"); + return (-1); + } +#endif + // Mandatory lock file + boost::interprocess::file_lock file_lock; + setLockingPermissions (file_name, file_lock); + + auto fields = pcl::getFields (); + std::vector fields_sizes; + std::size_t fsize = 0; + std::size_t data_size = 0; + std::size_t nri = 0; + // Compute the total size of the fields + for (const auto &field : fields) + { + if (field.name == "_") + continue; + + int fs = field.count * getFieldSize (field.datatype); + fsize += fs; + fields_sizes.push_back (fs); + fields[nri++] = field; + } + fields.resize (nri); + + data_size = indices.size () * fsize; + + // Prepare the map +#ifdef _WIN32 + HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL); + char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size)); + CloseHandle (fm); + +#else + // Allocate disk space for the entire file to prevent bus errors. + if (io::raw_fallocate (fd, data_idx + data_size) != 0) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + PCL_ERROR ("[pcl::PCDWriter::writeBinary] posix_fallocate errno: %d strerror: %s\n", errno, strerror (errno)); + + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during posix_fallocate ()!"); + return (-1); + } + + char *map = static_cast (::mmap (nullptr, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0)); + if (map == reinterpret_cast (-1)) //MAP_FAILED) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during mmap ()!"); + return (-1); + } +#endif + + // Copy the header + memcpy (&map[0], oss.str ().c_str (), data_idx); + + char *out = &map[0] + data_idx; + // Copy the data + for (const int &index : indices) + { + int nrj = 0; + for (const auto &field : fields) + { + memcpy (out, reinterpret_cast (&cloud.points[index]) + field.offset, fields_sizes[nrj]); + out += fields_sizes[nrj++]; + } + } + +#ifndef _WIN32 + // If the user set the synchronization flag on, call msync + if (map_synchronization_) + msync (map, data_idx + data_size, MS_SYNC); +#endif + + // Unmap the pages of memory +#ifdef _WIN32 + UnmapViewOfFile (map); +#else + if (::munmap (map, (data_idx + data_size)) == -1) + { + io::raw_close (fd); + resetLockingPermissions (file_name, file_lock); + throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!"); + return (-1); + } +#endif + // Close file +#ifdef _WIN32 + CloseHandle(h_native_file); +#else + io::raw_close (fd); +#endif + + resetLockingPermissions (file_name, file_lock); + return (0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::PCDWriter::writeASCII (const std::string &file_name, + const pcl::PointCloud &cloud, + const std::vector &indices, + const int precision) +{ + if (cloud.points.empty () || indices.empty ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!"); + return (-1); + } + + if (cloud.width * cloud.height != cloud.points.size ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!"); + return (-1); + } + + std::ofstream fs; + fs.open (file_name.c_str ()); // Open file + if (!fs.is_open () || fs.fail ()) + { + throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!"); + return (-1); + } + + // Mandatory lock file + boost::interprocess::file_lock file_lock; + setLockingPermissions (file_name, file_lock); + + fs.precision (precision); + fs.imbue (std::locale::classic ()); + + const auto fields = pcl::getFields (); + + // Write the header information + fs << generateHeader (cloud, static_cast (indices.size ())) << "DATA ascii\n"; + + std::ostringstream stream; + stream.precision (precision); + stream.imbue (std::locale::classic ()); + + // Iterate through the points + for (const int &index : indices) + { + for (std::size_t d = 0; d < fields.size (); ++d) + { + // Ignore invalid padded dimensions that are inherited from binary data + if (fields[d].name == "_") + continue; + + int count = fields[d].count; + if (count == 0) + count = 1; // we simply cannot tolerate 0 counts (coming from older converter code) + + for (int c = 0; c < count; ++c) + { + switch (fields[d].datatype) + { + case pcl::PCLPointField::INT8: + { + std::int8_t value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int8_t), sizeof (std::int8_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT8: + { + std::uint8_t value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint8_t), sizeof (std::uint8_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::INT16: + { + std::int16_t value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int16_t), sizeof (std::int16_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT16: + { + std::uint16_t value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint16_t), sizeof (std::uint16_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::INT32: + { + std::int32_t value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (std::int32_t), sizeof (std::int32_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::UINT32: + { + std::uint32_t value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (std::uint32_t), sizeof (std::uint32_t)); + stream << boost::numeric_cast(value); + break; + } + case pcl::PCLPointField::FLOAT32: + { + /* + * Despite the float type, store the rgb field as uint32 + * because several fully opaque color values are mapped to + * nan. + */ + if ("rgb" == fields[d].name) + { + std::uint32_t value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (float), sizeof (float)); + stream << boost::numeric_cast(value); + } + else + { + float value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (float), sizeof (float)); + if (std::isnan (value)) + stream << "nan"; + else + stream << boost::numeric_cast(value); + } + break; + } + case pcl::PCLPointField::FLOAT64: + { + double value; + memcpy (&value, reinterpret_cast (&cloud.points[index]) + fields[d].offset + c * sizeof (double), sizeof (double)); + if (std::isnan (value)) + stream << "nan"; + else + stream << boost::numeric_cast(value); + break; + } + default: + PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype); + break; + } + + if (d < fields.size () - 1 || c < static_cast (fields[d].count - 1)) + stream << " "; + } + } + // Copy the stream, trim it, and write it to disk + std::string result = stream.str (); + boost::trim (result); + stream.str (""); + fs << result << "\n"; + } + fs.close (); // Close file + + resetLockingPermissions (file_name, file_lock); + + return (0); +} + +#endif //#ifndef PCL_IO_PCD_IO_H_ diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/point_cloud_image_extractors.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/point_cloud_image_extractors.hpp new file mode 100755 index 0000000..526b3a8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/point_cloud_image_extractors.hpp @@ -0,0 +1,300 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ +#define PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ + +#include +#include +#include +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::PointCloudImageExtractor::extract (const PointCloud& cloud, pcl::PCLImage& img) const +{ + if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height) + return (false); + + bool result = this->extractImpl (cloud, img); + + if (paint_nans_with_black_ && result) + { + std::size_t size = img.encoding == "mono16" ? 2 : 3; + for (std::size_t i = 0; i < cloud.points.size (); ++i) + if (!pcl::isFinite (cloud[i])) + std::memset (&img.data[i * size], 0, size); + } + + return (result); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::PointCloudImageExtractorFromNormalField::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const +{ + std::vector fields; + int field_x_idx = pcl::getFieldIndex ("normal_x", fields); + int field_y_idx = pcl::getFieldIndex ("normal_y", fields); + int field_z_idx = pcl::getFieldIndex ("normal_z", fields); + if (field_x_idx == -1 || field_y_idx == -1 || field_z_idx == -1) + return (false); + const std::size_t offset_x = fields[field_x_idx].offset; + const std::size_t offset_y = fields[field_y_idx].offset; + const std::size_t offset_z = fields[field_z_idx].offset; + + img.encoding = "rgb8"; + img.width = cloud.width; + img.height = cloud.height; + img.step = img.width * sizeof (unsigned char) * 3; + img.data.resize (img.step * img.height); + + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + float x; + float y; + float z; + pcl::getFieldValue (cloud.points[i], offset_x, x); + pcl::getFieldValue (cloud.points[i], offset_y, y); + pcl::getFieldValue (cloud.points[i], offset_z, z); + img.data[i * 3 + 0] = static_cast((x + 1.0) * 127); + img.data[i * 3 + 1] = static_cast((y + 1.0) * 127); + img.data[i * 3 + 2] = static_cast((z + 1.0) * 127); + } + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::PointCloudImageExtractorFromRGBField::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const +{ + std::vector fields; + int field_idx = pcl::getFieldIndex ("rgb", fields); + if (field_idx == -1) + { + field_idx = pcl::getFieldIndex ("rgba", fields); + if (field_idx == -1) + return (false); + } + const std::size_t offset = fields[field_idx].offset; + + img.encoding = "rgb8"; + img.width = cloud.width; + img.height = cloud.height; + img.step = img.width * sizeof (unsigned char) * 3; + img.data.resize (img.step * img.height); + + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + std::uint32_t val; + pcl::getFieldValue (cloud.points[i], offset, val); + img.data[i * 3 + 0] = (val >> 16) & 0x0000ff; + img.data[i * 3 + 1] = (val >> 8) & 0x0000ff; + img.data[i * 3 + 2] = (val) & 0x0000ff; + } + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::PointCloudImageExtractorFromLabelField::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const +{ + std::vector fields; + int field_idx = pcl::getFieldIndex ("label", fields); + if (field_idx == -1) + return (false); + const std::size_t offset = fields[field_idx].offset; + + switch (color_mode_) + { + case COLORS_MONO: + { + img.encoding = "mono16"; + img.width = cloud.width; + img.height = cloud.height; + img.step = img.width * sizeof (unsigned short); + img.data.resize (img.step * img.height); + unsigned short* data = reinterpret_cast(&img.data[0]); + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + std::uint32_t val; + pcl::getFieldValue (cloud.points[i], offset, val); + data[i] = static_cast(val); + } + break; + } + case COLORS_RGB_RANDOM: + { + img.encoding = "rgb8"; + img.width = cloud.width; + img.height = cloud.height; + img.step = img.width * sizeof (unsigned char) * 3; + img.data.resize (img.step * img.height); + + std::srand(std::time(nullptr)); + std::map colormap; + + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + std::uint32_t val; + pcl::getFieldValue (cloud.points[i], offset, val); + if (colormap.count (val) == 0) + { + colormap[val] = i * 3; + img.data[i * 3 + 0] = static_cast ((std::rand () % 256)); + img.data[i * 3 + 1] = static_cast ((std::rand () % 256)); + img.data[i * 3 + 2] = static_cast ((std::rand () % 256)); + } + else + { + memcpy (&img.data[i * 3], &img.data[colormap[val]], 3); + } + } + break; + } + case COLORS_RGB_GLASBEY: + { + img.encoding = "rgb8"; + img.width = cloud.width; + img.height = cloud.height; + img.step = img.width * sizeof (unsigned char) * 3; + img.data.resize (img.step * img.height); + + std::srand(std::time(nullptr)); + std::set labels; + std::map colormap; + + // First pass: find unique labels + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + // If we need to paint NaN points with black do not waste colors on them + if (paint_nans_with_black_ && !pcl::isFinite (cloud.points[i])) + continue; + std::uint32_t val; + pcl::getFieldValue (cloud.points[i], offset, val); + labels.insert (val); + } + + // Assign Glasbey colors in ascending order of labels + // Note: the color LUT has a finite size (256 colors), therefore when + // there are more labels the colors will repeat + std::size_t color = 0; + for (const std::uint32_t &label : labels) + { + colormap[label] = color % GlasbeyLUT::size (); + ++color; + } + + // Second pass: copy colors from the LUT + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + std::uint32_t val; + pcl::getFieldValue (cloud.points[i], offset, val); + memcpy (&img.data[i * 3], GlasbeyLUT::data () + colormap[val] * 3, 3); + } + + break; + } + } + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::io::PointCloudImageExtractorWithScaling::extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const +{ + std::vector fields; + int field_idx = pcl::getFieldIndex (field_name_, fields); + if (field_idx == -1) + return (false); + const std::size_t offset = fields[field_idx].offset; + + img.encoding = "mono16"; + img.width = cloud.width; + img.height = cloud.height; + img.step = img.width * sizeof (unsigned short); + img.data.resize (img.step * img.height); + unsigned short* data = reinterpret_cast(&img.data[0]); + + float scaling_factor = scaling_factor_; + float data_min = 0.0f; + if (scaling_method_ == SCALING_FULL_RANGE) + { + float min = std::numeric_limits::infinity(); + float max = -std::numeric_limits::infinity(); + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + float val; + pcl::getFieldValue (cloud.points[i], offset, val); + if (val < min) + min = val; + if (val > max) + max = val; + } + scaling_factor = min == max ? 0 : std::numeric_limits::max() / (max - min); + data_min = min; + } + + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + float val; + pcl::getFieldValue (cloud.points[i], offset, val); + if (scaling_method_ == SCALING_NO) + { + data[i] = val; + } + else if (scaling_method_ == SCALING_FULL_RANGE) + { + data[i] = (val - data_min) * scaling_factor; + } + else if (scaling_method_ == SCALING_FIXED_FACTOR) + { + data[i] = val * scaling_factor; + } + } + + return (true); +} + +#endif // PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/io/impl/synchronized_queue.hpp b/deps_install/include/pcl-1.10/pcl/io/impl/synchronized_queue.hpp new file mode 100755 index 0000000..fe9aaf8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/impl/synchronized_queue.hpp @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012 + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/* SynchronizedQueue Template, taken from + * http://stackoverflow.com/questions/10139251/shared-queue-c + */ +#pragma once + +#include +#include +#include + +namespace pcl +{ + + template + class SynchronizedQueue + { + public: + + SynchronizedQueue () : + queue_(), request_to_end_(false), enqueue_data_(true) { } + + void + enqueue (const T& data) + { + std::unique_lock lock (mutex_); + + if (enqueue_data_) + { + queue_.push (data); + cond_.notify_one (); + } + } + + bool + dequeue (T& result) + { + std::unique_lock lock (mutex_); + + while (queue_.empty () && (!request_to_end_)) + { + cond_.wait (lock); + } + + if (request_to_end_) + { + doEndActions (); + return false; + } + + result = queue_.front (); + queue_.pop (); + + return true; + } + + void + stopQueue () + { + std::unique_lock lock (mutex_); + request_to_end_ = true; + cond_.notify_one (); + } + + unsigned int + size () + { + std::unique_lock lock (mutex_); + return static_cast (queue_.size ()); + } + + bool + isEmpty () const + { + std::unique_lock lock (mutex_); + return (queue_.empty ()); + } + + private: + void + doEndActions () + { + enqueue_data_ = false; + + while (!queue_.empty ()) + { + queue_.pop (); + } + } + + std::queue queue_; // Use STL queue to store data + mutable std::mutex mutex_; // The mutex to synchronise on + std::condition_variable cond_; // The condition to wait for + + bool request_to_end_; + bool enqueue_data_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/io/io.h b/deps_install/include/pcl-1.10/pcl/io/io.h new file mode 100755 index 0000000..e579aab --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/io.h @@ -0,0 +1,42 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include diff --git a/deps_install/include/pcl-1.10/pcl/io/io_exception.h b/deps_install/include/pcl-1.10/pcl/io/io_exception.h new file mode 100755 index 0000000..c78f0ec --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/io_exception.h @@ -0,0 +1,105 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + +#include +#include +#include +#include + + +//fom +#if defined _WIN32 && defined _MSC_VER && !defined __PRETTY_FUNCTION__ + #define __PRETTY_FUNCTION__ __FUNCTION__ +#endif + + +#define THROW_IO_EXCEPTION(format,...) throwIOException( __PRETTY_FUNCTION__, __FILE__, __LINE__, format , ##__VA_ARGS__ ) + + +namespace pcl +{ + namespace io + { + /** + * @brief General IO exception class + */ + class IOException : public std::exception + { + public: + IOException (const std::string& function_name, + const std::string& file_name, + unsigned line_number, + const std::string& message); + + ~IOException () noexcept; + + IOException& + operator= (const IOException& exception); + + const char* + what () const throw () override; + + const std::string& + getFunctionName () const; + + const std::string& + getFileName () const; + + unsigned + getLineNumber () const; + + protected: + std::string function_name_; + std::string file_name_; + unsigned line_number_; + std::string message_; + std::string message_long_; + }; + + inline void + throwIOException (const char* function, const char* file, unsigned line, const char* format, ...) + { + static char msg[1024]; + va_list args; + va_start (args, format); + vsnprintf (msg, 1024, format, args); + throw IOException (function, file, line, msg); + } + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/low_level_io.h b/deps_install/include/pcl-1.10/pcl/io/low_level_io.h new file mode 100755 index 0000000..39ad6d2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/low_level_io.h @@ -0,0 +1,214 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2018 Fizyr BV. - https://fizyr.com + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * This file defines compatibility wrappers for low level I/O functions. + * Implemented as inlinable functions to prevent any performance overhead. + */ + +#pragma once + +#ifdef _WIN32 +# ifndef WIN32_LEAN_AND_MEAN +# define WIN32_LEAN_AND_MEAN +# endif +# ifndef NOMINMAX +# define NOMINMAX +# endif +# include +# include +# include +using ssize_t = SSIZE_T; +#else +# include +# include +# include +# include +# include +# include +#endif +#include + +namespace pcl +{ + namespace io + { +#ifdef _WIN32 + inline int raw_open(const char * pathname, int flags, int mode) + { + return ::_open(pathname, flags, mode); + } + + inline int raw_open(const char * pathname, int flags) + { + return ::_open(pathname, flags); + } + + inline int raw_close(int fd) + { + return ::_close(fd); + } + + inline int raw_lseek(int fd, long offset, int whence) + { + return ::_lseek(fd, offset, whence); + } + + inline int raw_read(int fd, void * buffer, std::size_t count) + { + return ::_read(fd, buffer, count); + } + + inline int raw_write(int fd, const void * buffer, std::size_t count) + { + return ::_write(fd, buffer, count); + } + + inline int raw_ftruncate(int fd, long length) + { + return ::_chsize(fd, length); + } + + inline int raw_fallocate(int fd, long length) + { + // Doesn't actually allocate, but best we can do? + return raw_ftruncate(fd, length); + } +#else + inline int raw_open(const char * pathname, int flags, int mode) + { + return ::open(pathname, flags, mode); + } + + inline int raw_open(const char * pathname, int flags) + { + return ::open(pathname, flags); + } + + inline int raw_close(int fd) + { + return ::close(fd); + } + + inline off_t raw_lseek(int fd, off_t offset, int whence) + { + return ::lseek(fd, offset, whence); + } + + inline ssize_t raw_read(int fd, void * buffer, std::size_t count) + { + return ::read(fd, buffer, count); + } + + inline ssize_t raw_write(int fd, const void * buffer, std::size_t count) + { + return ::write(fd, buffer, count); + } + + inline int raw_ftruncate(int fd, off_t length) + { + return ::ftruncate(fd, length); + } + +# ifdef __APPLE__ + inline int raw_fallocate(int fd, off_t length) + { + // OS X doesn't have posix_fallocate, but it has a fcntl that does the job. + // It may make the file too big though, so we truncate before returning. + + // Try to allocate contiguous space first. + ::fstore_t store = {F_ALLOCATEALL | F_ALLOCATECONTIG, F_PEOFPOSMODE, 0, length, 0}; + if (::fcntl(fd, F_PREALLOCATE, &store) != -1) + return raw_ftruncate(fd, length); + + // Try fragmented if it failed. + store.fst_flags = F_ALLOCATEALL; + if (::fcntl(fd, F_PREALLOCATE, &store) != -1) + return raw_ftruncate(fd, length); + + // Fragmented also failed. + return -1; + } + +# else // __APPLE__ + inline int raw_fallocate(int fd, off_t length) + { +# ifdef ANDROID + // Android's libc doesn't have posix_fallocate. + if (::fallocate(fd, 0, 0, length) == 0) + return 0; +# else + // Conforming POSIX systems have posix_fallocate. + if (::posix_fallocate(fd, 0, length) == 0) + return 0; +# endif + + // EINVAL should indicate an unsupported filesystem. + // All other errors are passed up. + if (errno != EINVAL) + return -1; + + // Try to deal with unsupported filesystems by simply seeking + writing. + // This may not really allocate space, but the file size will be set. + // Writes to the mmapped file may still trigger SIGBUS errors later. + + // Remember the old position and seek to the desired length. + off_t old_pos = raw_lseek(fd, 0, SEEK_CUR); + if (old_pos == -1) + return -1; + if (raw_lseek(fd, length - 1, SEEK_SET) == -1) + return -1; + + // Write a single byte to resize the file. + char buffer = 0; + ssize_t written = raw_write(fd, &buffer, 1); + + // Seek back to the old position. + if (raw_lseek(fd, old_pos, SEEK_SET) == -1) + return -1; + + // Fail if we didn't write exactly one byte, + if (written != 1) + return -1; + + return 0; + } +# endif // __APPLE +#endif // _WIN32 + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/lzf.h b/deps_install/include/pcl-1.10/pcl/io/lzf.h new file mode 100755 index 0000000..ad98999 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/lzf.h @@ -0,0 +1,88 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2000-2008 Marc Alexander Lehmann + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Compress in_len bytes stored at the memory block starting at + * \a in_data and write the result to \a out_data, up to a maximum length + * of \a out_len bytes using Marc Lehmann's LZF algorithm. + * + * If the output buffer is not large enough or any error occurs return 0, + * otherwise return the number of bytes used, which might be considerably + * more than in_len (but less than 104% of the original size), so it + * makes sense to always use out_len == in_len - 1), to ensure _some_ + * compression, and store the data uncompressed otherwise (with a flag, of + * course. + * + * \note The buffers must not be overlapping. + * + * \param[in] in_data the input uncompressed buffer + * \param[in] in_len the length of the input buffer + * \param[out] out_data the output buffer where the compressed result will be stored + * \param[out] out_len the length of the output buffer + * + */ + PCL_EXPORTS unsigned int + lzfCompress (const void *const in_data, unsigned int in_len, + void *out_data, unsigned int out_len); + + /** \brief Decompress data compressed with the \a lzfCompress function and + * stored at location \a in_data and length \a in_len. The result will be + * stored at \a out_data up to a maximum of \a out_len characters. + * + * If the output buffer is not large enough to hold the decompressed + * data, a 0 is returned and errno is set to E2BIG. Otherwise the number + * of decompressed bytes (i.e. the original length of the data) is + * returned. + * + * If an error in the compressed data is detected, a zero is returned and + * errno is set to EINVAL. + * + * This function is very fast, about as fast as a copying loop. + * \param[in] in_data the input compressed buffer + * \param[in] in_len the length of the input buffer + * \param[out] out_data the output buffer (must be resized to \a out_len) + * \param[out] out_len the length of the output buffer + */ + PCL_EXPORTS unsigned int + lzfDecompress (const void *const in_data, unsigned int in_len, + void *out_data, unsigned int out_len); +} diff --git a/deps_install/include/pcl-1.10/pcl/io/lzf_image_io.h b/deps_install/include/pcl-1.10/pcl/io/lzf_image_io.h new file mode 100755 index 0000000..f5926c2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/lzf_image_io.h @@ -0,0 +1,631 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace io + { + /** \brief Basic camera parameters placeholder. */ + struct CameraParameters + { + /** fx */ + double focal_length_x; + /** fy */ + double focal_length_y; + /** cx */ + double principal_point_x; + /** cy */ + double principal_point_y; + }; + + /** \brief PCL-LZF image format reader. + * The PCL-LZF image format is nothing else but a LZF-modified compression over + * an existing file type (e.g., PNG). However, in certain situations, like RGB data for + * example, an [RGBRGB...RGB] array will be first reordered into [RR...RGG...GBB...B] + * in order to ensure better compression. + * + * The current list of compressors/decompressors include: + * * LZF compressed 24-bit [RR...RGG...GBB...B] data + * * LZF compressed 8-bit Bayer data + * * LZF compressed 16-bit YUV422 data + * * LZF compressed 16-bit depth data + * + * Please note that files found using the above mentioned extensions will be treated + * as such. Inherit from this class and overwrite the I/O methods if you plan to change + * this behavior. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFImageReader + { + public: + /** Empty constructor */ + LZFImageReader (); + /** Empty destructor */ + virtual ~LZFImageReader () {} + + /** \brief Read camera parameters from a given file and store them internally. + * \return true if operation successful, false otherwise + */ + bool + readParameters (const std::string &filename); + + /** \brief Read the parameters from a struct instead + * \param[in] parameters Camera parameters to use */ + inline void + setParameters (const CameraParameters ¶meters) + { + parameters_ = parameters; + } + + /** \brief Get the camera parameters currently being used + * returns a CameraParameters struct */ + inline CameraParameters + getParameters () const + { + return parameters_; + } + + /** \brief Get the image width as read from disk. */ + inline std::uint32_t + getWidth () const + { + return (width_); + } + + /** \brief Get the image height as read from disk. */ + inline std::uint32_t + getHeight () const + { + return (height_); + } + + /** \brief Get the type of the image read from disk. */ + inline std::string + getImageType () const + { + return (image_type_identifier_); + } + + protected: + /** \brief Read camera parameters from a given stream and store them internally. + * \return true if operation successful, false otherwise + */ + virtual bool + readParameters (std::istream&) { return (false); } + + /** \brief Load a compressed image array from disk + * \param[in] filename the file name to load the data from + * \param[out] data the size of the data + * \param uncompressed_size + * \return an array filled with the data loaded from disk, NULL if error + */ + bool + loadImageBlob (const std::string &filename, + std::vector &data, + std::uint32_t &uncompressed_size); + + /** \brief Realtime LZF decompression. + * \param[in] input the array to decompress + * \param[out] output the decompressed array + * \return true if operation successful, false otherwise + */ + bool + decompress (const std::vector &input, + std::vector &output); + + /** \brief The image width, as read from the file. */ + std::uint32_t width_; + + /** \brief The image height, as read from the file. */ + std::uint32_t height_; + + /** \brief The image type string, as read from the file. */ + std::string image_type_identifier_; + + /** \brief Internal set of camera parameters. */ + CameraParameters parameters_; + }; + + /** \brief PCL-LZF 16-bit depth image format reader. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFDepth16ImageReader : public LZFImageReader + { + public: + using LZFImageReader::readParameters; + + /** Empty constructor */ + LZFDepth16ImageReader () + : z_multiplication_factor_ (0.001) // Set default multiplication factor + {} + + /** Empty destructor */ + ~LZFDepth16ImageReader () {} + + /** \brief Read the data stored in a PCLZF depth file and convert it to a pcl::PointCloud type. + * \param[in] filename the file name to read the data from + * \param[out] cloud the resultant output point cloud + */ + template bool + read (const std::string &filename, pcl::PointCloud &cloud); + + /** \brief Read the data stored in a PCLZF depth file and convert it to a pcl::PointCloud type. + * \param[in] filename the file name to read the data from + * \param[in] num_threads The number of threads to use. 0 indicates OpenMP is free to choose. + * \param[out] cloud the resultant output point cloud + */ + template bool + readOMP (const std::string &filename, pcl::PointCloud &cloud, + unsigned int num_threads=0); + + /** \brief Read camera parameters from a given stream and store them internally. + * The parameters will be read from the \ ... \ tag. + * \return true if operation successful, false otherwise + */ + bool + readParameters (std::istream& is) override; + + protected: + /** \brief Z-value depth multiplication factor + * (i.e., if raw data is in [mm] and we want [m], we need to multiply with 0.001) + */ + double z_multiplication_factor_; + }; + + /** \brief PCL-LZF 24-bit RGB image format reader. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFRGB24ImageReader : public LZFImageReader + { + public: + using LZFImageReader::readParameters; + + /** Empty constructor */ + LZFRGB24ImageReader () {} + /** Empty destructor */ + ~LZFRGB24ImageReader () {} + + /** \brief Read the data stored in a PCLZF RGB file and convert it to a pcl::PointCloud type. + * \param[in] filename the file name to read the data from + * \param[out] cloud the resultant output point cloud + */ + template bool + read (const std::string &filename, pcl::PointCloud &cloud); + + /** \brief Read the data stored in a PCLZF RGB file and convert it to a pcl::PointCloud type. + * Note that, unless massively multithreaded, this will likely not result in a significant speedup and may even slow performance. + * \param[in] filename the file name to read the data from + * \param[in] num_threads The number of threads to use + * \param[out] cloud the resultant output point cloud + */ + template bool + readOMP (const std::string &filename, pcl::PointCloud &cloud, + unsigned int num_threads=0); + + /** \brief Read camera parameters from a given stream and store them internally. + * The parameters will be read from the \ ... \ tag. + * \return true if operation successful, false otherwise + */ + bool + readParameters (std::istream& is) override; + + protected: + }; + + /** \brief PCL-LZF 8-bit Bayer image format reader. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFYUV422ImageReader : public LZFRGB24ImageReader + { + public: + using LZFRGB24ImageReader::readParameters; + + /** Empty constructor */ + LZFYUV422ImageReader () {} + /** Empty destructor */ + ~LZFYUV422ImageReader () {} + + /** \brief Read the data stored in a PCLZF YUV422 16bit file and convert it to a pcl::PointCloud type. + * \param[in] filename the file name to read the data from + * \param[out] cloud the resultant output point cloud + */ + template bool + read (const std::string &filename, pcl::PointCloud &cloud); + + /** \brief Read the data stored in a PCLZF YUV422 file and convert it to a pcl::PointCloud type. + * Note that, unless massively multithreaded, this will likely not result in a significant speedup + * \param[in] filename the file name to read the data from + * \param[in] num_threads The number of threads to use + * \param[out] cloud the resultant output point cloud + */ + template bool + readOMP (const std::string &filename, pcl::PointCloud &cloud, + unsigned int num_threads=0); + }; + + /** \brief PCL-LZF 8-bit Bayer image format reader. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFBayer8ImageReader : public LZFRGB24ImageReader + { + public: + using LZFRGB24ImageReader::readParameters; + + /** Empty constructor */ + LZFBayer8ImageReader () {} + /** Empty destructor */ + ~LZFBayer8ImageReader () {} + + /** \brief Read the data stored in a PCLZF Bayer 8bit file and convert it to a pcl::PointCloud type. + * \param[in] filename the file name to read the data from + * \param[out] cloud the resultant output point cloud + */ + template bool + read (const std::string &filename, pcl::PointCloud &cloud); + + /** \brief Read the data stored in a PCLZF Bayer 8bit file and convert it to a pcl::PointCloud type. + * Note that, unless massively multithreaded, this will likely not result in a significant speedup and may even slow performance. + * \param[in] filename the file name to read the data from + * \param[in] num_threads The number of threads to use + * \param[out] cloud the resultant output point cloud + */ + template bool + readOMP (const std::string &filename, pcl::PointCloud &cloud, + unsigned int num_threads=0); + }; + + /** \brief PCL-LZF image format writer. + * The PCL-LZF image format is nothing else but a LZF-modified compression over + * an existing file type (e.g., PNG). However, in certain situations, like RGB data for + * example, an [RGBRGB...RGB] array will be first reordered into [RR...RGG...GBB...B] + * in order to ensure better compression. + * + * The current list of compressors/decompressors include: + * * LZF compressed 24-bit [RR...RGG...GBB...B] data + * * LZF compressed 8-bit Bayer data + * * LZF compressed 16-bit YUV422 data + * * LZF compressed 16-bit depth data + * + * Please note that files found using the above mentioned extensions will be treated + * as such. Inherit from this class and overwrite the I/O methods if you plan to change + * this behavior. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFImageWriter + { + public: + /** Empty constructor */ + LZFImageWriter () {} + /** Empty destructor */ + virtual ~LZFImageWriter () {} + + /** \brief Save an image into PCL-LZF format. Virtual. + * \param[in] data the array holding the image + * \param[in] width the with of the data array + * \param[in] height the height of the data array + * \param[in] filename the file name to write + * \return true if operation successful, false otherwise + */ + virtual bool + write (const char* data, + std::uint32_t width, std::uint32_t height, + const std::string &filename) = 0; + + /** \brief Write camera parameters to disk. Virtual. + * \param[in] parameters the camera parameters + * \param[in] filename the file name to write + * \return true if operation successful, false otherwise + */ + virtual bool + writeParameters (const CameraParameters ¶meters, + const std::string &filename) = 0; + + /** \brief Save an image and its camera parameters into PCL-LZF format. + * \param[in] data the array holding the image + * \param[in] width the with of the data array + * \param[in] height the height of the data array + * \param[in] parameters the camera parameters + * \param[in] filename_data the file name to write the data to + * \param[in] filename_xml the file name to write the parameters to + * \return true if operation successful, false otherwise + */ + virtual bool + write (const char* data, + std::uint32_t width, std::uint32_t height, + const CameraParameters ¶meters, + const std::string &filename_data, + const std::string &filename_xml) + { + bool res1 = write (data, width, height, filename_data); + bool res2 = writeParameters (parameters, filename_xml); + return (res1 && res2); + } + + /** \brief Write a single image/camera parameter to file, given an XML tag + * \param[in] parameter the value of the parameter to write + * \param[in] tag the value of the XML tag + * \param[in] filename the file name to write + * \return true if operation successful, false otherwise + * Example: + * \code + * pcl::io::LZFDepthImageWriter w; + * w.writeParameter (0.001, "depth.multiplication_factor", "parameters.xml"); + * \endcode + */ + bool + writeParameter (const double ¶meter, const std::string &tag, + const std::string &filename); + protected: + /** \brief Save a compressed image array to disk + * \param[in] data the data to save + * \param[in] data_size the size of the data + * \param[in] filename the file name to write the data to + * \return true if operation successful, false otherwise + */ + bool + saveImageBlob (const char* data, std::size_t data_size, + const std::string &filename); + + /** \brief Realtime LZF compression. + * \param[in] input the array to compress + * \param[in] input_size the size of the array to compress + * \param[in] width the with of the data array + * \param[in] height the height of the data array + * \param[in] image_type the type of the image to save. This should be an up to + * 16 characters string describing the data type. Examples are: "bayer8", "rgb24", + * "yuv422", "depth16". + * \param[out] output the compressed output array (must be pre-allocated!) + * \return the number of bytes in the output array + */ + std::uint32_t + compress (const char* input, std::uint32_t input_size, + std::uint32_t width, std::uint32_t height, + const std::string &image_type, + char *output); + }; + + /** \brief PCL-LZF 16-bit depth image format writer. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFDepth16ImageWriter : public LZFImageWriter + { + public: + /** Empty constructor */ + LZFDepth16ImageWriter () + : z_multiplication_factor_ (0.001) // Set default multiplication factor + {} + + /** Empty destructor */ + ~LZFDepth16ImageWriter () {} + + /** \brief Save a 16-bit depth image into PCL-LZF format. + * \param[in] data the array holding the depth image + * \param[in] width the with of the data array + * \param[in] height the height of the data array + * \param[in] filename the file name to write (preferred extension: .pclzf) + * \return true if operation successful, false otherwise + */ + bool + write (const char* data, + std::uint32_t width, std::uint32_t height, + const std::string &filename) override; + + /** \brief Write camera parameters to disk. + * \param[in] parameters the camera parameters + * \param[in] filename the file name to write + * \return true if operation successful, false otherwise + * This overwrites the following parameters in the xml file, under the + * \ tag: + * \...\ + * \...\ + * \...\ + * \...\ + * \...\ + */ + bool + writeParameters (const CameraParameters ¶meters, + const std::string &filename) override; + + protected: + /** \brief Z-value depth multiplication factor + * (i.e., if raw data is in [mm] and we want [m], we need to multiply with 0.001) + */ + double z_multiplication_factor_; + }; + + /** \brief PCL-LZF 24-bit RGB image format writer. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFRGB24ImageWriter : public LZFImageWriter + { + public: + /** Empty constructor */ + LZFRGB24ImageWriter () {} + /** Empty destructor */ + ~LZFRGB24ImageWriter () {} + + /** \brief Save a 24-bit RGB image into PCL-LZF format. + * \param[in] data the array holding the RGB image (as [RGB..RGB] or [BGR..BGR]) + * \param[in] width the with of the data array + * \param[in] height the height of the data array + * \param[in] filename the file name to write (preferred extension: .pclzf) + * \return true if operation successful, false otherwise + */ + bool + write (const char *data, + std::uint32_t width, std::uint32_t height, + const std::string &filename) override; + + /** \brief Write camera parameters to disk. + * \param[in] parameters the camera parameters + * \param[in] filename the file name to write + * \return true if operation successful, false otherwise + */ + bool + writeParameters (const CameraParameters ¶meters, + const std::string &filename) override; + + protected: + }; + + /** \brief PCL-LZF 16-bit YUV422 image format writer. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFYUV422ImageWriter : public LZFRGB24ImageWriter + { + public: + /** Empty constructor */ + LZFYUV422ImageWriter () {} + /** Empty destructor */ + ~LZFYUV422ImageWriter () {} + + /** \brief Save a 16-bit YUV422 image into PCL-LZF format. + * \param[in] data the array holding the YUV422 image (as [YUYV...YUYV]) + * \param[in] width the with of the data array + * \param[in] height the height of the data array + * \param[in] filename the file name to write (preferred extension: .pclzf) + * \return true if operation successful, false otherwise + */ + bool + write (const char *data, + std::uint32_t width, std::uint32_t height, + const std::string &filename) override; + }; + + /** \brief PCL-LZF 8-bit Bayer image format writer. + * + * The main advantage of using the PCL-LZF image I/O routines is a very good file size + * versus I/O speed ratio. Tests performed using LZF, Snappy, ZIP, GZ2, BZIP2, as well + * as PNG, JPEG, and TIFF compression have shown that the internal PCL LZF methods + * provide the best score for the types of applications PCL is suited for. + * + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS LZFBayer8ImageWriter : public LZFRGB24ImageWriter + { + public: + /** Empty constructor */ + LZFBayer8ImageWriter () {} + /** Empty destructor */ + ~LZFBayer8ImageWriter () {} + + /** \brief Save a 8-bit Bayer image into PCL-LZF format. + * \param[in] data the array holding the 8-bit Bayer array + * \param[in] width the with of the data array + * \param[in] height the height of the data array + * \param[in] filename the file name to write (preferred extension: .pclzf) + * \return true if operation successful, false otherwise + */ + bool + write (const char *data, + std::uint32_t width, std::uint32_t height, + const std::string &filename) override; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/io/obj_io.h b/deps_install/include/pcl-1.10/pcl/io/obj_io.h new file mode 100755 index 0000000..a9ca88c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/obj_io.h @@ -0,0 +1,345 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2013, Open Perception, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + class PCL_EXPORTS MTLReader + { + public: + /** \brief empty constructor */ + MTLReader (); + + /** \brief empty destructor */ + virtual ~MTLReader() {} + + /** \brief Read a MTL file given its full path. + * \param[in] filename full path to MTL file + * \return 0 on success < 0 else. + */ + int + read (const std::string& filename); + + /** \brief Read a MTL file given an OBJ file full path and the MTL file name. + * \param[in] obj_file_name full path to OBJ file + * \param[in] mtl_file_name MTL file name + * \return 0 on success < 0 else. + */ + int + read (const std::string& obj_file_name, const std::string& mtl_file_name); + + std::vector::const_iterator + getMaterial (const std::string& material_name) const; + + /// materials array + std::vector materials_; + + private: + /// converts CIE XYZ to RGB + inline void + cie2rgb (const Eigen::Vector3f& xyz, pcl::TexMaterial::RGB& rgb) const; + /// fill a pcl::TexMaterial::RGB from a split line containing CIE x y z values + int + fillRGBfromXYZ (const std::vector& split_line, pcl::TexMaterial::RGB& rgb); + /// fill a pcl::TexMaterial::RGB from a split line containing r g b values + int + fillRGBfromRGB (const std::vector& split_line, pcl::TexMaterial::RGB& rgb); + /// matrix to convert CIE to RGB + Eigen::Matrix3f xyz_to_rgb_matrix_; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + class PCL_EXPORTS OBJReader : public FileReader + { + public: + /** \brief empty constructor */ + OBJReader() {} + /** \brief empty destructor */ + ~OBJReader() {} + /** \brief Read a point cloud data header from a FILE file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given FILE file. Useful for fast + * evaluation of the underlying data structure. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] origin the sensor acquisition origin always null + * \param[out] orientation the sensor acquisition orientation always identity + * \param[out] file_version always 0 + * \param data_type + * \param data_idx + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + * + * \return 0 on success. + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, int &data_type, unsigned int &data_idx, + const int offset) override; + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] origin the sensor acquisition origin always null + * \param[out] orientation the sensor acquisition orientation always identity + * \param[out] file_version always 0 + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset = 0) override; + + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/TextureMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant TextureMesh read from disk + * \param[out] origin the sensor origin always null + * \param[out] orientation the sensor orientation always identity + * \param[out] file_version always 0 + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::TextureMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/TextureMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant TextureMesh read from disk + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::TextureMesh &mesh, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PolygonMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant PolygonMesh read from disk + * \param[out] origin the sensor origin always null + * \param[out] orientation the sensor orientation always identity + * \param[out] file_version always 0 + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &file_version, const int offset = 0); + + /** \brief Read a point cloud data from a FILE file and store it into a + * pcl/PolygonMesh. + * \param[in] file_name the name of the file containing data + * \param[out] mesh the resultant PolygonMesh read from disk + * \param[in] offset the offset in the file where to expect the true + * header to begin. + * + * \return 0 on success. + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset = 0); + + /** \brief Read a point cloud data from any FILE file, and convert it to the given + * template format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + template inline int + read (const std::string &file_name, pcl::PointCloud &cloud, + const int offset =0) + { + pcl::PCLPointCloud2 blob; + int file_version; + int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, + file_version, offset); + if (res < 0) + return (res); + + pcl::fromPCLPointCloud2 (blob, cloud); + return (0); + } + + private: + /// Usually OBJ files come MTL files where texture materials are stored + std::vector companions_; + }; + + namespace io + { + /** \brief Load any OBJ file into a templated PointCloud type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \param[out] origin the sensor acquisition origin, null + * \param[out] orientation the sensor acquisition orientation, identity + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) + { + pcl::OBJReader p; + int obj_version; + return (p.read (file_name, cloud, origin, orientation, obj_version)); + } + + /** \brief Load an OBJ file into a PCLPointCloud2 blob type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) + { + pcl::OBJReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any OBJ file into a templated PointCloud type + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + template inline int + loadOBJFile (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::OBJReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any OBJ file into a PolygonMesh type. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant mesh + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::PolygonMesh &mesh) + { + pcl::OBJReader p; + return (p.read (file_name, mesh)); + } + + /** \brief Load any OBJ file into a TextureMesh type. + * \param[in] file_name the name of the file to load + * \param[out] mesh the resultant mesh + * \return 0 on success < 0 on error + * + * \ingroup io + */ + inline int + loadOBJFile (const std::string &file_name, pcl::TextureMesh &mesh) + { + pcl::OBJReader p; + return (p.read (file_name, mesh)); + } + + /** \brief Saves a TextureMesh in ascii OBJ format. + * \param[in] file_name the name of the file to write to disk + * \param[in] tex_mesh the texture mesh to save + * \param[in] precision the output ASCII precision + * \ingroup io + */ + PCL_EXPORTS int + saveOBJFile (const std::string &file_name, + const pcl::TextureMesh &tex_mesh, + unsigned precision = 5); + + /** \brief Saves a PolygonMesh in ascii PLY format. + * \param[in] file_name the name of the file to write to disk + * \param[in] mesh the polygonal mesh to save + * \param[in] precision the output ASCII precision default 5 + * \ingroup io + */ + PCL_EXPORTS int + saveOBJFile (const std::string &file_name, + const pcl::PolygonMesh &mesh, + unsigned precision = 5); + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/oni_grabber.h b/deps_install/include/pcl-1.10/pcl/io/oni_grabber.h new file mode 100755 index 0000000..877ff80 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/oni_grabber.h @@ -0,0 +1,203 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +#ifdef HAVE_OPENNI + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + struct PointXYZ; + struct PointXYZRGB; + struct PointXYZRGBA; + struct PointXYZI; + + /** \brief A simple ONI grabber. + * \author Suat Gedikli + * \ingroup io + */ + class PCL_EXPORTS ONIGrabber : public Grabber + { + public: + //define callback signature typedefs + using sig_cb_openni_image = void (const openni_wrapper::Image::Ptr &); + using sig_cb_openni_depth_image = void (const openni_wrapper::DepthImage::Ptr &); + using sig_cb_openni_ir_image = void (const openni_wrapper::IRImage::Ptr &); + using sig_cb_openni_image_depth_image = void (const openni_wrapper::Image::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) ; + using sig_cb_openni_ir_depth_image = void (const openni_wrapper::IRImage::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) ; + using sig_cb_openni_point_cloud = void (const pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_rgb = void (const pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_rgba = void (const pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_i = void (const pcl::PointCloud::ConstPtr &); + + /** \brief constructor + * \param[in] file_name the path to the ONI file + * \param[in] repeat whether the play back should be in an infinite loop or not + * \param[in] stream whether the playback should be in streaming mode or in triggered mode. + */ + ONIGrabber (const std::string& file_name, bool repeat, bool stream); + + /** \brief destructor never throws an exception */ + ~ONIGrabber () noexcept; + + /** \brief For devices that are streaming, the streams are started by calling this method. + * Trigger-based devices, just trigger the device once for each call of start. + */ + void + start () override; + + /** \brief For devices that are streaming, the streams are stopped. + * This method has no effect for triggered devices. + */ + void + stop () override; + + /** \brief returns the name of the concrete subclass. + * \return the name of the concrete driver. + */ + std::string + getName () const override; + + /** \brief Indicates whether the grabber is streaming or not. This value is not defined for triggered devices. + * \return true if grabber is running / streaming. False otherwise. + */ + bool + isRunning () const override; + + /** \brief returns the frames pre second. 0 if it is trigger based. */ + float + getFramesPerSecond () const override; + + /** \brief Check if there is any data left in the ONI file to process. */ + inline bool + hasDataLeft () + { + return (device_->hasDataLeft ()); + } + + protected: + /** \brief internal OpenNI (openni_wrapper) callback that handles image streams */ + void + imageCallback (openni_wrapper::Image::Ptr image, void* cookie); + + /** \brief internal OpenNI (openni_wrapper) callback that handles depth streams */ + void + depthCallback (openni_wrapper::DepthImage::Ptr depth_image, void* cookie); + + /** \brief internal OpenNI (openni_wrapper) callback that handles IR streams */ + void + irCallback (openni_wrapper::IRImage::Ptr ir_image, void* cookie); + + /** \brief internal callback that handles synchronized image + depth streams */ + void + imageDepthImageCallback (const openni_wrapper::Image::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image); + + /** \brief internal callback that handles synchronized IR + depth streams */ + void + irDepthImageCallback (const openni_wrapper::IRImage::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image); + + /** \brief internal method to assemble a point cloud object */ + pcl::PointCloud::Ptr + convertToXYZPointCloud (const openni_wrapper::DepthImage::Ptr &depth) const; + + /** \brief internal method to assemble a point cloud object */ + pcl::PointCloud::Ptr + convertToXYZRGBPointCloud (const openni_wrapper::Image::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image) const; + + /** \brief internal method to assemble a point cloud object */ + pcl::PointCloud::Ptr + convertToXYZRGBAPointCloud (const openni_wrapper::Image::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image) const; + + /** \brief internal method to assemble a point cloud object */ + pcl::PointCloud::Ptr + convertToXYZIPointCloud (const openni_wrapper::IRImage::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image) const; + + /** \brief synchronizer object to synchronize image and depth streams*/ + Synchronizer rgb_sync_; + + /** \brief synchronizer object to synchronize IR and depth streams*/ + Synchronizer ir_sync_; + + /** \brief the actual openni device*/ + openni_wrapper::DeviceONI::Ptr device_; + std::string rgb_frame_id_; + std::string depth_frame_id_; + bool running_; + unsigned image_width_; + unsigned image_height_; + unsigned depth_width_; + unsigned depth_height_; + openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle; + openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle; + openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle; + boost::signals2::signal* image_signal_; + boost::signals2::signal* depth_image_signal_; + boost::signals2::signal* ir_image_signal_; + boost::signals2::signal* image_depth_image_signal_; + boost::signals2::signal* ir_depth_image_signal_; + boost::signals2::signal* point_cloud_signal_; + boost::signals2::signal* point_cloud_i_signal_; + boost::signals2::signal* point_cloud_rgb_signal_; + boost::signals2::signal* point_cloud_rgba_signal_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // namespace +#endif // HAVE_OPENNI diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni.h new file mode 100755 index 0000000..bb31ef3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef HAVE_OPENNI2 + +#include + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +// Standard resolutions, ported from OpenNI 1.x. To be removed later. +#define XN_QQVGA_X_RES 160 +#define XN_QQVGA_Y_RES 120 +#define XN_CGA_X_RES 320 +#define XN_CGA_Y_RES 200 +#define XN_QVGA_X_RES 320 +#define XN_QVGA_Y_RES 240 +#define XN_VGA_X_RES 640 +#define XN_VGA_Y_RES 480 +#define XN_SVGA_X_RES 800 +#define XN_SVGA_Y_RES 600 +#define XN_XGA_X_RES 1024 +#define XN_XGA_Y_RES 768 +#define XN_720P_X_RES 1280 +#define XN_720P_Y_RES 720 +#define XN_SXGA_X_RES 1280 +#define XN_SXGA_Y_RES 1024 +#define XN_UXGA_X_RES 1600 +#define XN_UXGA_Y_RES 1200 +#define XN_1080P_X_RES 1920 +#define XN_1080P_Y_RES 1080 +#define XN_QCIF_X_RES 176 +#define XN_QCIF_Y_RES 144 +#define XN_240P_X_RES 423 +#define XN_240P_Y_RES 240 +#define XN_CIF_X_RES 352 +#define XN_CIF_Y_RES 288 +#define XN_WVGA_X_RES 640 +#define XN_WVGA_Y_RES 360 +#define XN_480P_X_RES 864 +#define XN_480P_Y_RES 480 +#define XN_576P_X_RES 1024 +#define XN_576P_Y_RES 576 +#define XN_DV_X_RES 960 +#define XN_DV_Y_RES 720 + +#endif // HAVE_OPENNI2 diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_convert.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_convert.h new file mode 100755 index 0000000..fe1fe67 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_convert.h @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + + +#pragma once + +#include "pcl/io/openni2/openni2_device_info.h" +#include "pcl/io/openni2/openni2_video_mode.h" + +#include "OpenNI.h" + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + const OpenNI2DeviceInfo + openni2_convert (const openni::DeviceInfo* pInfo); + + const openni::VideoMode + grabberModeToOpenniMode (const OpenNI2VideoMode& input); + + const OpenNI2VideoMode + openniModeToGrabberMode (const openni::VideoMode& input); + + const std::vector + openniModeToGrabberMode (const openni::Array& input); + + } // namespace + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device.h new file mode 100755 index 0000000..0336500 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device.h @@ -0,0 +1,334 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include "openni.h" +#include "pcl/io/openni2/openni2_video_mode.h" +#include "pcl/io/io_exception.h" + +#include + +// Template frame wrappers +#include +#include +#include + +#include +#include +#include +#include +#include + + + +namespace openni +{ + class Device; + class DeviceInfo; + class VideoStream; + class SensorInfo; +} + +namespace pcl +{ + namespace io + { + namespace openni2 + { + using DepthImage = pcl::io::DepthImage; + using IRImage = pcl::io::IRImage; + using Image = pcl::io::Image; + + class OpenNI2FrameListener; + + class PCL_EXPORTS OpenNI2Device + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using ImageCallbackFunction = std::function; + using DepthImageCallbackFunction = std::function; + using IRImageCallbackFunction = std::function; + using CallbackHandle = unsigned; + + using StreamCallbackFunction = std::function; + + OpenNI2Device (const std::string& device_URI); + virtual ~OpenNI2Device (); + + const std::string + getUri () const; + const std::string + getVendor () const; + const std::string + getName () const; + std::uint16_t + getUsbVendorId () const; + std::uint16_t + getUsbProductId () const; + + const std::string + getStringID () const; + + bool + isValid () const; + + bool + hasIRSensor () const; + bool + hasColorSensor () const; + bool + hasDepthSensor () const; + + void + startIRStream (); + void + startColorStream (); + void + startDepthStream (); + + void + stopAllStreams (); + + void + stopIRStream (); + void + stopColorStream (); + void + stopDepthStream (); + + bool + isIRStreamStarted (); + bool + isColorStreamStarted (); + bool + isDepthStreamStarted (); + + bool + isImageRegistrationModeSupported () const; + void + setImageRegistrationMode (bool enabled); + bool + isDepthRegistered () const; + + const OpenNI2VideoMode + getIRVideoMode (); + const OpenNI2VideoMode + getColorVideoMode (); + const OpenNI2VideoMode + getDepthVideoMode (); + + const std::vector& + getSupportedIRVideoModes () const; + const std::vector& + getSupportedColorVideoModes () const; + const std::vector& + getSupportedDepthVideoModes () const; + + bool + isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + bool + isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + bool + isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + + bool + findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + bool + findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + bool + findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + + void + setIRVideoMode (const OpenNI2VideoMode& video_mode); + void + setColorVideoMode (const OpenNI2VideoMode& video_mode); + void + setDepthVideoMode (const OpenNI2VideoMode& video_mode); + + OpenNI2VideoMode + getDefaultIRMode () const; + OpenNI2VideoMode + getDefaultColorMode () const; + OpenNI2VideoMode + getDefaultDepthMode () const; + + float + getIRFocalLength () const; + float + getColorFocalLength () const; + float + getDepthFocalLength () const; + + // Baseline between sensors. Returns 0 if this value does not exist. + float + getBaseline(); + + // Value of pixels in shadow or that have no valid measurement + std::uint64_t + getShadowValue(); + + void + setAutoExposure (bool enable); + void + setAutoWhiteBalance (bool enable); + + inline bool + isSynchronized () + { + return (openni_device_->getDepthColorSyncEnabled ()); + } + + inline bool + isSynchronizationSupported () + { + return (true); // Not sure how to query this from the hardware + } + + inline bool + isFile() + { + return (openni_device_->isFile()); + } + + void + setSynchronization (bool enableSync); + + bool + getAutoExposure () const; + bool + getAutoWhiteBalance () const; + + void + setUseDeviceTimer (bool enable); + + /** \brief Get absolute number of depth frames in the current stream. + * This function returns 0 if the current device is not a file stream or + * if the current mode has no depth stream. + */ + int + getDepthFrameCount (); + + /** \brief Get absolute number of color frames in the current stream. + * This function returns 0 if the current device is not a file stream or + * if the current mode has no color stream. + */ + int + getColorFrameCount (); + + /** \brief Get absolute number of ir frames in the current stream. + * This function returns 0 if the current device is not a file stream or + * if the current mode has no ir stream. + */ + int + getIRFrameCount (); + + /** \brief Set the playback speed if the device is an recorded stream. + * If setting the device playback speed fails, because the device is no recorded stream or + * any other reason this function returns false. Otherwise true is returned. + * \param[in] speed The playback speed factor 1.0 means the same speed as recorded, + * 0.5 half the speed, 2.0 double speed and so on. + * \return True on success, false otherwise. + */ + bool + setPlaybackSpeed (double speed); + + /************************************************************************************/ + // Callbacks from openni::VideoStream to grabber. Internal interface + void + setColorCallback (StreamCallbackFunction color_callback); + void + setDepthCallback (StreamCallbackFunction depth_callback); + void + setIRCallback (StreamCallbackFunction ir_callback); + + protected: + void shutdown (); + + std::shared_ptr + getIRVideoStream () const; + std::shared_ptr + getColorVideoStream () const; + std::shared_ptr + getDepthVideoStream () const; + + + void + processColorFrame (openni::VideoStream& stream); + void + processDepthFrame (openni::VideoStream& stream); + void + processIRFrame (openni::VideoStream& stream); + + + bool + findCompatibleVideoMode (const std::vector& supportedModes, + const OpenNI2VideoMode& output_mode, OpenNI2VideoMode& mode) const; + + bool + resizingSupported (std::size_t input_width, std::size_t input_height, std::size_t output_width, std::size_t output_height) const; + + // Members + + std::shared_ptr openni_device_; + std::shared_ptr device_info_; + + std::shared_ptr ir_frame_listener; + std::shared_ptr color_frame_listener; + std::shared_ptr depth_frame_listener; + + mutable std::shared_ptr ir_video_stream_; + mutable std::shared_ptr color_video_stream_; + mutable std::shared_ptr depth_video_stream_; + + mutable std::vector ir_video_modes_; + mutable std::vector color_video_modes_; + mutable std::vector depth_video_modes_; + + bool ir_video_started_; + bool color_video_started_; + bool depth_video_started_; + + /** \brief distance between the projector and the IR camera in meters*/ + float baseline_; + /** the value for shadow (occluded pixels) */ + std::uint64_t shadow_value_; + /** the value for pixels without a valid disparity measurement */ + std::uint64_t no_sample_value_; + }; + + PCL_EXPORTS std::ostream& operator<< (std::ostream& stream, const OpenNI2Device& device); + + } // namespace + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device_info.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device_info.h new file mode 100755 index 0000000..2dd5c2f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device_info.h @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + struct OpenNI2DeviceInfo + { + std::string uri_; + std::string vendor_; + std::string name_; + std::uint16_t vendor_id_; + std::uint16_t product_id_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2DeviceInfo& device_info); + + } // namespace + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device_manager.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device_manager.h new file mode 100755 index 0000000..c0678e8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_device_manager.h @@ -0,0 +1,97 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2DeviceListener; + + class PCL_EXPORTS OpenNI2DeviceManager + { + public: + OpenNI2DeviceManager (); + virtual ~OpenNI2DeviceManager (); + + // This may not actually be a singleton yet. Need to work out cross-dll incerface. + // Based on http://stackoverflow.com/a/13431981/1789618 + static shared_ptr getInstance () + { + static auto instance = pcl::make_shared(); + return (instance); + } + + std::shared_ptr> + getConnectedDeviceInfos () const; + + std::shared_ptr> + getConnectedDeviceURIs () const; + + std::size_t + getNumOfConnectedDevices () const; + + OpenNI2Device::Ptr + getAnyDevice (); + + OpenNI2Device::Ptr + getDevice (const std::string& device_URI); + + OpenNI2Device::Ptr + getDeviceByIndex (int index); + + OpenNI2Device::Ptr + getFileDevice (const std::string& path); + + protected: + std::shared_ptr device_listener_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2DeviceManager& device_manager); + + } // namespace + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_frame_listener.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_frame_listener.h new file mode 100755 index 0000000..87273df --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_frame_listener.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + using StreamCallbackFunction = std::function; + + /* Each NewFrameListener may only listen to one VideoStream at a time. + **/ + class OpenNI2FrameListener : public openni::VideoStream::NewFrameListener + { + public: + + OpenNI2FrameListener () + {} + OpenNI2FrameListener (StreamCallbackFunction cb) + : callback_(std::move(cb)) {} + + ~OpenNI2FrameListener () + { }; + + inline void + onNewFrame (openni::VideoStream& stream) override + { + if (callback_) + callback_(stream); + } + + void + setCallback (StreamCallbackFunction cb) + { + callback_ = std::move(cb); + } + + private: + StreamCallbackFunction callback_; + }; + + } // namespace + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_metadata_wrapper.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_metadata_wrapper.h new file mode 100755 index 0000000..d49dbae --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_metadata_wrapper.h @@ -0,0 +1,112 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#if defined(HAVE_OPENNI2) + +#include + +#include +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + class Openni2FrameWrapper : public pcl::io::FrameWrapper + { + public: + Openni2FrameWrapper (const openni::VideoFrameRef& metadata) + : metadata_(metadata) + {} + + inline const void* + getData () const override + { + return (metadata_.getData ()); + } + + inline unsigned + getDataSize () const override + { + return (metadata_.getDataSize ()); + } + + inline unsigned + getWidth () const override + { + return (metadata_.getWidth ()); + } + + inline unsigned + getHeight () const override + { + return (metadata_.getHeight ()); + } + + inline unsigned + getFrameID () const override + { + return (metadata_.getFrameIndex ()); + } + + inline std::uint64_t + getTimestamp () const override + { + return (metadata_.getTimestamp ()); + } + + + const inline openni::VideoFrameRef& + getMetaData () const + { + return (metadata_); + } + + private: + openni::VideoFrameRef metadata_; // Internally reference counted + }; + + } // namespace + } +} +#endif // HAVE_OPENNI2 diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_timer_filter.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_timer_filter.h new file mode 100755 index 0000000..50d6433 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_timer_filter.h @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#pragma once + +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2TimerFilter + { + public: + OpenNI2TimerFilter (std::size_t filter_len); + virtual ~OpenNI2TimerFilter (); + + void + addSample (double sample); + + double + getMedian (); + + double + getMovingAvg (); + + void + clear (); + + private: + std::size_t filter_len_; + + std::deque buffer_; + }; + + } // namespace + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_video_mode.h b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_video_mode.h new file mode 100755 index 0000000..dbd87e4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2/openni2_video_mode.h @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#pragma once + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + // copied from OniEnums.h + enum PixelFormat + { + // Depth + PIXEL_FORMAT_DEPTH_1_MM = 100, + PIXEL_FORMAT_DEPTH_100_UM = 101, + PIXEL_FORMAT_SHIFT_9_2 = 102, + PIXEL_FORMAT_SHIFT_9_3 = 103, + + // Color + PIXEL_FORMAT_RGB888 = 200, + PIXEL_FORMAT_YUV422 = 201, + PIXEL_FORMAT_GRAY8 = 202, + PIXEL_FORMAT_GRAY16 = 203, + PIXEL_FORMAT_JPEG = 204, + PIXEL_FORMAT_YUYV = 205, + }; + + struct OpenNI2VideoMode + { + OpenNI2VideoMode () + :x_resolution_(0), y_resolution_(0), frame_rate_(0) + {} + + OpenNI2VideoMode (int xResolution, int yResolution, int frameRate) + :x_resolution_(xResolution), y_resolution_(yResolution), frame_rate_(frameRate) + {} + + int x_resolution_; + int y_resolution_; + int frame_rate_; + PixelFormat pixel_format_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2VideoMode& video_mode); + + bool + operator== (const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); + + bool + operator!= (const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); + + } // namespace + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni2_grabber.h b/deps_install/include/pcl-1.10/pcl/io/openni2_grabber.h new file mode 100755 index 0000000..83a2aab --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni2_grabber.h @@ -0,0 +1,510 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#ifdef HAVE_OPENNI2 + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace pcl +{ + struct PointXYZ; + struct PointXYZRGB; + struct PointXYZRGBA; + struct PointXYZI; + + namespace io + { + + /** \brief Grabber for OpenNI 2 devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) + * \ingroup io + */ + class PCL_EXPORTS OpenNI2Grabber : public Grabber + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + // Templated images + using DepthImage = pcl::io::DepthImage; + using IRImage = pcl::io::IRImage; + using Image = pcl::io::Image; + + /** \brief Basic camera parameters placeholder. */ + struct CameraParameters + { + /** fx */ + double focal_length_x; + /** fy */ + double focal_length_y; + /** cx */ + double principal_point_x; + /** cy */ + double principal_point_y; + + CameraParameters (double initValue) + : focal_length_x (initValue), focal_length_y (initValue), + principal_point_x (initValue), principal_point_y (initValue) + {} + + CameraParameters (double fx, double fy, double cx, double cy) + : focal_length_x (fx), focal_length_y (fy), principal_point_x (cx), principal_point_y (cy) + { } + }; + + enum Mode + { + OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz + OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect + OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect + OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion + OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion + OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect + OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion + OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN) + }; + + //define callback signature typedefs + using sig_cb_openni_image = void (const Image::Ptr &); + using sig_cb_openni_depth_image = void (const DepthImage::Ptr &); + using sig_cb_openni_ir_image = void (const IRImage::Ptr &); + using sig_cb_openni_image_depth_image = void (const Image::Ptr &, const DepthImage::Ptr &, float) ; + using sig_cb_openni_ir_depth_image = void (const IRImage::Ptr &, const DepthImage::Ptr &, float) ; + using sig_cb_openni_point_cloud = void (const typename pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_rgb = void (const typename pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_rgba = void (const typename pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_i = void (const typename pcl::PointCloud::ConstPtr &); + + public: + /** \brief Constructor + * \param[in] device_id ID of the device, which might be a serial number, bus@address, URI or the index of the device. + * \param[in] depth_mode the mode of the depth stream + * \param[in] image_mode the mode of the image stream + * Depending on the value of \a device_id, the device is opened as follows: + * * If it corresponds to a file path, the device is opened with OpenNI2DeviceManager::getFileDevice + * * If it is an index of the form "#1234", the device is opened with OpenNI2DeviceManager::getDeviceByIndex + * * If it corresponds to an URI, the device is opened with OpenNI2DeviceManager::getDevice + * * If it is an empty string, the device is opened with OpenNI2DeviceManager::getAnyDevice + * * Otherwise a pcl::IOException instance is thrown + */ + OpenNI2Grabber (const std::string& device_id = "", + const Mode& depth_mode = OpenNI_Default_Mode, + const Mode& image_mode = OpenNI_Default_Mode); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + ~OpenNI2Grabber () noexcept; + + /** \brief Start the data acquisition. */ + void + start () override; + + /** \brief Stop the data acquisition. */ + void + stop () override; + + /** \brief Check if the data acquisition is still running. */ + bool + isRunning () const override; + + std::string + getName () const override; + + /** \brief Obtain the number of frames per second (FPS). */ + float + getFramesPerSecond () const override; + + /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */ + inline pcl::io::openni2::OpenNI2Device::Ptr + getDevice () const; + + /** \brief Obtain a list of the available depth modes that this device supports. */ + std::vector > + getAvailableDepthModes () const; + + /** \brief Obtain a list of the available image modes that this device supports. */ + std::vector > + getAvailableImageModes () const; + + /** \brief Set the RGB camera parameters (fx, fy, cx, cy) + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_length_y the RGB focal length (fy) + * \param[in] rgb_principal_point_x the RGB principal point (cx) + * \param[in] rgb_principal_point_y the RGB principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setRGBCameraIntrinsics (const double rgb_focal_length_x, + const double rgb_focal_length_y, + const double rgb_principal_point_x, + const double rgb_principal_point_y) + { + rgb_parameters_ = CameraParameters ( + rgb_focal_length_x, rgb_focal_length_y, + rgb_principal_point_x, rgb_principal_point_y); + } + + /** \brief Get the RGB camera parameters (fx, fy, cx, cy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + * \param[out] rgb_principal_point_x the RGB principal point (cx) + * \param[out] rgb_principal_point_y the RGB principal point (cy) + */ + inline void + getRGBCameraIntrinsics (double &rgb_focal_length_x, + double &rgb_focal_length_y, + double &rgb_principal_point_x, + double &rgb_principal_point_y) const + { + rgb_focal_length_x = rgb_parameters_.focal_length_x; + rgb_focal_length_y = rgb_parameters_.focal_length_y; + rgb_principal_point_x = rgb_parameters_.principal_point_x; + rgb_principal_point_y = rgb_parameters_.principal_point_y; + } + + + /** \brief Set the RGB image focal length (fx = fy). + * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length) + { + rgb_parameters_.focal_length_x = rgb_focal_length; + rgb_parameters_.focal_length_y = rgb_focal_length; + } + + /** \brief Set the RGB image focal length + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_ulength_y the RGB focal length (fy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y) + { + rgb_parameters_.focal_length_x = rgb_focal_length_x; + rgb_parameters_.focal_length_y = rgb_focal_length_y; + } + + /** \brief Return the RGB focal length parameters (fx, fy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + */ + inline void + getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const + { + rgb_focal_length_x = rgb_parameters_.focal_length_x; + rgb_focal_length_y = rgb_parameters_.focal_length_y; + } + + /** \brief Set the Depth camera parameters (fx, fy, cx, cy) + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * \param[in] depth_principal_point_x the Depth principal point (cx) + * \param[in] depth_principal_point_y the Depth principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthCameraIntrinsics (const double depth_focal_length_x, + const double depth_focal_length_y, + const double depth_principal_point_x, + const double depth_principal_point_y) + { + depth_parameters_ = CameraParameters ( + depth_focal_length_x, depth_focal_length_y, + depth_principal_point_x, depth_principal_point_y); + } + + /** \brief Get the Depth camera parameters (fx, fy, cx, cy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + * \param[out] depth_principal_point_x the Depth principal point (cx) + * \param[out] depth_principal_point_y the Depth principal point (cy) + */ + inline void + getDepthCameraIntrinsics (double &depth_focal_length_x, + double &depth_focal_length_y, + double &depth_principal_point_x, + double &depth_principal_point_y) const + { + depth_focal_length_x = depth_parameters_.focal_length_x; + depth_focal_length_y = depth_parameters_.focal_length_y; + depth_principal_point_x = depth_parameters_.principal_point_x; + depth_principal_point_y = depth_parameters_.principal_point_y; + } + + /** \brief Set the Depth image focal length (fx = fy). + * \param[in] depth_focal_length the Depth focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length) + { + depth_parameters_.focal_length_x = depth_focal_length; + depth_parameters_.focal_length_y = depth_focal_length; + } + + + /** \brief Set the Depth image focal length + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y) + { + depth_parameters_.focal_length_x = depth_focal_length_x; + depth_parameters_.focal_length_y = depth_focal_length_y; + } + + /** \brief Return the Depth focal length parameters (fx, fy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + */ + inline void + getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const + { + depth_focal_length_x = depth_parameters_.focal_length_x; + depth_focal_length_y = depth_parameters_.focal_length_y; + } + + protected: + + /** \brief Sets up an OpenNI device. */ + void + setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); + + /** \brief Update mode maps. */ + void + updateModeMaps (); + + /** \brief Start synchronization. */ + void + startSynchronization (); + + /** \brief Stop synchronization. */ + void + stopSynchronization (); + + // TODO: rename to mapMode2OniMode + /** \brief Map config modes. */ + bool + mapMode2XnMode (int mode, pcl::io::openni2::OpenNI2VideoMode& videoMode) const; + + // callback methods + /** \brief RGB image callback. */ + virtual void + imageCallback (pcl::io::openni2::Image::Ptr image, void* cookie); + + /** \brief Depth image callback. */ + virtual void + depthCallback (pcl::io::openni2::DepthImage::Ptr depth_image, void* cookie); + + /** \brief IR image callback. */ + virtual void + irCallback (pcl::io::openni2::IRImage::Ptr ir_image, void* cookie); + + /** \brief RGB + Depth image callback. */ + virtual void + imageDepthImageCallback (const pcl::io::openni2::Image::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief IR + Depth image callback. */ + virtual void + irDepthImageCallback (const pcl::io::openni2::IRImage::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief Process changed signals. */ + void + signalsChanged () override; + + // helper methods + + /** \brief Check if the RGB and Depth images are required to be synchronized or not. */ + virtual void + checkImageAndDepthSynchronizationRequired (); + + /** \brief Check if the RGB image stream is required or not. */ + virtual void + checkImageStreamRequired (); + + /** \brief Check if the depth stream is required or not. */ + virtual void + checkDepthStreamRequired (); + + /** \brief Check if the IR image stream is required or not. */ + virtual void + checkIRStreamRequired (); + + + // Point cloud conversion /////////////////////////////////////////////// + + /** \brief Convert a Depth image to a pcl::PointCloud + * \param[in] depth the depth image to convert + */ + pcl::PointCloud::Ptr + convertToXYZPointCloud (const pcl::io::openni2::DepthImage::Ptr &depth); + + /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud + * \param[in] image the RGB image to convert + * \param[in] depth_image the depth image to convert + */ + template typename pcl::PointCloud::Ptr + convertToXYZRGBPointCloud (const pcl::io::openni2::Image::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud + * \param[in] image the IR image to convert + * \param[in] depth_image the depth image to convert + */ + pcl::PointCloud::Ptr + convertToXYZIPointCloud (const pcl::io::openni2::IRImage::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + std::vector color_resize_buffer_; + std::vector depth_resize_buffer_; + std::vector ir_resize_buffer_; + + // Stream callbacks ///////////////////////////////////////////////////// + void + processColorFrame (openni::VideoStream& stream); + + void + processDepthFrame (openni::VideoStream& stream); + + void + processIRFrame (openni::VideoStream& stream); + + + Synchronizer rgb_sync_; + Synchronizer ir_sync_; + + /** \brief The actual openni device. */ + pcl::io::openni2::OpenNI2Device::Ptr device_; + + std::string rgb_frame_id_; + std::string depth_frame_id_; + unsigned image_width_; + unsigned image_height_; + unsigned depth_width_; + unsigned depth_height_; + + bool image_required_; + bool depth_required_; + bool ir_required_; + bool sync_required_; + + boost::signals2::signal* image_signal_; + boost::signals2::signal* depth_image_signal_; + boost::signals2::signal* ir_image_signal_; + boost::signals2::signal* image_depth_image_signal_; + boost::signals2::signal* ir_depth_image_signal_; + boost::signals2::signal* point_cloud_signal_; + boost::signals2::signal* point_cloud_i_signal_; + boost::signals2::signal* point_cloud_rgb_signal_; + boost::signals2::signal* point_cloud_rgba_signal_; + + struct modeComp + { + bool operator () (const openni::VideoMode& mode1, const openni::VideoMode & mode2) const + { + if (mode1.getResolutionX () < mode2.getResolutionX ()) + return true; + if (mode1.getResolutionX () > mode2.getResolutionX ()) + return false; + if (mode1.getResolutionY () < mode2.getResolutionY ()) + return true; + if (mode1.getResolutionY () > mode2.getResolutionY ()) + return false; + return (mode1.getFps () < mode2.getFps ()); + } + }; + + // Mapping from config (enum) modes to native OpenNI modes + std::map config2oni_map_; + + pcl::io::openni2::OpenNI2Device::CallbackHandle depth_callback_handle_; + pcl::io::openni2::OpenNI2Device::CallbackHandle image_callback_handle_; + pcl::io::openni2::OpenNI2Device::CallbackHandle ir_callback_handle_; + bool running_; + + + CameraParameters rgb_parameters_; + CameraParameters depth_parameters_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + pcl::io::openni2::OpenNI2Device::Ptr + OpenNI2Grabber::getDevice () const + { + return device_; + } + + } // namespace +} + +#endif // HAVE_OPENNI2 diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/image.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image.h new file mode 100755 index 0000000..4cb43ee --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image.h @@ -0,0 +1,214 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Image interface class providing an interface to fill a RGB or Grayscale image buffer. + * @param[in] image_metadata + * @ingroup io + */ + class PCL_EXPORTS Image + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using Clock = std::chrono::high_resolution_clock; + using Timestamp = std::chrono::high_resolution_clock::time_point; + + enum Encoding + { + BAYER_GRBG, + YUV422, + RGB + }; + + Image (FrameWrapper::Ptr image_metadata) + : wrapper_ (std::move(image_metadata)) + , timestamp_ (Clock::now ()) + {} + + Image (FrameWrapper::Ptr image_metadata, Timestamp time) + : wrapper_ (std::move(image_metadata)) + , timestamp_ (time) + {} + + /** + * @brief virtual Destructor that never throws an exception. + */ + inline virtual ~Image () + {} + + /** + * @param[in] input_width width of input image + * @param[in] input_height height of input image + * @param[in] output_width width of desired output image + * @param[in] output_height height of desired output image + * @return whether the resizing is supported or not. + */ + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, + unsigned output_width, unsigned output_height) const = 0; + + /** + * @brief fills a user given buffer with the RGB values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] rgb_buffer the output RGB buffer. + * @param[in] rgb_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const = 0; + + /** + * @brief returns the encoding of the native data. + * @return encoding + */ + virtual Encoding + getEncoding () const = 0; + + /** + * @brief fills a user given buffer with the raw values. + * @param[in,out] rgb_buffer + */ + virtual void + fillRaw (unsigned char* rgb_buffer) const + { + memcpy (rgb_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + } + + /** + * @brief fills a user given buffer with the gray values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] gray_buffer the output gray buffer. + * @param[in] gray_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, + unsigned gray_line_step = 0) const = 0; + + /** + * @return width of the image + */ + unsigned + getWidth () const + { + return (wrapper_->getWidth ()); + } + + /** + * @return height of the image + */ + unsigned + getHeight () const + { + return (wrapper_->getHeight ()); + } + + /** + * @return frame id of the image. + * @note frame ids are ascending, but not necessarily synchronized with other streams + */ + unsigned + getFrameID () const + { + return (wrapper_->getFrameID ()); + } + + /** + * @return the timestamp of the image + * @note the time value is not synchronized with the system time + */ + std::uint64_t + getTimestamp () const + { + return (wrapper_->getTimestamp ()); + } + + + /** + * @return the timestamp of the image + * @note the time value *is* synchronized with the system time. + */ + Timestamp + getSystemTimestamp () const + { + return (timestamp_); + } + + // Get a const pointer to the raw depth buffer + const void* + getData () + { + return (wrapper_->getData ()); + } + + // Data buffer size in bytes + int + getDataSize () const + { + return (wrapper_->getDataSize ()); + } + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_depth.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_depth.h new file mode 100755 index 0000000..2e16ebd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_depth.h @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** \brief This class provides methods to fill a depth or disparity image. + */ + class PCL_EXPORTS DepthImage + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using Clock = std::chrono::high_resolution_clock; + using Timestamp = std::chrono::high_resolution_clock::time_point; + + /** \brief Constructor + * \param[in] depth_metadata the actual data from the OpenNI library + * \param[in] baseline the baseline of the "stereo" camera, i.e. the distance between the projector and the IR camera for + * Primesense like cameras. e.g. 7.5cm for PSDK5 and PSDK6 reference design. + * \param[in] focal_length focal length of the "stereo" frame. + * \param[in] shadow_value defines which values in the depth data are indicating shadow (resulting from the parallax between projector and IR camera) + * \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined . + * \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not. + */ + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, std::uint64_t shadow_value, std::uint64_t no_sample_value); + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, std::uint64_t shadow_value, std::uint64_t no_sample_value, Timestamp time); + + /** \brief Destructor. Never throws an exception. */ + ~DepthImage (); + + /** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method + * \return the actual depth data of type openni::VideoFrameRef. + */ + const FrameWrapper::Ptr + getMetaData () const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width the width of the desired disparity image. + * \param[in] height the height of the desired disparity image. + * \param[in,out] disparity_buffer the float pointer to the actual memory buffer to be filled with the disparity values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired depth image. + * \param[in] height height the height of the desired depth image. + * \param[in,out] depth_buffer the float pointer to the actual memory buffer to be filled with the depth values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the raw values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired raw image. + * \param[in] height height the height of the desired raw image. + * \param[in,out] depth_buffer the unsigned short pointer to the actual memory buffer to be filled with the raw values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step = 0) const; + + /** \brief method to access the baseline of the "stereo" frame that was used to retrieve the depth image. + * \return baseline in meters + */ + float + getBaseline () const; + + /** \brief method to access the focal length of the "stereo" frame that was used to retrieve the depth image. + * \return focal length in pixels + */ + float + getFocalLength () const; + + /** \brief method to access the shadow value, that indicates pixels lying in shadow in the depth image. + * \return shadow value + */ + std::uint64_t + getShadowValue () const; + + /** \brief method to access the no-sample value, that indicates pixels where no disparity could be determined for the depth image. + * \return no-sample value + */ + std::uint64_t + getNoSampleValue () const; + + /** \return the width of the depth image */ + unsigned + getWidth () const; + + /** \return the height of the depth image */ + unsigned + getHeight () const; + + /** \return an ascending id for the depth frame + * \attention not necessarily synchronized with other streams + */ + unsigned + getFrameID () const; + + /** \return a ascending timestamp for the depth frame + * \attention its not the system time, thus can not be used directly to synchronize different sensors. + * But definitely synchronized with other streams + */ + std::uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + pcl::io::FrameWrapper::Ptr wrapper_; + + float baseline_; + float focal_length_; + std::uint64_t shadow_value_; + std::uint64_t no_sample_value_; + Timestamp timestamp_; + }; + +}} // namespace diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_ir.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_ir.h new file mode 100755 index 0000000..ad6c275 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_ir.h @@ -0,0 +1,114 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + +#pragma once + +#include + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Class containing just a reference to IR meta data. + */ + class PCL_EXPORTS IRImage + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using Clock = std::chrono::high_resolution_clock; + using Timestamp = std::chrono::high_resolution_clock::time_point; + + IRImage (FrameWrapper::Ptr ir_metadata); + IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time); + + ~IRImage () noexcept + {} + + void + fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step = 0) const; + + unsigned + getWidth () const; + + unsigned + getHeight () const; + + unsigned + getFrameID () const; + + std::uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer. If the data is accessed just read-only, then this method is faster than a fillXXX method + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + /** \brief method to access the internal data structure wrapper, which needs to be cast to an + * approperate subclass before the getMetadata(..) function is available to access the native data type. + */ + const FrameWrapper::Ptr + getMetaData () const; + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_metadata_wrapper.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_metadata_wrapper.h new file mode 100755 index 0000000..fcc290f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_metadata_wrapper.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace io + { + + /** + * Pure abstract interface to wrap native frame data types. + */ + class FrameWrapper + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + virtual + ~FrameWrapper() = default; + + virtual const void* + getData () const = 0; + + virtual unsigned + getDataSize () const = 0; + + virtual unsigned + getWidth () const = 0; + + virtual unsigned + getHeight () const = 0; + + virtual unsigned + getFrameID () const = 0; + + // Microseconds from some arbitrary start point + virtual std::uint64_t + getTimestamp () const = 0; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_rgb24.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_rgb24.h new file mode 100755 index 0000000..36d5e03 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_rgb24.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image. + * @ingroup io + */ + class PCL_EXPORTS ImageRGB24 : public pcl::io::Image + { + public: + + ImageRGB24 (FrameWrapper::Ptr image_metadata); + ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + ~ImageRGB24 () noexcept; + + inline Encoding + getEncoding () const override + { + return (RGB); + } + + void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; + + void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; + + bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; + + private: + + // Struct used for type conversion + struct RGB888Pixel + { + std::uint8_t r; + std::uint8_t g; + std::uint8_t b; + }; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_yuv422.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_yuv422.h new file mode 100755 index 0000000..08f7d22 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/image_yuv422.h @@ -0,0 +1,77 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + +#pragma once + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief Concrete implementation of the interface Image for a YUV 422 image used by Primesense devices. + * @ingroup io + */ + class PCL_EXPORTS ImageYUV422 : public pcl::io::Image + { + public: + ImageYUV422 (FrameWrapper::Ptr image_metadata); + ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + + ~ImageYUV422 () noexcept; + + inline Encoding + getEncoding () const override + { + return (YUV422); + } + + void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; + + void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; + + bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; + }; + + } // namespace +} diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni.h new file mode 100755 index 0000000..c60a0cf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni.h @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#ifdef HAVE_OPENNI + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +//work around for qt 5 bug: https://bugreports.qt-project.org/browse/QTBUG-29331 +#ifndef Q_MOC_RUN +#include +#endif // Q_MOC_RUN +#include + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_depth_image.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_depth_image.h new file mode 100755 index 0000000..66e5440 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_depth_image.h @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include "openni.h" + +//#include // <-- because current header is included in NVCC-compiled code and contains . Consider +#include +#include "openni_exception.h" +#include + +namespace openni_wrapper +{ + /** \brief This class provides methods to fill a depth or disparity image. + * \author Suat Gedikli + */ + class PCL_EXPORTS DepthImage + { + public: + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; + + /** \brief Constructor + * \param[in] depth_meta_data the actual data from the OpenNI library + * \param[in] baseline the baseline of the "stereo" camera, i.e. the distance between the projector and the IR camera for + * Primesense like cameras. e.g. 7.5cm for PSDK5 and PSDK6 reference design. + * \param[in] focal_length focal length of the "stereo" frame. + * \param[in] shadow_value defines which values in the depth data are indicating shadow (resulting from the parallax between projector and IR camera) + * \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined . + * \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not. + */ + inline DepthImage (pcl::shared_ptr depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) noexcept; + + /** \brief Destructor. Never throws an exception. */ + inline virtual ~DepthImage () noexcept; + + /** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method + * \return the actual depth data of type xn::DepthMetaData. + */ + inline const xn::DepthMetaData& + getDepthMetaData () const throw (); + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width the width of the desired disparity image. + * \param[in] height the height of the desired disparity image. + * \param[in,out] disparity_buffer the float pointer to the actual memory buffer to be filled with the disparity values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired depth image. + * \param[in] height height the height of the desired depth image. + * \param[in,out] depth_buffer the float pointer to the actual memory buffer to be filled with the depth values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the raw values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired raw image. + * \param[in] height height the height of the desired raw image. + * \param[in,out] depth_buffer the unsigned short pointer to the actual memory buffer to be filled with the raw values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step = 0) const; + + /** \brief method to access the baseline of the "stereo" frame that was used to retrieve the depth image. + * \return baseline in meters + */ + inline float + getBaseline () const throw (); + + /** \brief method to access the focal length of the "stereo" frame that was used to retrieve the depth image. + * \return focal length in pixels + */ + inline float + getFocalLength () const throw (); + + /** \brief method to access the shadow value, that indicates pixels lying in shadow in the depth image. + * \return shadow value + */ + inline XnUInt64 + getShadowValue () const throw (); + + /** \brief method to access the no-sample value, that indicates pixels where no disparity could be determined for the depth image. + * \return no-sample value + */ + inline XnUInt64 + getNoSampleValue () const throw (); + + /** \return the width of the depth image */ + inline unsigned + getWidth () const throw (); + + /** \return the height of the depth image */ + inline unsigned + getHeight () const throw (); + + /** \return an ascending id for the depth frame + * \attention not necessarily synchronized with other streams + */ + inline unsigned + getFrameID () const throw (); + + /** \return a ascending timestamp for the depth frame + * \attention its not the system time, thus can not be used directly to synchronize different sensors. + * But definitely synchronized with other streams + */ + inline unsigned long + getTimeStamp () const throw (); + + protected: + pcl::shared_ptr depth_md_; + float baseline_; + float focal_length_; + XnUInt64 shadow_value_; + XnUInt64 no_sample_value_; + } ; + + DepthImage::DepthImage (pcl::shared_ptr depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) noexcept + : depth_md_ (std::move(depth_meta_data)) + , baseline_ (baseline) + , focal_length_ (focal_length) + , shadow_value_ (shadow_value) + , no_sample_value_ (no_sample_value) { } + + DepthImage::~DepthImage () noexcept { } + + const xn::DepthMetaData& + DepthImage::getDepthMetaData () const throw () + { + return *depth_md_; + } + + float + DepthImage::getBaseline () const throw () + { + return baseline_; + } + + float + DepthImage::getFocalLength () const throw () + { + return focal_length_; + } + + XnUInt64 + DepthImage::getShadowValue () const throw () + { + return shadow_value_; + } + + XnUInt64 + DepthImage::getNoSampleValue () const throw () + { + return no_sample_value_; + } + + unsigned + DepthImage::getWidth () const throw () + { + return depth_md_->XRes (); + } + + unsigned + DepthImage::getHeight () const throw () + { + return depth_md_->YRes (); + } + + unsigned + DepthImage::getFrameID () const throw () + { + return depth_md_->FrameID (); + } + + unsigned long + DepthImage::getTimeStamp () const throw () + { + return static_cast (depth_md_->Timestamp ()); + } +} // namespace +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device.h new file mode 100755 index 0000000..afdc35b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device.h @@ -0,0 +1,619 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include "openni_exception.h" +#include "openni.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +/// @todo Get rid of all exception-specifications, these are useless and soon to be deprecated + +#ifndef _WIN32 +#define __stdcall +#endif + +namespace openni_wrapper +{ + /** \brief Class representing an astract device for OpenNI devices: Primesense PSDK, Microsoft Kinect, Asus Xtion Pro/Live. + * \author Suat Gedikli + * \ingroup io + */ + class PCL_EXPORTS OpenNIDevice + { + public: + enum DepthMode + { + OpenNI_shift_values = 0, // Shift values (disparity) + OpenNI_12_bit_depth = 1, // Default mode: regular 12-bit depth + }; + + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; + + using ImageCallbackFunction = std::function; + using DepthImageCallbackFunction = std::function; + using IRImageCallbackFunction = std::function; + using CallbackHandle = unsigned; + + public: + + /** \brief virtual destructor. Never throws an exception. */ + virtual ~OpenNIDevice () noexcept; + + /** \brief finds an image output mode that can be used to retrieve images in desired output mode. + * e.g If device just supports VGA at 30Hz, then the desired mode QVGA at 30Hz would be possible by down sampling, + * but the modes VGA at 25Hz and SXGA at 30Hz would not be compatible. + * \param[in] output_mode the desired output mode + * \param[out] mode the compatible mode that the device natively supports. + * \return true, if a compatible mode could be found, false otherwise. + */ + bool + findCompatibleImageMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw (); + + /** \brief finds a depth output mode that can be used to retrieve depth images in desired output mode. + * e.g If device just supports VGA at 30Hz, then a desired mode of QVGA at 30Hz would be possbile by downsampling, + * but the modes VGA at 25Hz and SXGA at 30Hz would not be compatible. + * \param[in] output_mode the desired output mode + * \param[out] mode the compatible mode that the device natively supports. + * \return true, if a compatible mode could be found, false otherwise. + */ + bool + findCompatibleDepthMode (const XnMapOutputMode& output_mode, XnMapOutputMode& mode ) const throw (); + + /** \brief returns whether a given mode is natively supported by the device or not + * \param[in] output_mode mode to be checked + * \return true if mode natively available, false otherwise + */ + bool + isImageModeSupported (const XnMapOutputMode& output_mode) const throw (); + + /** \brief returns whether a given mode is natively supported by the device or not + * \param[in] output_mode mode to be checked + * \return true if mode natively available, false otherwise + */ + bool + isDepthModeSupported (const XnMapOutputMode& output_mode) const throw (); + + /** \brief returns the default image mode, which is simply the first entry in the list of modes + * \return the default image mode + */ + const XnMapOutputMode& + getDefaultImageMode () const throw (); + + /** \brief returns the default depth mode, which is simply the first entry in the list of modes + * \return the default depth mode + */ + const XnMapOutputMode& + getDefaultDepthMode () const throw (); + + /** \brief returns the default IR mode, which is simply the first entry in the list of modes + * \return the default IR mode + */ + const XnMapOutputMode& + getDefaultIRMode () const throw (); + + /** \brief sets the output mode of the image stream + * \param[in] output_mode the desired output mode + */ + void + setImageOutputMode (const XnMapOutputMode& output_mode); + + /** \brief sets the output mode of the depth stream + * \param[in] output_mode the desired output mode + */ + void + setDepthOutputMode (const XnMapOutputMode& output_mode); + + /** \brief sets the output mode of the IR stream + * \param[in] output_mode the desired output mode + */ + void + setIROutputMode (const XnMapOutputMode& output_mode); + + /** \return the current output mode of the image stream */ + XnMapOutputMode + getImageOutputMode () const; + + /** \return the current output mode of the depth stream */ + XnMapOutputMode + getDepthOutputMode () const; + + /** \return the current output mode of the IR stream */ + XnMapOutputMode + getIROutputMode () const; + + /** \brief set the depth stream registration on or off + * \param[in] on_off + */ + void + setDepthRegistration (bool on_off); + + /** \return whether the depth stream is registered to the RGB camera fram or not. */ + bool + isDepthRegistered () const throw (); + + /** \return whether a registration of the depth stream to the RGB camera frame is supported or not. */ + bool + isDepthRegistrationSupported () const throw (); + + /** \brief set the hardware synchronization between Depth and RGB stream on or off. + * \param[in] on_off + */ + void + setSynchronization (bool on_off); + + /** \return true if Depth stream is synchronized to RGB stream, false otherwise. */ + bool + isSynchronized () const throw (); + + /** \return true if the Device supports hardware synchronization between Depth and RGB streams or not. */ + virtual bool + isSynchronizationSupported () const throw (); + + /** \return true if depth stream is a cropped version of the native depth stream, false otherwise. */ + bool + isDepthCropped () const; + + /** \brief turn on cropping for the depth stream. + * \param[in] x x-position of the rectangular subregion. + * \param[in] y y-position of the rectangular subregion. + * \param[in] width width of the rectangular subregion. + * \param[in] height height of the rectangular subregion. + */ + void + setDepthCropping (unsigned x, unsigned y, unsigned width, unsigned height); + + /** \return true if cropping of the depth stream is supported, false otherwise. */ + bool + isDepthCroppingSupported () const throw (); + + /** \brief returns the focal length for the color camera in pixels. The pixels are assumed to be square. + * Result depends on the output resolution of the image. + */ + inline float + getImageFocalLength (int output_x_resolution = 0) const throw (); + + /** \brief returns the focal length for the IR camera in pixels. The pixels are assumed to be square. + * Result depends on the output resolution of the depth image. + */ + inline float + getDepthFocalLength (int output_x_resolution = 0) const throw (); + + /** \return Baseline of the "stereo" frame. i.e. for PSDK compatible devices its the distance between the Projector and the IR camera. */ + inline float + getBaseline () const throw (); + + /** \brief starts the image stream. */ + virtual void + startImageStream (); + + /** \brief stops the image stream. */ + virtual void + stopImageStream (); + + /** \brief starts the depth stream. */ + virtual void + startDepthStream (); + + /** \brief stops the depth stream. */ + virtual void + stopDepthStream (); + + /** \brief starts the IR stream. */ + virtual void + startIRStream (); + + /** \brief stops the IR stream. */ + virtual void + stopIRStream (); + + /** \return true if the device supports an image stream, false otherwise. */ + bool + hasImageStream () const throw (); + + /** \return true if the device supports a depth stream, false otherwise. */ + bool + hasDepthStream () const throw (); + + /** \return true if the device supports an IR stream, false otherwise. */ + bool + hasIRStream () const throw (); + + /** \return true if the image stream is running / started, false otherwise. */ + virtual bool + isImageStreamRunning () const throw (); + + /** \return true if the depth stream is running / started, false otherwise. */ + virtual bool + isDepthStreamRunning () const throw (); + + /** \return true if the IR stream is running / started, false otherwise. */ + virtual bool + isIRStreamRunning () const throw (); + + /** \brief registers a callback function of std::function type for the image stream with an optional user defined parameter. + * The callback will always be called with a new image and the user data "cookie". + * \param[in] callback the user callback to be called if a new image is available + * \param[in] cookie the cookie that needs to be passed to the callback together with the new image. + * \return a callback handler that can be used to remove the user callback from list of image-stream callbacks. + */ + CallbackHandle + registerImageCallback (const ImageCallbackFunction& callback, void* cookie = nullptr) noexcept; + + /** \brief registers a callback function for the image stream with an optional user defined parameter. + * This version is used to register a member function of any class. + * The callback will always be called with a new image and the user data "cookie". + * \param[in] callback the user callback to be called if a new image is available + * \param instance + * \param[in] cookie the cookie that needs to be passed to the callback together with the new image. + * \return a callback handler that can be used to remove the user callback from list of image-stream callbacks. + */ + template CallbackHandle + registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept; + + /** \brief unregisters a callback function. i.e. removes that function from the list of image stream callbacks. + * \param[in] callbackHandle the handle of the callback to unregister. + * \return true, if callback was in list and could be unregistered, false otherwise. + */ + bool + unregisterImageCallback (const CallbackHandle& callbackHandle) noexcept; + + + /** \brief registers a callback function of std::function type for the depth stream with an optional user defined parameter. + * The callback will always be called with a new depth image and the user data "cookie". + * \param[in] callback the user callback to be called if a new depth image is available + * \param[in] cookie the cookie that needs to be passed to the callback together with the new depth image. + * \return a callback handler that can be used to remove the user callback from list of depth-stream callbacks. + */ + CallbackHandle + registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = nullptr) noexcept; + + /** \brief registers a callback function for the depth stream with an optional user defined parameter. + * This version is used to register a member function of any class. + * The callback will always be called with a new depth image and the user data "cookie". + * \param[in] callback the user callback to be called if a new depth image is available + * \param instance + * \param[in] cookie the cookie that needs to be passed to the callback together with the new depth image. + * \return a callback handler that can be used to remove the user callback from list of depth-stream callbacks. + */ + template CallbackHandle + registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept; + + /** \brief unregisters a callback function. i.e. removes that function from the list of depth stream callbacks. + * \param[in] callbackHandle the handle of the callback to unregister. + * \return true, if callback was in list and could be unregistered, false otherwise. + */ + bool + unregisterDepthCallback (const CallbackHandle& callbackHandle) noexcept; + + /** \brief registers a callback function of std::function type for the IR stream with an optional user defined parameter. + * The callback will always be called with a new IR image and the user data "cookie". + * \param[in] callback the user callback to be called if a new IR image is available + * \param[in] cookie the cookie that needs to be passed to the callback together with the new IR image. + * \return a callback handler that can be used to remove the user callback from list of IR-stream callbacks. + */ + CallbackHandle + registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = nullptr) noexcept; + + /** \brief registers a callback function for the IR stream with an optional user defined parameter. + * This version is used to register a member function of any class. + * The callback will always be called with a new IR image and the user data "cookie". + * \param[in] callback the user callback to be called if a new IR image is available + * \param instance + * \param[in] cookie the cookie that needs to be passed to the callback together with the new IR image. + * \return a callback handler that can be used to remove the user callback from list of IR-stream callbacks. + */ + template CallbackHandle + registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept; + + /** \brief unregisters a callback function. i.e. removes that function from the list of IR stream callbacks. + * \param[in] callbackHandle the handle of the callback to unregister. + * \return true, if callback was in list and could be unregistered, false otherwise. + */ + bool + unregisterIRCallback (const CallbackHandle& callbackHandle) noexcept; + + /** \brief returns the serial number for device. + * \attention This might be an empty string!!! + */ + const char* + getSerialNumber () const throw (); + + /** \brief returns the connection string for current device, which has following format vendorID/productID\@BusID/DeviceID. */ + const char* + getConnectionString () const throw (); + + /** \return the Vendor name of the USB device. */ + const char* + getVendorName () const throw (); + + /** \return the product name of the USB device. */ + const char* + getProductName () const throw (); + + /** \return the vendor ID of the USB device. */ + unsigned short + getVendorID () const throw (); + + /** \return the product ID of the USB device. */ + unsigned short + getProductID () const throw (); + + /** \return the USB bus on which the device is connected. */ + unsigned char + getBus () const throw (); + + /** \return the USB Address of the device. */ + unsigned char + getAddress () const throw (); + + /** \brief Set the RGB image focal length. + * \param[in] focal_length the RGB image focal length + */ + inline void + setRGBFocalLength (float focal_length) + { + rgb_focal_length_SXGA_ = focal_length; + } + + /** \brief Set the depth image focal length. + * \param[in] focal_length the depth image focal length + */ + inline void + setDepthFocalLength (float focal_length) + { + depth_focal_length_SXGA_ = focal_length; + } + + /** \brief Set the depth output format. Use 12bit depth values or shift values. + * \param[in] depth_mode the depth output format + */ + void + setDepthOutputFormat (const DepthMode& depth_mode = OpenNI_12_bit_depth); + + /** \brief Get the depth output format as set by the user. */ + XnUInt64 + getDepthOutputFormat () const; + + + /** \brief Convert shift to depth value. */ + std::uint16_t + shiftToDepth (std::uint16_t shift_value) const + { + assert (shift_conversion_parameters_.init_); + + std::uint16_t ret = 0; + + // lookup depth value in shift lookup table + if (shift_value; + using ActualDepthImageCallbackFunction = std::function; + using ActualIRImageCallbackFunction = std::function; + + OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); + OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); + OpenNIDevice (xn::Context& context); + static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + + // This is a workaround, since in the NewDepthDataAvailable function WaitAndUpdateData leads to a dead-lock behaviour + // and retrieving image data without WaitAndUpdateData leads to incomplete images!!! + void + ImageDataThreadFunction (); + + void + DepthDataThreadFunction (); + + void + IRDataThreadFunction (); + + virtual bool + isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () = 0; + + void + setRegistration (bool on_off); + + virtual Image::Ptr + getCurrentImage (pcl::shared_ptr image_data) const throw () = 0; + + void + Init (); + + void InitShiftToDepthConversion(); + void ReadDeviceParametersFromSensorNode(); + + struct ShiftConversion + { + ShiftConversion() : init_(false) {} + + XnUInt16 zero_plane_distance_; + XnFloat zero_plane_pixel_size_; + XnFloat emitter_dcmos_distace_; + XnUInt32 max_shift_; + XnUInt32 device_max_shift_; + XnUInt32 const_shift_; + XnUInt32 pixel_size_factor_; + XnUInt32 param_coeff_; + XnUInt32 shift_scale_; + XnUInt32 min_depth_; + XnUInt32 max_depth_; + bool init_; + + } shift_conversion_parameters_; + + std::vector shift_to_depth_table_; + + // holds the callback functions together with custom data + // since same callback function can be registered multiple times with e.g. different custom data + // we use a map structure with a handle as the key + std::map image_callback_; + std::map depth_callback_; + std::map ir_callback_; + + std::vector available_image_modes_; + std::vector available_depth_modes_; + + /** \brief context to OpenNI driver*/ + xn::Context& context_; + /** \brief node object for current device */ + xn::NodeInfo device_node_info_; + + /** \brief Depth generator object. */ + xn::DepthGenerator depth_generator_; + /** \brief Image generator object. */ + xn::ImageGenerator image_generator_; + /** \brief IR generator object. */ + xn::IRGenerator ir_generator_; + + XnCallbackHandle depth_callback_handle_; + XnCallbackHandle image_callback_handle_; + XnCallbackHandle ir_callback_handle_; + + /** \brief focal length for IR camera producing depth information in native SXGA mode */ + float depth_focal_length_SXGA_; + /** \brief distance between the projector and the IR camera*/ + float baseline_; + /** \brief focal length for regular camera producing color images in native SXGA mode */ + float rgb_focal_length_SXGA_; + + /** the value for shadow (occluded pixels) */ + XnUInt64 shadow_value_; + /** the value for pixels without a valid disparity measurement */ + XnUInt64 no_sample_value_; + + OpenNIDevice::CallbackHandle image_callback_handle_counter_; + OpenNIDevice::CallbackHandle depth_callback_handle_counter_; + OpenNIDevice::CallbackHandle ir_callback_handle_counter_; + + bool quit_; + mutable std::mutex image_mutex_; + mutable std::mutex depth_mutex_; + mutable std::mutex ir_mutex_; + std::condition_variable image_condition_; + std::condition_variable depth_condition_; + std::condition_variable ir_condition_; + std::thread image_thread_; + std::thread depth_thread_; + std::thread ir_thread_; + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + float + OpenNIDevice::getImageFocalLength (int output_x_resolution) const throw () + { + if (output_x_resolution == 0) + output_x_resolution = getImageOutputMode ().nXRes; + + float scale = static_cast (output_x_resolution) / static_cast (XN_SXGA_X_RES); + return (rgb_focal_length_SXGA_ * scale); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + float + OpenNIDevice::getDepthFocalLength (int output_x_resolution) const throw () + { + if (output_x_resolution == 0) + output_x_resolution = getDepthOutputMode ().nXRes; + + float scale = static_cast (output_x_resolution) / static_cast (XN_SXGA_X_RES); + if (isDepthRegistered ()) + return (rgb_focal_length_SXGA_ * scale); + return (depth_focal_length_SXGA_ * scale); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + float + OpenNIDevice::getBaseline () const throw () + { + return (baseline_); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template OpenNIDevice::CallbackHandle + OpenNIDevice::registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* custom_data) noexcept + { + image_callback_[image_callback_handle_counter_] = [=, &instance] (Image::Ptr img) { (instance.*callback) (img, custom_data); }; + return (image_callback_handle_counter_++); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template OpenNIDevice::CallbackHandle + OpenNIDevice::registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* custom_data) noexcept + { + depth_callback_[depth_callback_handle_counter_] = [=, &instance] (DepthImage::Ptr img) { (instance.*callback) (img, custom_data); }; + return (depth_callback_handle_counter_++); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template OpenNIDevice::CallbackHandle + OpenNIDevice::registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* custom_data) noexcept + { + ir_callback_[ir_callback_handle_counter_] = [=, &instance] (IRImage::Ptr img) { (instance.*callback) (img, custom_data); }; + return (ir_callback_handle_counter_++); + } + +} +#endif // HAVE_OPENNI diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_kinect.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_kinect.h new file mode 100755 index 0000000..c719c7e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_kinect.h @@ -0,0 +1,88 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include "openni_device.h" +#include "openni_driver.h" +#include "openni_image_bayer_grbg.h" + +namespace openni_wrapper +{ + + /** + * @brief Concrete implementation of the interface OpenNIDevice for a MS Kinect device. + * @author Suat Gedikli + * @date 02.january 2011 + * @ingroup io + */ + class DeviceKinect : public OpenNIDevice + { + friend class OpenNIDriver; + public: + DeviceKinect (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); + ~DeviceKinect () noexcept; + + inline void setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) noexcept; + inline const ImageBayerGRBG::DebayeringMethod& getDebayeringMethod () const throw (); + + bool isSynchronizationSupported () const throw () override; + + protected: + Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; + void enumAvailableModes () noexcept; + bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override; + ImageBayerGRBG::DebayeringMethod debayering_method_; + } ; + + void + DeviceKinect::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) noexcept + { + debayering_method_ = debayering_method; + } + + const ImageBayerGRBG::DebayeringMethod& + DeviceKinect::getDebayeringMethod () const throw () + { + return debayering_method_; + } +} // namespace + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_oni.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_oni.h new file mode 100755 index 0000000..913ec8e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_oni.h @@ -0,0 +1,119 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include "openni_device.h" +#include "openni_driver.h" + +#include + +#include +#include + +namespace openni_wrapper +{ + + /** + * @brief Concrete implementation of the interface OpenNIDevice for a virtual device playing back an ONI file. + * @author Suat Gedikli + * @date 19. june 2011 + * @ingroup io + */ + class DeviceONI : public OpenNIDevice + { + friend class OpenNIDriver; + public: + + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; + + DeviceONI (xn::Context& context, const std::string& file_name, bool repeat = false, bool streaming = true); + ~DeviceONI () noexcept; + + void startImageStream () override; + void stopImageStream () override; + + void startDepthStream () override; + void stopDepthStream () override; + + void startIRStream () override; + void stopIRStream () override; + + bool isImageStreamRunning () const throw () override; + bool isDepthStreamRunning () const throw () override; + bool isIRStreamRunning () const throw () override; + + bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override; + + /** \brief Trigger a new frame in the ONI stream. + * \param[in] relative_offset the relative offset in case we want to seek in the file + */ + bool + trigger (int relative_offset = 0); + + bool isStreaming () const throw (); + + /** \brief Check if there is any data left in the ONI file to process. */ + inline bool + hasDataLeft () + { + return (!player_.IsEOF ()); + } + + protected: + Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; + + void PlayerThreadFunction (); + static void __stdcall NewONIDepthDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewONIImageDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewONIIRDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + + xn::Player player_; + std::thread player_thread_; + mutable std::mutex player_mutex_; + std::condition_variable player_condition_; + bool streaming_; + bool depth_stream_running_; + bool image_stream_running_; + bool ir_stream_running_; + }; +} //namespace openni_wrapper +#endif //HAVE_OPENNI diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_primesense.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_primesense.h new file mode 100755 index 0000000..08b19df --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_primesense.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include "openni_device.h" +#include "openni_driver.h" + +#include + + +namespace openni_wrapper +{ +/** + * @brief Concrete implementation of the interface OpenNIDevice for a Primesense device. + * @author Suat Gedikli + * @date 02.january 2011 + * @ingroup io + */ +class DevicePrimesense : public OpenNIDevice +{ + friend class OpenNIDriver; +public: + DevicePrimesense (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); + ~DevicePrimesense () noexcept; + //virtual void setImageOutputMode (const XnMapOutputMode& output_mode); + +protected: + Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; + void enumAvailableModes () noexcept; + bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override; + + void startImageStream () override; + void startDepthStream () override; +}; +} // namespace + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_xtion.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_xtion.h new file mode 100755 index 0000000..ec21027 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_device_xtion.h @@ -0,0 +1,76 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include "openni_device.h" +#include "openni_driver.h" +#include "openni_image_yuv_422.h" + +#include + + +namespace openni_wrapper +{ + + /** + * @brief Concrete implementation of the interface OpenNIDevice for a Asus Xtion Pro device. + * @author Suat Gedikli + * @date 02.january 2011 + * @ingroup io + */ + class DeviceXtionPro : public OpenNIDevice + { + friend class OpenNIDriver; + public: + DeviceXtionPro (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); + ~DeviceXtionPro () noexcept; + //virtual void setImageOutputMode (const XnMapOutputMode& output_mode); + + protected: + Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; + void enumAvailableModes () noexcept; + bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override; + + void startDepthStream () override; + } ; +} // namespace + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_driver.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_driver.h new file mode 100755 index 0000000..6ece494 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_driver.h @@ -0,0 +1,252 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#ifdef HAVE_OPENNI + +#include "openni.h" +#include "openni_exception.h" +#include "openni_device.h" +#include +#include + +#include +#include +#include +#include + +namespace openni_wrapper +{ + //class OpenNIDevice; + + /** + * @brief Driver class implemented as Singleton. This class contains the xn::Context object used by all devices. It \ + * provides methods for enumerating and accessing devices. + * @author Suat Gedikli + * @date 02.january 2011 + * @ingroup io + */ + class PCL_EXPORTS OpenNIDriver + { + public: + /** + * @author Suat Gedikli + * @brief virtual Destructor that never throws an exception + */ + ~OpenNIDriver () noexcept; + + /** + * @author Suat Gedikli + * @brief static access method to the only instance. + * @return the only instance of this class. + */ + inline static OpenNIDriver& getInstance (); + + /** + * @author Suat Gedikli + * @brief enumerates all devices and updates the list of available devices. + * @return the number of devices found. + */ + unsigned updateDeviceList (); + + /** + * @author Suat Gedikli + * @return the number of available devices. + */ + inline unsigned getNumberDevices () const throw (); + + /** + * @author Suat Gedikli + * @brief creates a virtual device from an ONI file. + * @param[in] path the path to the ONI file + * @param[in] repeat whether the ONI playback should be repeated in an infinite loop or not. + * @param[in] stream whether the device should be created as a streaming or trigger-based device. + * @return the shared_ptr to the newly created virtual device. + */ + OpenNIDevice::Ptr createVirtualDevice (const std::string& path, bool repeat, bool stream) const; + + /** + * @author Suat Gedikli + * @brief returns the device with a given index, where the index is its position in the device list. + * @param[in] index index of the device to be retrieved. + * @return shared_ptr to the device, null if no matching device found. + */ + OpenNIDevice::Ptr getDeviceByIndex (unsigned index) const; + + /** + * @author Suat Gedikli + * @brief returns the device with the given serial number. + * @param[in] serial_number the serial number of the device to be retrieved. + * @return shared_ptr to the device, null if no matching device found. + */ + OpenNIDevice::Ptr getDeviceBySerialNumber (const std::string& serial_number) const; + +#ifndef _WIN32 + /** + * @author Suat Gedikli + * @brief returns the device that is given by the USB bus/address combination. + * @param[in] bus the USB bus id + * @param[in] address the USB address + * @return shared_ptr to the device, null if no matching device found. + */ + OpenNIDevice::Ptr getDeviceByAddress (unsigned char bus, unsigned char address) const; +#endif + + /** + * @author Suat Gedikli + * @brief method to retrieve the serial number of a device without creating it. + * @param[in] index the index of the device in the device list. + * @return the serial number of the device. + */ + const char* getSerialNumber (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief method to retrieve the connection string of a device without creating it. + * @param[in] index the index of the device in the device list. + * @return the connection string of the device. + */ + const char* getConnectionString (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief method to retrieve the vendor name of the USB device without creating it. + * @param[in] index the index of the device in the device list. + * @return the vendor name of the USB device. + */ + const char* getVendorName (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief method to retrieve the product name of the USB device without creating it. + * @param[in] index the index of the device in the device list. + * @return the product name of the USB device. + */ + const char* getProductName (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief method to retrieve the vendor id of the USB device without creating it. + * @param[in] index the index of the device in the device list. + * @return the vendor id of the USB device. + */ + unsigned short getVendorID (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief method to retrieve the product id of the USB device without creating it. + * @param[in] index the index of the device in the device list. + * @return the product id of the USB device. + */ + unsigned short getProductID (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief method to retrieve the bus id of the USB device without creating it. + * @param[in] index the index of the device in the device list. + * @return the bus id of the USB device. + */ + unsigned char getBus (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief method to retrieve the vaddress of the USB device without creating it. + * @param[in] index the index of the device in the device list. + * @return the address of the USB device. + */ + unsigned char getAddress (unsigned index) const throw (); + + /** + * @author Suat Gedikli + * @brief stops all streams from all devices. + */ + void stopAll (); + + /** + * @author Suat Gedikli + * @brief decomposes the connection string into vendor id and product id. + * @param[in] connection_string the string containing the connection information + * @param[out] vendorId the vendor id + * @param[out] productId the product id + */ + static void + getDeviceType (const std::string& connection_string, unsigned short& vendorId, unsigned short& productId); + protected: + + struct PCL_EXPORTS DeviceContext + { + DeviceContext (const xn::NodeInfo& device_node, xn::NodeInfo* image_node, xn::NodeInfo* depth_node, xn::NodeInfo * ir_node); + DeviceContext (const xn::NodeInfo & device_node); + DeviceContext (const DeviceContext&); + xn::NodeInfo device_node; + std::shared_ptr image_node; + std::shared_ptr depth_node; + std::shared_ptr ir_node; + boost::weak_ptr device; + } ; + + OpenNIDriver (); + OpenNIDevice::Ptr getDevice (unsigned index) const; + +#ifndef _WIN32 + // workaround to get additional device nformation like serial number, vendor and product name, until Primesense fix this + void getDeviceInfos () noexcept; +#endif + + mutable std::vector device_context_; + mutable xn::Context context_; + + std::map< unsigned char, std::map > bus_map_; + std::map< std::string, unsigned > serial_map_; + std::map< std::string, unsigned > connection_string_map_; + } ; + + OpenNIDriver& + OpenNIDriver::getInstance () + { + static OpenNIDriver driver; + return driver; + } + + unsigned + OpenNIDriver::getNumberDevices () const throw () + { + return static_cast (device_context_.size ()); + } +} // namespace +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_exception.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_exception.h new file mode 100755 index 0000000..dd65e3f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_exception.h @@ -0,0 +1,138 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#ifdef HAVE_OPENNI + +#include +#include +#include +#include +//#include <-- because current header is included in NVCC-compiled code and contains . Consider + + +//fom +#if defined _WIN32 && defined _MSC_VER && !defined __PRETTY_FUNCTION__ + #define __PRETTY_FUNCTION__ __FUNCTION__ +#endif + + +#define THROW_OPENNI_EXCEPTION(format,...) throwOpenNIException( __PRETTY_FUNCTION__, __FILE__, __LINE__, format , ##__VA_ARGS__ ) + + +namespace openni_wrapper +{ + + /** + * @brief General exception class + * @author Suat Gedikli + * @date 02.january 2011 + * @ingroup io + */ + class OpenNIException : public std::exception + { + public: + /** + * @brief Constructor + * @note use the MACRO THROW_OPENNI_EXCEPTION, that takes care about the parameters function_name, file_name and line_number + * @param[in] function_name the function in which this exception was created. + * @param[in] file_name the file name in which this exception was created. + * @param[in] line_number the line number where this exception was created. + * @param[in] message the message of the exception + */ + OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) noexcept; + + /** + * @brief virtual Destructor that never throws an exception + */ + ~OpenNIException () noexcept; + + /** + * @brief Assignment operator to allow copying the message of another exception variable. + * @param[in] exception left hand side + * @return + */ + OpenNIException & operator= (const OpenNIException& exception) noexcept; + + /** + * @brief virtual method, derived from std::exception + * @return the message of the exception. + */ + const char* what () const throw () override; + + /** + * @return the function name in which the exception was created. + */ + const std::string& getFunctionName () const throw (); + + /** + * @return the filename in which the exception was created. + */ + const std::string& getFileName () const throw (); + + /** + * @return the line number where the exception was created. + */ + unsigned getLineNumber () const throw (); + protected: + std::string function_name_; + std::string file_name_; + unsigned line_number_; + std::string message_; + std::string message_long_; + } ; + + /** + * @brief inline function used by the macro THROW_OPENNI_EXCEPTION to create an instance of OpenNIException with correct values for function, file and line_number + * @param[in] function_name the function name. Will be filled in by the macro THROW_OPENNI_EXCEPTION + * @param[in] file_name the file name. Will be filled in by the macro THROW_OPENNI_EXCEPTION + * @param[in] line_number the line number. Will be filled in by the macro THROW_OPENNI_EXCEPTION + * @param[in] format the printf-style format string + * @param[in] ... optional arguments for the printf style format. + */ + inline void + throwOpenNIException (const char* function_name, const char* file_name, unsigned line_number, const char* format, ...) + { + static char msg[1024]; + va_list args; + va_start (args, format); + vsprintf (msg, format, args); + throw OpenNIException (function_name, file_name, line_number, msg); + } +} // namespace openni_camera +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image.h new file mode 100755 index 0000000..df8c931 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image.h @@ -0,0 +1,209 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include +#include "openni.h" +#include "openni_exception.h" +#include + +namespace openni_wrapper +{ + + /** + * @brief Image class containing just a reference to image meta data. Thus this class + * just provides an interface to fill a RGB or Grayscale image buffer. + * @author Suat Gedikli + * @date 02.january 2011 + * @param[in] image_meta_data + * @ingroup io + */ + class PCL_EXPORTS Image + { + public: + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; + + enum Encoding + { + BAYER_GRBG, + YUV422, + RGB + }; + + /** + * @author Suat Gedikli + * @brief Constructor + * @param[in] image_meta_data the actual image data from the OpenNI driver + */ + inline Image (pcl::shared_ptr image_meta_data) noexcept; + + /** + * @author Suat Gedikli + * @brief virtual Destructor that never throws an exception. + */ + inline virtual ~Image () noexcept; + + /** + * @author Suat Gedikli + * @param[in] input_width width of input image + * @param[in] input_height height of input image + * @param[in] output_width width of desired output image + * @param[in] output_height height of desired output image + * @return whether the resizing is supported or not. + */ + virtual bool isResizingSupported (unsigned input_width, unsigned input_height, + unsigned output_width, unsigned output_height) const = 0; + + /** + * @author Suat Gedikli + * @brief fills a user given buffer with the RGB values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] rgb_buffer the output RGB buffer. + * @param[in] rgb_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, + unsigned rgb_line_step = 0) const = 0; + + /** + * @author Suat Gedikli + * @brief returns the encoding of the native data. + * @return encoding + */ + virtual Encoding getEncoding () const = 0; + + /** + * @author Suat Gedikli + * @brief fills a user given buffer with the raw values. + * @param[in,out] rgb_buffer + */ + inline void + fillRaw (unsigned char* rgb_buffer) const throw () + { + memcpy (rgb_buffer, image_md_->Data (), image_md_->DataSize ()); + } + + /** + * @author Suat Gedikli + * @brief fills a user given buffer with the gray values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] gray_buffer the output gray buffer. + * @param[in] gray_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, + unsigned gray_line_step = 0) const = 0; + + /** + * @author Suat Gedikli + * @return width of the image + */ + inline unsigned getWidth () const throw (); + + /** + * @author Suat Gedikli + * @return height of the image + */ + inline unsigned getHeight () const throw (); + + /** + * @author Suat Gedikli + * @return frame id of the image. + * @note frame ids are ascending, but not necessarily synch'ed with other streams + */ + inline unsigned getFrameID () const throw (); + + /** + * @author Suat Gedikli + * @return the time stamp of the image + * @note the time value is not synche'ed with the system time + */ + inline unsigned long getTimeStamp () const throw (); + + /** + * @author Suat Gedikli + * @return the actual data in native OpenNI format. + */ + inline const xn::ImageMetaData& getMetaData () const throw (); + + protected: + pcl::shared_ptr image_md_; + } ; + + Image::Image (pcl::shared_ptr image_meta_data) noexcept + : image_md_ (std::move(image_meta_data)) + { + } + + Image::~Image () noexcept { } + + unsigned + Image::getWidth () const throw () + { + return image_md_->XRes (); + } + + unsigned + Image::getHeight () const throw () + { + return image_md_->YRes (); + } + + unsigned + Image::getFrameID () const throw () + { + return image_md_->FrameID (); + } + + unsigned long + Image::getTimeStamp () const throw () + { + return static_cast (image_md_->Timestamp ()); + } + + const xn::ImageMetaData& + Image::getMetaData () const throw () + { + return *image_md_; + } +} // namespace +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_bayer_grbg.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_bayer_grbg.h new file mode 100755 index 0000000..ca6803a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_bayer_grbg.h @@ -0,0 +1,104 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include +#include "openni_image.h" + +namespace openni_wrapper +{ + /** \brief This class provides methods to fill a RGB or Grayscale image buffer from underlying Bayer pattern image. + * \author Suat Gedikli + * \ingroup io + */ + class PCL_EXPORTS ImageBayerGRBG : public Image + { + public: + enum DebayeringMethod + { + Bilinear = 0, + EdgeAware, + EdgeAwareWeighted + }; + + ImageBayerGRBG (pcl::shared_ptr image_meta_data, DebayeringMethod method) noexcept; + ~ImageBayerGRBG () noexcept; + + inline Encoding + getEncoding () const override + { + return (BAYER_GRBG); + } + + void fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; + void fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; + bool isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; + inline void setDebayeringMethod (const DebayeringMethod& method) noexcept; + inline DebayeringMethod getDebayeringMethod () const throw (); + inline static bool resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height); + + + protected: + DebayeringMethod debayering_method_; + }; + + void + ImageBayerGRBG::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& method) noexcept + { + debayering_method_ = method; + } + + ImageBayerGRBG::DebayeringMethod + ImageBayerGRBG::getDebayeringMethod () const throw () + { + return debayering_method_; + } + + bool + ImageBayerGRBG::resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) + { + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); + } +} // namespace + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_rgb24.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_rgb24.h new file mode 100755 index 0000000..0ee267a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_rgb24.h @@ -0,0 +1,82 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include "openni_image.h" +#include + +namespace openni_wrapper +{ + + /** + * @brief This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image. + * @author Suat Gedikli + * @date 19. June 2011 + * @ingroup io + */ + class PCL_EXPORTS ImageRGB24 : public Image + { + public: + + ImageRGB24 (pcl::shared_ptr image_meta_data) noexcept; + ~ImageRGB24 () noexcept; + + inline Encoding + getEncoding () const override + { + return (RGB); + } + + void fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; + void fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; + bool isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; + inline static bool resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height); + } ; + + bool + ImageRGB24::resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) + { + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); + } + +} // namespace openni_wrapper + +#endif // HAVE_OPENNI diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_yuv_422.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_yuv_422.h new file mode 100755 index 0000000..f7d5208 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_image_yuv_422.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_OPENNI + +#include +#include "openni_image.h" + +namespace openni_wrapper +{ + + /** + * @brief Concrete implementation of the interface Image for a YUV 422 image used by Primesense devices. + * @author Suat Gedikli + * @date 02.january 2011 + * @ingroup io + */ + class PCL_EXPORTS ImageYUV422 : public Image + { + public: + ImageYUV422 (pcl::shared_ptr image_meta_data) noexcept; + ~ImageYUV422 () noexcept; + + inline Encoding + getEncoding () const override + { + return (YUV422); + } + + bool isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; + void fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; + void fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; + inline static bool resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height); + } ; + + bool + ImageYUV422::resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) + { + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0); + } +} // namespace + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_ir_image.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_ir_image.h new file mode 100755 index 0000000..fac253d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_ir_image.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef __OPENNI_IR_IMAGE__ +#define __OPENNI_IR_IMAGE__ + +#include +#include +#include "openni.h" +#include "openni_exception.h" +#include + +namespace openni_wrapper +{ + +/** + * @brief Class containing just a reference to IR meta data. + * @author Patrick Mihelich , Suat Gedikli + */ +class PCL_EXPORTS IRImage +{ +public: + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; + + inline IRImage (pcl::shared_ptr ir_meta_data) noexcept; + inline virtual ~IRImage () noexcept; + + void fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step = 0) const; + + inline unsigned getWidth () const throw (); + inline unsigned getHeight () const throw (); + inline unsigned getFrameID () const throw (); + inline unsigned long getTimeStamp () const throw (); + inline const xn::IRMetaData& getMetaData () const throw (); + +protected: + pcl::shared_ptr ir_md_; +}; + +IRImage::IRImage (pcl::shared_ptr ir_meta_data) noexcept +: ir_md_ (std::move(ir_meta_data)) +{ +} + +IRImage::~IRImage () noexcept +{ +} + +unsigned IRImage::getWidth () const throw () +{ + return ir_md_->XRes (); +} + +unsigned IRImage::getHeight () const throw () +{ + return ir_md_->YRes (); +} + +unsigned IRImage::getFrameID () const throw () +{ + return ir_md_->FrameID (); +} + +unsigned long IRImage::getTimeStamp () const throw () +{ + return static_cast (ir_md_->Timestamp ()); +} + +const xn::IRMetaData& IRImage::getMetaData () const throw () +{ + return *ir_md_; +} +} // namespace +#endif //__OPENNI_IR_IMAGE__ diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_shift_to_depth_conversion.h b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_shift_to_depth_conversion.h new file mode 100755 index 0000000..bd8b275 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_camera/openni_shift_to_depth_conversion.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#ifdef HAVE_OPENNI + +#include +#include +#include + +namespace openni_wrapper +{ + /** \brief This class provides conversion of the openni 11-bit shift data to depth; + */ + class PCL_EXPORTS ShiftToDepthConverter + { + public: + /** \brief Constructor. */ + ShiftToDepthConverter () : init_(false) {} + + /** \brief Destructor. */ + virtual ~ShiftToDepthConverter () {}; + + /** \brief This method generates a look-up table to convert openni shift values to depth + */ + void + generateLookupTable () + { + // lookup of 11 bit shift values + constexpr std::size_t table_size = 1 << 10; + + lookupTable_.clear(); + lookupTable_.resize(table_size); + + // constants taken from openni driver + constexpr std::int16_t nConstShift = 800; + constexpr double nParamCoeff = 4.000000; + constexpr double dPlanePixelSize = 0.104200; + constexpr double nShiftScale = 10.000000; + constexpr double dPlaneDsr = 120.000000; + constexpr double dPlaneDcl = 7.500000; + + for (std::size_t i=0; i(i - nConstShift) / nParamCoeff)-0.375; + double dMetric = dFixedRefX * dPlanePixelSize; + lookupTable_[i] = static_cast((nShiftScale * ((dMetric * dPlaneDsr / (dPlaneDcl - dMetric)) + dPlaneDsr) ) / 1000.0f); + } + + init_ = true; + } + + /** \brief Generate a look-up table for converting openni shift values to depth + */ + inline float + shiftToDepth (std::uint16_t shift_val) + { + assert (init_); + + constexpr float bad_point = std::numeric_limits::quiet_NaN (); + + float ret = bad_point; + + // lookup depth value in shift lookup table + if (shift_val lookupTable_; + bool init_; + } ; +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/openni_grabber.h b/deps_install/include/pcl-1.10/pcl/io/openni_grabber.h new file mode 100755 index 0000000..bc16916 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/openni_grabber.h @@ -0,0 +1,503 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#ifdef HAVE_OPENNI + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + struct PointXYZ; + struct PointXYZRGB; + struct PointXYZRGBA; + struct PointXYZI; + + /** \brief Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) + * \author Nico Blodow , Suat Gedikli + * \ingroup io + */ + class PCL_EXPORTS OpenNIGrabber : public Grabber + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + enum Mode + { + OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz + OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect + OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect + OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion + OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion + OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect + OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion + OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN) + }; + + //define callback signature typedefs + using sig_cb_openni_image = void (const openni_wrapper::Image::Ptr &); + using sig_cb_openni_depth_image = void (const openni_wrapper::DepthImage::Ptr &); + using sig_cb_openni_ir_image = void (const openni_wrapper::IRImage::Ptr &); + using sig_cb_openni_image_depth_image = void (const openni_wrapper::Image::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) ; + using sig_cb_openni_ir_depth_image = void (const openni_wrapper::IRImage::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) ; + using sig_cb_openni_point_cloud = void (const pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_rgb = void (const pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_rgba = void (const pcl::PointCloud::ConstPtr &); + using sig_cb_openni_point_cloud_i = void (const pcl::PointCloud::ConstPtr &); + + public: + /** \brief Constructor + * \param[in] device_id ID of the device, which might be a serial number, bus\@address or the index of the device. + * \param[in] depth_mode the mode of the depth stream + * \param[in] image_mode the mode of the image stream + */ + OpenNIGrabber (const std::string& device_id = "", + const Mode& depth_mode = OpenNI_Default_Mode, + const Mode& image_mode = OpenNI_Default_Mode); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + ~OpenNIGrabber () noexcept; + + /** \brief Start the data acquisition. */ + void + start () override; + + /** \brief Stop the data acquisition. */ + void + stop () override; + + /** \brief Check if the data acquisition is still running. */ + bool + isRunning () const override; + + std::string + getName () const override; + + /** \brief Obtain the number of frames per second (FPS). */ + float + getFramesPerSecond () const override; + + /** \brief Get a boost shared pointer to the \ref pcl::openni_wrapper::OpenNIDevice object. */ + inline openni_wrapper::OpenNIDevice::Ptr + getDevice () const; + + /** \brief Obtain a list of the available depth modes that this device supports. */ + std::vector > + getAvailableDepthModes () const; + + /** \brief Obtain a list of the available image modes that this device supports. */ + std::vector > + getAvailableImageModes () const; + + /** \brief Set the RGB camera parameters (fx, fy, cx, cy) + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_length_y the RGB focal length (fy) + * \param[in] rgb_principal_point_x the RGB principal point (cx) + * \param[in] rgb_principal_point_y the RGB principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setRGBCameraIntrinsics (const double rgb_focal_length_x, + const double rgb_focal_length_y, + const double rgb_principal_point_x, + const double rgb_principal_point_y) + { + rgb_focal_length_x_ = rgb_focal_length_x; + rgb_focal_length_y_ = rgb_focal_length_y; + rgb_principal_point_x_ = rgb_principal_point_x; + rgb_principal_point_y_ = rgb_principal_point_y; + } + + /** \brief Get the RGB camera parameters (fx, fy, cx, cy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + * \param[out] rgb_principal_point_x the RGB principal point (cx) + * \param[out] rgb_principal_point_y the RGB principal point (cy) + */ + inline void + getRGBCameraIntrinsics (double &rgb_focal_length_x, + double &rgb_focal_length_y, + double &rgb_principal_point_x, + double &rgb_principal_point_y) const + { + rgb_focal_length_x = rgb_focal_length_x_; + rgb_focal_length_y = rgb_focal_length_y_; + rgb_principal_point_x = rgb_principal_point_x_; + rgb_principal_point_y = rgb_principal_point_y_; + } + + + /** \brief Set the RGB image focal length (fx = fy). + * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length) + { + rgb_focal_length_x_ = rgb_focal_length_y_ = rgb_focal_length; + } + + /** \brief Set the RGB image focal length + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_length_y the RGB focal length (fy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y) + { + rgb_focal_length_x_ = rgb_focal_length_x; + rgb_focal_length_y_ = rgb_focal_length_y; + } + + /** \brief Return the RGB focal length parameters (fx, fy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + */ + inline void + getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const + { + rgb_focal_length_x = rgb_focal_length_x_; + rgb_focal_length_y = rgb_focal_length_y_; + } + + /** \brief Set the Depth camera parameters (fx, fy, cx, cy) + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * \param[in] depth_principal_point_x the Depth principal point (cx) + * \param[in] depth_principal_point_y the Depth principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthCameraIntrinsics (const double depth_focal_length_x, + const double depth_focal_length_y, + const double depth_principal_point_x, + const double depth_principal_point_y) + { + depth_focal_length_x_ = depth_focal_length_x; + depth_focal_length_y_ = depth_focal_length_y; + depth_principal_point_x_ = depth_principal_point_x; + depth_principal_point_y_ = depth_principal_point_y; + } + + /** \brief Get the Depth camera parameters (fx, fy, cx, cy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + * \param[out] depth_principal_point_x the Depth principal point (cx) + * \param[out] depth_principal_point_y the Depth principal point (cy) + */ + inline void + getDepthCameraIntrinsics (double &depth_focal_length_x, + double &depth_focal_length_y, + double &depth_principal_point_x, + double &depth_principal_point_y) const + { + depth_focal_length_x = depth_focal_length_x_; + depth_focal_length_y = depth_focal_length_y_; + depth_principal_point_x = depth_principal_point_x_; + depth_principal_point_y = depth_principal_point_y_; + } + + /** \brief Set the Depth image focal length (fx = fy). + * \param[in] depth_focal_length the Depth focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length) + { + depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length; + } + + + /** \brief Set the Depth image focal length + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y) + { + depth_focal_length_x_ = depth_focal_length_x; + depth_focal_length_y_ = depth_focal_length_y; + } + + /** \brief Return the Depth focal length parameters (fx, fy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + */ + inline void + getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const + { + depth_focal_length_x = depth_focal_length_x_; + depth_focal_length_y = depth_focal_length_y_; + } + + /** \brief Convert vector of raw shift values to depth values + * \param[in] shift_data_ptr input shift data + * \param[out] depth_data_ptr generated depth data + * \param[in] size of shift and depth buffer + */ + inline void + convertShiftToDepth ( + const std::uint16_t* shift_data_ptr, + std::uint16_t* depth_data_ptr, + std::size_t size) const + { + // get openni device instance + auto openni_device = this->getDevice (); + + const std::uint16_t* shift_data_it = shift_data_ptr; + std::uint16_t* depth_data_it = depth_data_ptr; + + // shift-to-depth lookup + for (std::size_t i=0; ishiftToDepth(*shift_data_it); + + shift_data_it++; + depth_data_it++; + } + + } + + + protected: + /** \brief On initialization processing. */ + void + onInit (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); + + /** \brief Sets up an OpenNI device. */ + void + setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); + + /** \brief Update mode maps. */ + void + updateModeMaps (); + + /** \brief Start synchronization. */ + void + startSynchronization (); + + /** \brief Stop synchronization. */ + void + stopSynchronization (); + + /** \brief Map config modes. */ + bool + mapConfigMode2XnMode (int mode, XnMapOutputMode &xnmode) const; + + // callback methods + /** \brief RGB image callback. */ + virtual void + imageCallback (openni_wrapper::Image::Ptr image, void* cookie); + + /** \brief Depth image callback. */ + virtual void + depthCallback (openni_wrapper::DepthImage::Ptr depth_image, void* cookie); + + /** \brief IR image callback. */ + virtual void + irCallback (openni_wrapper::IRImage::Ptr ir_image, void* cookie); + + /** \brief RGB + Depth image callback. */ + virtual void + imageDepthImageCallback (const openni_wrapper::Image::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image); + + /** \brief IR + Depth image callback. */ + virtual void + irDepthImageCallback (const openni_wrapper::IRImage::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image); + + /** \brief Process changed signals. */ + void + signalsChanged () override; + + // helper methods + + /** \brief Check if the RGB and Depth images are required to be synchronized or not. */ + virtual void + checkImageAndDepthSynchronizationRequired (); + + /** \brief Check if the RGB image stream is required or not. */ + virtual void + checkImageStreamRequired (); + + /** \brief Check if the depth stream is required or not. */ + virtual void + checkDepthStreamRequired (); + + /** \brief Check if the IR image stream is required or not. */ + virtual void + checkIRStreamRequired (); + + + /** \brief Convert a Depth image to a pcl::PointCloud + * \param[in] depth the depth image to convert + */ + pcl::PointCloud::Ptr + convertToXYZPointCloud (const openni_wrapper::DepthImage::Ptr &depth) const; + + /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud + * \param[in] image the RGB image to convert + * \param[in] depth_image the depth image to convert + */ + template typename pcl::PointCloud::Ptr + convertToXYZRGBPointCloud (const openni_wrapper::Image::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image) const; + + /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud + * \param[in] image the IR image to convert + * \param[in] depth_image the depth image to convert + */ + pcl::PointCloud::Ptr + convertToXYZIPointCloud (const openni_wrapper::IRImage::Ptr &image, + const openni_wrapper::DepthImage::Ptr &depth_image) const; + + + Synchronizer rgb_sync_; + Synchronizer ir_sync_; + + /** \brief The actual openni device. */ + openni_wrapper::OpenNIDevice::Ptr device_; + + std::string rgb_frame_id_; + std::string depth_frame_id_; + unsigned image_width_; + unsigned image_height_; + unsigned depth_width_; + unsigned depth_height_; + + bool image_required_; + bool depth_required_; + bool ir_required_; + bool sync_required_; + + boost::signals2::signal* image_signal_; + boost::signals2::signal* depth_image_signal_; + boost::signals2::signal* ir_image_signal_; + boost::signals2::signal* image_depth_image_signal_; + boost::signals2::signal* ir_depth_image_signal_; + boost::signals2::signal* point_cloud_signal_; + boost::signals2::signal* point_cloud_i_signal_; + boost::signals2::signal* point_cloud_rgb_signal_; + boost::signals2::signal* point_cloud_rgba_signal_; + + struct modeComp + { + + bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode & mode2) const + { + if (mode1.nXRes < mode2.nXRes) + return true; + if (mode1.nXRes > mode2.nXRes) + return false; + if (mode1.nYRes < mode2.nYRes) + return true; + if (mode1.nYRes > mode2.nYRes) + return false; + return (mode1.nFPS < mode2.nFPS); + } + } ; + std::map config2xn_map_; + + openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle; + openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle; + openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle; + bool running_; + + mutable unsigned rgb_array_size_; + mutable unsigned depth_buffer_size_; + mutable boost::shared_array rgb_array_; + mutable boost::shared_array depth_buffer_; + mutable boost::shared_array ir_buffer_; + + /** \brief The RGB image focal length (fx). */ + double rgb_focal_length_x_; + /** \brief The RGB image focal length (fy). */ + double rgb_focal_length_y_; + /** \brief The RGB image principal point (cx). */ + double rgb_principal_point_x_; + /** \brief The RGB image principal point (cy). */ + double rgb_principal_point_y_; + /** \brief The depth image focal length (fx). */ + double depth_focal_length_x_; + /** \brief The depth image focal length (fy). */ + double depth_focal_length_y_; + /** \brief The depth image principal point (cx). */ + double depth_principal_point_x_; + /** \brief The depth image principal point (cy). */ + double depth_principal_point_y_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + openni_wrapper::OpenNIDevice::Ptr + OpenNIGrabber::getDevice () const + { + return device_; + } + +} // namespace pcl +#endif // HAVE_OPENNI diff --git a/deps_install/include/pcl-1.10/pcl/io/pcd_grabber.h b/deps_install/include/pcl-1.10/pcl/io/pcd_grabber.h new file mode 100755 index 0000000..de546da --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/pcd_grabber.h @@ -0,0 +1,313 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#ifdef HAVE_OPENNI +#include +#include +#include +#endif + +#include +#include + +namespace pcl +{ + /** \brief Base class for PCD file grabber. + * \ingroup io + */ + class PCL_EXPORTS PCDGrabberBase : public Grabber + { + public: + /** \brief Constructor taking just one PCD file or one TAR file containing multiple PCD files. + * \param[in] pcd_file path to the PCD file + * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. + * \param[in] repeat whether to play PCD file in an endless loop or not. + */ + PCDGrabberBase (const std::string& pcd_file, float frames_per_second, bool repeat); + + /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list. + * \param[in] pcd_files vector of paths to PCD files. + * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. + * \param[in] repeat whether to play PCD file in an endless loop or not. + */ + PCDGrabberBase (const std::vector& pcd_files, float frames_per_second, bool repeat); + + /** \brief Copy constructor. + * \param[in] src the PCD Grabber base object to copy into this + */ + PCDGrabberBase (const PCDGrabberBase &src) : impl_ () + { + *this = src; + } + + /** \brief Copy operator. + * \param[in] src the PCD Grabber base object to copy into this + */ + PCDGrabberBase& + operator = (const PCDGrabberBase &src) + { + impl_ = src.impl_; + return (*this); + } + + /** \brief Virtual destructor. */ + ~PCDGrabberBase () noexcept; + + /** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */ + void + start () override; + + /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */ + void + stop () override; + + /** \brief Triggers a callback with new data */ + virtual void + trigger (); + + /** \brief Indicates whether the grabber is streaming or not. + * \return true if grabber is started and hasn't run out of PCD files. + */ + bool + isRunning () const override; + + /** \return The name of the grabber */ + std::string + getName () const override; + + /** \brief Rewinds to the first PCD file in the list.*/ + virtual void + rewind (); + + /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */ + float + getFramesPerSecond () const override; + + /** \brief Returns whether the repeat flag is on */ + bool + isRepeatOn () const; + + /** \brief Get cloud (in ROS form) at a particular location */ + bool + getCloudAt (std::size_t idx, + pcl::PCLPointCloud2 &blob, + Eigen::Vector4f &origin, + Eigen::Quaternionf &orientation) const; + + /** \brief Returns the size */ + std::size_t + numFrames () const; + + private: + virtual void + publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const = 0; + + // to separate and hide the implementation from interface: PIMPL + struct PCDGrabberImpl; + PCDGrabberImpl* impl_; + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template class PointCloud; + template class PCDGrabber : public PCDGrabberBase, public FileGrabber + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + PCDGrabber (const std::string& pcd_path, float frames_per_second = 0, bool repeat = false); + PCDGrabber (const std::vector& pcd_files, float frames_per_second = 0, bool repeat = false); + + /** \brief Virtual destructor. */ + ~PCDGrabber () noexcept + { + stop (); + } + + // Inherited from FileGrabber + const typename pcl::PointCloud::ConstPtr + operator[] (std::size_t idx) const override; + + // Inherited from FileGrabber + std::size_t + size () const override; + protected: + + void + publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const override; + + boost::signals2::signal::ConstPtr&)>* signal_; + boost::signals2::signal* file_name_signal_; + +#ifdef HAVE_OPENNI + boost::signals2::signal* depth_image_signal_; + boost::signals2::signal* image_signal_; + boost::signals2::signal* image_depth_image_signal_; +#endif + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + PCDGrabber::PCDGrabber (const std::string& pcd_path, float frames_per_second, bool repeat) + : PCDGrabberBase (pcd_path, frames_per_second, repeat) + { + signal_ = createSignal::ConstPtr&)>(); + file_name_signal_ = createSignal(); +#ifdef HAVE_OPENNI + depth_image_signal_ = createSignal (); + image_signal_ = createSignal (); + image_depth_image_signal_ = createSignal (); +#endif + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template + PCDGrabber::PCDGrabber (const std::vector& pcd_files, float frames_per_second, bool repeat) + : PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ () + { + signal_ = createSignal::ConstPtr&)>(); + file_name_signal_ = createSignal(); +#ifdef HAVE_OPENNI + depth_image_signal_ = createSignal (); + image_signal_ = createSignal (); + image_depth_image_signal_ = createSignal (); +#endif + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template const typename pcl::PointCloud::ConstPtr + PCDGrabber::operator[] (std::size_t idx) const + { + pcl::PCLPointCloud2 blob; + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + getCloudAt (idx, blob, origin, orientation); + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::fromPCLPointCloud2 (blob, *cloud); + cloud->sensor_origin_ = origin; + cloud->sensor_orientation_ = orientation; + return (cloud); + } + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template std::size_t + PCDGrabber::size () const + { + return (numFrames ()); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void + PCDGrabber::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const + { + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::fromPCLPointCloud2 (blob, *cloud); + cloud->sensor_origin_ = origin; + cloud->sensor_orientation_ = orientation; + + signal_->operator () (cloud); + if (file_name_signal_->num_slots() > 0) + file_name_signal_->operator()(file_name); + +#ifdef HAVE_OPENNI + // If dataset is not organized, return + if (!cloud->isOrganized ()) + return; + + shared_ptr depth_meta_data (new xn::DepthMetaData); + depth_meta_data->AllocateData (cloud->width, cloud->height); + XnDepthPixel* depth_map = depth_meta_data->WritableData (); + std::uint32_t k = 0; + for (std::uint32_t i = 0; i < cloud->height; ++i) + for (std::uint32_t j = 0; j < cloud->width; ++j) + { + depth_map[k] = static_cast ((*cloud)[k].z * 1000); + ++k; + } + + openni_wrapper::DepthImage::Ptr depth_image (new openni_wrapper::DepthImage (depth_meta_data, 0.075f, 525, 0, 0)); + if (depth_image_signal_->num_slots() > 0) + depth_image_signal_->operator()(depth_image); + + // ---[ RGB special case + std::vector fields; + int rgba_index = pcl::getFieldIndex ("rgb", fields); + if (rgba_index == -1) + rgba_index = pcl::getFieldIndex ("rgba", fields); + if (rgba_index >= 0) + { + rgba_index = fields[rgba_index].offset; + + shared_ptr image_meta_data (new xn::ImageMetaData); + image_meta_data->AllocateData (cloud->width, cloud->height, XN_PIXEL_FORMAT_RGB24); + XnRGB24Pixel* image_map = image_meta_data->WritableRGB24Data (); + k = 0; + for (std::uint32_t i = 0; i < cloud->height; ++i) + { + for (std::uint32_t j = 0; j < cloud->width; ++j) + { + // Fill r/g/b data, assuming that the order is BGRA + pcl::RGB rgb; + memcpy (&rgb, reinterpret_cast (&cloud->points[k]) + rgba_index, sizeof (RGB)); + image_map[k].nRed = static_cast (rgb.r); + image_map[k].nGreen = static_cast (rgb.g); + image_map[k].nBlue = static_cast (rgb.b); + ++k; + } + } + + openni_wrapper::Image::Ptr image (new openni_wrapper::ImageRGB24 (image_meta_data)); + if (image_signal_->num_slots() > 0) + image_signal_->operator()(image); + + if (image_depth_image_signal_->num_slots() > 0) + image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f); + } +#endif + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/pcd_io.h b/deps_install/include/pcl-1.10/pcl/io/pcd_io.h new file mode 100755 index 0000000..881542b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/pcd_io.h @@ -0,0 +1,788 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Point Cloud Data (PCD) file format reader. + * \author Radu B. Rusu + * \ingroup io + */ + class PCL_EXPORTS PCDReader : public FileReader + { + public: + /** Empty constructor */ + PCDReader () {} + /** Empty destructor */ + ~PCDReader () {} + + /** \brief Various PCD file versions. + * + * PCD_V6 represents PCD files with version 0.6, which contain the following fields: + * - lines beginning with # are treated as comments + * - FIELDS ... + * - SIZE ... + * - TYPE ... + * - COUNT ... + * - WIDTH ... + * - HEIGHT ... + * - POINTS ... + * - DATA ascii/binary + * + * Everything that follows \b DATA is interpreted as data points and + * will be read accordingly. + * + * PCD_V7 represents PCD files with version 0.7 and has an important + * addon: it adds sensor origin/orientation (aka viewpoint) information + * to a dataset through the use of a new header field: + * - VIEWPOINT tx ty tz qw qx qy qz + */ + enum + { + PCD_V6 = 0, + PCD_V7 = 1 + }; + + /** \brief Read a point cloud data header from a PCD-formatted, binary istream. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given PCD stream. Useful for fast + * evaluation of the underlying data structure. + * + * \attention The PCD data is \b always stored in ROW major format! The + * read/write PCD methods will detect column major input and automatically convert it. + * + * \param[in] binary_istream a std::istream with openmode set to std::ios::binary. + * \param[out] cloud the resultant point cloud dataset (only these + * members will be filled: width, height, point_step, + * row_step, fields[]; data is resized but not written) + * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present) + * \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7) + * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) + * \param[out] data_idx the offset of cloud data within the file + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readHeader (std::istream &binary_istream, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, + int &data_type, unsigned int &data_idx); + + /** \brief Read a point cloud data header from a PCD file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given PCD file. Useful for fast + * evaluation of the underlying data structure. + * + * \attention The PCD data is \b always stored in ROW major format! The + * read/write PCD methods will detect column major input and automatically convert it. + * + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only these + * members will be filled: width, height, point_step, + * row_step, fields[]; data is resized but not written) + * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present) + * \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7) + * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) + * \param[out] data_idx the offset of cloud data within the file + * \param[in] offset the offset of where to expect the PCD Header in the + * file (optional parameter). One usage example for setting the offset + * parameter is for reading data from a TAR "archive containing multiple + * PCD files: TAR files always add a 512 byte header in front of the + * actual file, so set the offset to the next byte after the header + * (e.g., 513). + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, + int &data_type, unsigned int &data_idx, const int offset = 0) override; + + + /** \brief Read a point cloud data header from a PCD file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given PCD file. Useful for fast + * evaluation of the underlying data structure. + * + * \attention The PCD data is \b always stored in ROW major format! The + * read/write PCD methods will detect column major input and automatically convert it. + * + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only these + * members will be filled: width, height, point_step, + * row_step, fields[]; data is resized but not written) + * \param[in] offset the offset of where to expect the PCD Header in the + * file (optional parameter). One usage example for setting the offset + * parameter is for reading data from a TAR "archive containing multiple + * PCD files: TAR files always add a 512 byte header in front of the + * actual file, so set the offset to the next byte after the header + * (e.g., 513). + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0); + + /** \brief Read the point cloud data (body) from a PCD stream. + * + * Reads the cloud points from a text-formatted stream. For use after + * readHeader(), when the resulting data_type == 0. + * + * \attention This assumes the stream has been seeked to the position + * indicated by the data_idx result of readHeader(). + * + * \param[in] stream the stream from which to read the body. + * \param[out] cloud the resultant point cloud dataset to be filled. + * \param[in] pcd_version the PCD version of the stream (from readHeader()). + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readBodyASCII (std::istream &stream, pcl::PCLPointCloud2 &cloud, int pcd_version); + + /** \brief Read the point cloud data (body) from a block of memory. + * + * Reads the cloud points from a binary-formatted memory block. For use + * after readHeader(), when the resulting data_type is nonzero. + * + * \param[in] data the memory location from which to read the body. + * \param[out] cloud the resultant point cloud dataset to be filled. + * \param[in] pcd_version the PCD version of the stream (from readHeader()). + * \param[in] compressed indicates whether the PCD block contains compressed + * data. This should be true if the data_type returne by readHeader() == 2. + * \param[in] data_idx the offset of the body, as reported by readHeader(). + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + readBodyBinary (const unsigned char *data, pcl::PCLPointCloud2 &cloud, + int pcd_version, bool compressed, unsigned int data_idx); + + /** \brief Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present) + * \param[out] pcd_version the PCD version of the file (either PCD_V6 or PCD_V7) + * \param[in] offset the offset of where to expect the PCD Header in the + * file (optional parameter). One usage example for setting the offset + * parameter is for reading data from a TAR "archive containing multiple + * PCD files: TAR files always add a 512 byte header in front of the + * actual file, so set the offset to the next byte after the header + * (e.g., 513). + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset = 0) override; + + /** \brief Read a point cloud data from a PCD (PCD_V6) and store it into a pcl/PCLPointCloud2. + * + * \note This function is provided for backwards compatibility only and + * it can only read PCD_V6 files correctly, as pcl::PCLPointCloud2 + * does not contain a sensor origin/orientation. Reading any file + * > PCD_V6 will generate a warning. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset of where to expect the PCD Header in the + * file (optional parameter). One usage example for setting the offset + * parameter is for reading data from a TAR "archive containing multiple + * PCD files: TAR files always add a 512 byte header in front of the + * actual file, so set the offset to the next byte after the header + * (e.g., 513). + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0); + + /** \brief Read a point cloud data from any PCD file, and convert it to the given template format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset of where to expect the PCD Header in the + * file (optional parameter). One usage example for setting the offset + * parameter is for reading data from a TAR "archive containing multiple + * PCD files: TAR files always add a 512 byte header in front of the + * actual file, so set the offset to the next byte after the header + * (e.g., 513). + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + template int + read (const std::string &file_name, pcl::PointCloud &cloud, const int offset = 0) + { + pcl::PCLPointCloud2 blob; + int pcd_version; + int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, + pcd_version, offset); + + // If no error, convert the data + if (res == 0) + pcl::fromPCLPointCloud2 (blob, cloud); + return (res); + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief Point Cloud Data (PCD) file format writer. + * \author Radu Bogdan Rusu + * \ingroup io + */ + class PCL_EXPORTS PCDWriter : public FileWriter + { + public: + PCDWriter() : map_synchronization_(false) {} + ~PCDWriter() {} + + /** \brief Set whether mmap() synchornization via msync() is desired before munmap() calls. + * Setting this to true could prevent NFS data loss (see + * http://www.pcl-developers.org/PCD-IO-consistency-on-NFS-msync-needed-td4885942.html). + * Default: false + * \note This option should be used by advanced users only! + * \note Please note that using msync() on certain systems can reduce the I/O performance by up to 80%! + * \param[in] sync set to true if msync() should be called before munmap() + */ + void + setMapSynchronization (bool sync) + { + map_synchronization_ = sync; + } + + /** \brief Generate the header of a PCD file format + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + */ + std::string + generateHeaderBinary (const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation); + + /** \brief Generate the header of a BINARY_COMPRESSED PCD file format + * \param[out] os the stream into which to write the header + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * + * \return + * * < 0 (-1) on error + * * == 0 on success + */ + int + generateHeaderBinaryCompressed (std::ostream &os, + const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation); + + /** \brief Generate the header of a BINARY_COMPRESSED PCD file format + * \param[out] os the stream into which to write the header + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + */ + std::string + generateHeaderBinaryCompressed (const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation); + + /** \brief Generate the header of a PCD file format + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + */ + std::string + generateHeaderASCII (const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation); + + /** \brief Generate the header of a PCD file format + * \param[in] cloud the point cloud data message + * \param[in] nr_points if given, use this to fill in WIDTH, HEIGHT (=1), and POINTS in the header + * By default, nr_points is set to INTMAX, and the data in the header is used instead. + */ + template static std::string + generateHeader (const pcl::PointCloud &cloud, + const int nr_points = std::numeric_limits::max ()); + + /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \param[in] precision the specified output numeric stream precision (default: 8) + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + * + * As an intermediary solution, precision 8 is used, which guarantees lossless storage for RGB. + */ + int + writeASCII (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const int precision = 8); + + /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + */ + int + writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); + + /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \return + * (-1) for a general error + * (-2) if the input cloud is too large for the file format + * 0 on success + */ + int + writeBinaryCompressed (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); + + /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY_COMPRESSED format + * \param[out] os the stream into which to write the data + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \return + * (-1) for a general error + * (-2) if the input cloud is too large for the file format + * 0 on success + */ + int + writeBinaryCompressed (std::ostream &os, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); + + /** \brief Save point cloud data to a PCD file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \param[in] binary set to true if the file is to be written in a binary + * PCD format, false (default) for ASCII + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + * + * As an intermediary solution, precision 8 is used, which guarantees lossless storage for RGB. + */ + inline int + write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary = false) override + { + if (binary) + return (writeBinary (file_name, cloud, origin, orientation)); + return (writeASCII (file_name, cloud, origin, orientation, 8)); + } + + /** \brief Save point cloud data to a PCD file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message (boost shared pointer) + * \param[in] binary set to true if the file is to be written in a binary PCD format, + * false (default) for ASCII + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + */ + inline int + write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary = false) + { + return (write (file_name, *cloud, origin, orientation, binary)); + } + + /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + */ + template int + writeBinary (const std::string &file_name, + const pcl::PointCloud &cloud); + + /** \brief Save point cloud data to a binary comprssed PCD file + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \return + * (-1) for a general error + * (-2) if the input cloud is too large for the file format + * 0 on success + */ + template int + writeBinaryCompressed (const std::string &file_name, + const pcl::PointCloud &cloud); + + /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] indices the set of point indices that we want written to disk + */ + template int + writeBinary (const std::string &file_name, + const pcl::PointCloud &cloud, + const std::vector &indices); + + /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] precision the specified output numeric stream precision (default: 8) + */ + template int + writeASCII (const std::string &file_name, + const pcl::PointCloud &cloud, + const int precision = 8); + + /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] indices the set of point indices that we want written to disk + * \param[in] precision the specified output numeric stream precision (default: 8) + */ + template int + writeASCII (const std::string &file_name, + const pcl::PointCloud &cloud, + const std::vector &indices, + const int precision = 8); + + /** \brief Save point cloud data to a PCD file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the pcl::PointCloud data + * \param[in] binary set to true if the file is to be written in a binary + * PCD format, false (default) for ASCII + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + */ + template inline int + write (const std::string &file_name, + const pcl::PointCloud &cloud, + const bool binary = false) + { + if (binary) + return (writeBinary (file_name, cloud)); + return (writeASCII (file_name, cloud)); + } + + /** \brief Save point cloud data to a PCD file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the pcl::PointCloud data + * \param[in] indices the set of point indices that we want written to disk + * \param[in] binary set to true if the file is to be written in a binary + * PCD format, false (default) for ASCII + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + */ + template inline int + write (const std::string &file_name, + const pcl::PointCloud &cloud, + const std::vector &indices, + bool binary = false) + { + if (binary) + return (writeBinary (file_name, cloud, indices)); + return (writeASCII (file_name, cloud, indices)); + } + + protected: + /** \brief Set permissions for file locking (Boost 1.49+). + * \param[in] file_name the file name to set permission for file locking + * \param[in,out] lock the file lock + */ + void + setLockingPermissions (const std::string &file_name, + boost::interprocess::file_lock &lock); + + /** \brief Reset permissions for file locking (Boost 1.49+). + * \param[in] file_name the file name to reset permission for file locking + * \param[in,out] lock the file lock + */ + void + resetLockingPermissions (const std::string &file_name, + boost::interprocess::file_lock &lock); + + private: + /** \brief Set to true if msync() should be called before munmap(). Prevents data loss on NFS systems. */ + bool map_synchronization_; + }; + + namespace io + { + /** \brief Load a PCD v.6 file into a templated PointCloud type. + * + * Any PCD files > v.6 will generate a warning as a + * pcl/PCLPointCloud2 message cannot hold the sensor origin. + * + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + inline int + loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) + { + pcl::PCDReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any PCD file into a templated PointCloud type. + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present) + * \param[out] orientation the sensor acquisition orientation (only for > + * PCD_V7 - identity if not present) + * \ingroup io + */ + inline int + loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) + { + pcl::PCDReader p; + int pcd_version; + return (p.read (file_name, cloud, origin, orientation, pcd_version)); + } + + /** \brief Load any PCD file into a templated PointCloud type + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant templated point cloud + * \ingroup io + */ + template inline int + loadPCDFile (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::PCDReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Save point cloud data to a PCD file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \param[in] binary_mode true for binary mode, false (default) for ASCII + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + * \ingroup io + */ + inline int + savePCDFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary_mode = false) + { + PCDWriter w; + return (w.write (file_name, cloud, origin, orientation, binary_mode)); + } + + /** \brief Templated version for saving point cloud data to a PCD file + * containing a specific given cloud format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] binary_mode true for binary mode, false (default) for ASCII + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + * \ingroup io + */ + template inline int + savePCDFile (const std::string &file_name, const pcl::PointCloud &cloud, bool binary_mode = false) + { + PCDWriter w; + return (w.write (file_name, cloud, binary_mode)); + } + + /** + * \brief Templated version for saving point cloud data to a PCD file + * containing a specific given cloud format. + * + * This version is to retain backwards compatibility. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + * \ingroup io + */ + template inline int + savePCDFileASCII (const std::string &file_name, const pcl::PointCloud &cloud) + { + PCDWriter w; + return (w.write (file_name, cloud, false)); + } + + /** + * \brief Templated version for saving point cloud data to a PCD file + * containing a specific given cloud format. The resulting file will be an uncompressed binary. + * + * This version is to retain backwards compatibility. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \ingroup io + */ + template inline int + savePCDFileBinary (const std::string &file_name, const pcl::PointCloud &cloud) + { + PCDWriter w; + return (w.write (file_name, cloud, true)); + } + + /** + * \brief Templated version for saving point cloud data to a PCD file + * containing a specific given cloud format + * + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] indices the set of indices to save + * \param[in] binary_mode true for binary mode, false (default) for ASCII + * + * Caution: PointCloud structures containing an RGB field have + * traditionally used packed float values to store RGB data. Storing a + * float as ASCII can introduce variations to the smallest bits, and + * thus significantly alter the data. This is a known issue, and the fix + * involves switching RGB data to be stored as a packed integer in + * future versions of PCL. + * \ingroup io + */ + template int + savePCDFile (const std::string &file_name, + const pcl::PointCloud &cloud, + const std::vector &indices, + const bool binary_mode = false) + { + // Save the data + PCDWriter w; + return (w.write (file_name, cloud, indices, binary_mode)); + } + + + /** + * \brief Templated version for saving point cloud data to a PCD file + * containing a specific given cloud format. This method will write a compressed binary file. + * + * This version is to retain backwards compatibility. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \ingroup io + */ + template inline int + savePCDFileBinaryCompressed (const std::string &file_name, const pcl::PointCloud &cloud) + { + PCDWriter w; + return (w.writeBinaryCompressed (file_name, cloud)); + } + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/io/ply/byte_order.h b/deps_install/include/pcl-1.10/pcl/io/ply/byte_order.h new file mode 100755 index 0000000..4632547 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/ply/byte_order.h @@ -0,0 +1,109 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2007-2012, Ares Lagae + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl +{ + namespace io + { + namespace ply + { + /** \file byte_order.h + * defines byte shift operations and endianness. + * \author Ares Lagae as part of libply, Nizar Sallem + * \ingroup io + */ + + enum byte_order + { + little_endian_byte_order, + big_endian_byte_order, +#if BOOST_ENDIAN_BIG_BYTE + host_byte_order = big_endian_byte_order, +#elif BOOST_ENDIAN_LITTLE_BYTE + host_byte_order = little_endian_byte_order, +#else +#error "unable to determine system endianness" +#endif + network_byte_order = big_endian_byte_order + }; + + template + void swap_byte_order (char* bytes); + + template <> + inline void swap_byte_order<1> (char*) {} + + template <> + inline void swap_byte_order<2> (char* bytes) + { + std::swap (bytes[0], bytes[1]); + } + + template <> + inline void swap_byte_order<4> (char* bytes) + { + std::swap (bytes[0], bytes[3]); + std::swap (bytes[1], bytes[2]); + } + + template <> + inline void swap_byte_order<8> (char* bytes) + { + std::swap (bytes[0], bytes[7]); + std::swap (bytes[1], bytes[6]); + std::swap (bytes[2], bytes[5]); + std::swap (bytes[3], bytes[4]); + } + + template + void swap_byte_order (T& value) + { + swap_byte_order (reinterpret_cast (&value)); + } + + } // namespace ply + } // namespace io +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/io/ply/io_operators.h b/deps_install/include/pcl-1.10/pcl/io/ply/io_operators.h new file mode 100755 index 0000000..31a0fb0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/ply/io_operators.h @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2007-2012, Ares Lagae + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace io + { + namespace ply + { + /** \file io_operators.h + * defines output operators for int8 and uint8 + * \author Ares Lagae as part of libply, Nizar Sallem + * \ingroup io + */ + namespace io_operators + { + + inline std::istream& operator>> (std::istream& istream, int8 &value) + { + int16 tmp; + if (istream >> tmp) + { + if (tmp <= std::numeric_limits::max ()) + value = static_cast (tmp); + else + istream.setstate (std::ios_base::failbit); + } + return (istream); + } + + inline std::istream& operator>> (std::istream& istream, uint8 &value) + { + uint16 tmp; + if (istream >> tmp) + { + if (tmp <= std::numeric_limits::max ()) + value = static_cast (tmp); + else + istream.setstate (std::ios_base::failbit); + } + return (istream); + } + + inline std::ostream& operator<<(std::ostream& ostream, int8 value) + { + return (ostream << static_cast (value)); + } + + inline std::ostream& operator<<(std::ostream& ostream, uint8 value) + { + return (ostream << static_cast (value)); + } + + } // namespace io_operators + } // namespace ply + } // namespace io +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/io/ply/ply.h b/deps_install/include/pcl-1.10/pcl/io/ply/ply.h new file mode 100755 index 0000000..3016031 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/ply/ply.h @@ -0,0 +1,101 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2007-2012, Ares Lagae + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +/** \file ply.h contains standard typedefs and generic type traits + * \author Ares Lagae as part of libply, Nizar Sallem + * Ported with agreement from the author under the terms of the BSD + * license. + * \ingroup io + */ +namespace pcl +{ + namespace io + { + namespace ply + { + using int8 = std::int8_t; + using int16 = std::int16_t; + using int32 = std::int32_t; + using uint8 = std::uint8_t; + using uint16 = std::uint16_t; + using uint32 = std::uint32_t; + + using float32 = float; + using float64 = double; + + template + struct type_traits; + +#ifdef PLY_TYPE_TRAITS +# error +#endif + +#define PLY_TYPE_TRAITS(TYPE, PARSE_TYPE, NAME, OLD_NAME) \ + template <> \ + struct type_traits \ + { \ + using type = TYPE; \ + using parse_type = PARSE_TYPE; \ + static const char* name () { return NAME; } \ + static const char* old_name () { return OLD_NAME; } \ + }; + + PLY_TYPE_TRAITS(int8, int16, "int8", "char") + PLY_TYPE_TRAITS(int16, int16, "int16", "short") + PLY_TYPE_TRAITS(int32, int32, "int32", "int") + PLY_TYPE_TRAITS(uint8, uint16, "uint8", "uchar") + PLY_TYPE_TRAITS(uint16, uint16, "uint16", "ushort") + PLY_TYPE_TRAITS(uint32, uint32, "uint32", "uint") + PLY_TYPE_TRAITS(float32, float32, "float32", "float") + PLY_TYPE_TRAITS(float64, float64, "float64", "double") + + +#undef PLY_TYPE_TRAITS + + using format_type = int; + enum format { ascii_format, binary_little_endian_format, binary_big_endian_format, unknown }; + } // namespace ply + } // namespace io +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/io/ply/ply_parser.h b/deps_install/include/pcl-1.10/pcl/io/ply/ply_parser.h new file mode 100755 index 0000000..a689633 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/ply/ply_parser.h @@ -0,0 +1,718 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2007-2012, Ares Lagae + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifdef BUILD_Maintainer +# if defined __GNUC__ +# pragma GCC system_header +# elif defined _MSC_VER +# pragma warning(push, 1) +# endif +#endif + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace io + { + namespace ply + { + /** Class ply_parser parses a PLY file and generates appropriate atomic + * parsers for the body. + * \author Ares Lagae as part of libply, Nizar Sallem + * Ported with agreement from the author under the terms of the BSD + * license. + */ + class PCL_EXPORTS ply_parser + { + public: + + using info_callback_type = std::function; + using warning_callback_type = std::function; + using error_callback_type = std::function; + + using magic_callback_type = std::function; + using format_callback_type = std::function; + using comment_callback_type = std::function; + using obj_info_callback_type = std::function; + using end_header_callback_type = std::function; + + using begin_element_callback_type = std::function; + using end_element_callback_type = std::function; + using element_callbacks_type = std::tuple; + using element_definition_callback_type = std::function; + + template + struct scalar_property_callback_type + { + using type = std::function; + }; + + template + struct scalar_property_definition_callback_type + { + using scalar_property_callback_type = typename scalar_property_callback_type::type; + using type = std::function; + }; + + using scalar_types = boost::mpl::vector; + + class scalar_property_definition_callbacks_type + { + private: + template + struct callbacks_element + { +// callbacks_element () : callback (); + using scalar_type = T; + typename scalar_property_definition_callback_type::type callback; + }; + + using callbacks = boost::mpl::inherit_linearly< + scalar_types, + boost::mpl::inherit< + boost::mpl::_1, + callbacks_element + > + >::type; + callbacks callbacks_; + + public: + template + const typename scalar_property_definition_callback_type::type& + get () const + { + return (static_cast&> (callbacks_).callback); + } + + template + typename scalar_property_definition_callback_type::type& + get () + { + return (static_cast&> (callbacks_).callback); + } + + template + friend typename scalar_property_definition_callback_type::type& + at (scalar_property_definition_callbacks_type& scalar_property_definition_callbacks); + + template + friend const typename scalar_property_definition_callback_type::type& + at (const scalar_property_definition_callbacks_type& scalar_property_definition_callbacks); + }; + + template static + typename scalar_property_definition_callback_type::type& + at (scalar_property_definition_callbacks_type& scalar_property_definition_callbacks) + { + return (scalar_property_definition_callbacks.get ()); + } + + + template static + const typename scalar_property_definition_callback_type::type& + at (const scalar_property_definition_callbacks_type& scalar_property_definition_callbacks) + { + return (scalar_property_definition_callbacks.get ()); + } + + template + struct list_property_begin_callback_type + { + using type = std::function; + }; + + template + struct list_property_element_callback_type + { + using type = std::function; + }; + + template + struct list_property_end_callback_type + { + using type = std::function; + }; + + template + struct list_property_definition_callback_type + { + using list_property_begin_callback_type = typename list_property_begin_callback_type::type; + using list_property_element_callback_type = typename list_property_element_callback_type::type; + using list_property_end_callback_type = typename list_property_end_callback_type::type; + using type = std::function (const std::string&, const std::string&)>; + }; + + using size_types = boost::mpl::vector; + + class list_property_definition_callbacks_type + { + private: + template struct pair_with : boost::mpl::pair {}; + template + + struct sequence_product : + boost::mpl::fold, + boost::mpl::joint_view< + boost::mpl::_1,boost::mpl::transform > > > + {}; + + template + struct callbacks_element + { + using size_type = typename T::first; + using scalar_type = typename T::second; + typename list_property_definition_callback_type::type callback; + }; + + using callbacks = boost::mpl::inherit_linearly::type, boost::mpl::inherit > >::type; + callbacks callbacks_; + + public: + template + typename list_property_definition_callback_type::type& + get () + { + return (static_cast >&> (callbacks_).callback); + } + + template + const typename list_property_definition_callback_type::type& + get () const + { + return (static_cast >&> (callbacks_).callback); + } + + template + friend typename list_property_definition_callback_type::type& + at (list_property_definition_callbacks_type& list_property_definition_callbacks); + + template + friend const typename list_property_definition_callback_type::type& + at (const list_property_definition_callbacks_type& list_property_definition_callbacks); + }; + + template static + typename list_property_definition_callback_type::type& + at (list_property_definition_callbacks_type& list_property_definition_callbacks) + { + return (list_property_definition_callbacks.get ()); + } + + template static + const typename list_property_definition_callback_type::type& + at (const list_property_definition_callbacks_type& list_property_definition_callbacks) + { + return (list_property_definition_callbacks.get ()); + } + + + inline void + info_callback (const info_callback_type& info_callback); + + inline void + warning_callback (const warning_callback_type& warning_callback); + + inline void + error_callback (const error_callback_type& error_callback); + + inline void + magic_callback (const magic_callback_type& magic_callback); + + inline void + format_callback (const format_callback_type& format_callback); + + inline void + element_definition_callback (const element_definition_callback_type& element_definition_callback); + + inline void + scalar_property_definition_callbacks (const scalar_property_definition_callbacks_type& scalar_property_definition_callbacks); + + inline void + list_property_definition_callbacks (const list_property_definition_callbacks_type& list_property_definition_callbacks); + + inline void + comment_callback (const comment_callback_type& comment_callback); + + inline void + obj_info_callback (const obj_info_callback_type& obj_info_callback); + + inline void + end_header_callback (const end_header_callback_type& end_header_callback); + + using flags_type = int; + enum flags { }; + + ply_parser () : + line_number_ (0), current_element_ () + {} + + bool parse (const std::string& filename); + //inline bool parse (const std::string& filename); + + private: + + struct property + { + property (const std::string& name) : name (name) {} + virtual ~property () {} + virtual bool parse (class ply_parser& ply_parser, format_type format, std::istream& istream) = 0; + std::string name; + }; + + template + struct scalar_property : public property + { + using scalar_type = ScalarType; + using callback_type = typename scalar_property_callback_type::type; + scalar_property (const std::string& name, callback_type callback) + : property (name) + , callback (callback) + {} + bool parse (class ply_parser& ply_parser, + format_type format, + std::istream& istream) override + { + return ply_parser.parse_scalar_property (format, istream, callback); + } + callback_type callback; + }; + + template + struct list_property : public property + { + using size_type = SizeType; + using scalar_type = ScalarType; + using begin_callback_type = typename list_property_begin_callback_type::type; + using element_callback_type = typename list_property_element_callback_type::type; + using end_callback_type = typename list_property_end_callback_type::type; + list_property (const std::string& name, + begin_callback_type begin_callback, + element_callback_type element_callback, + end_callback_type end_callback) + : property (name) + , begin_callback (begin_callback) + , element_callback (element_callback) + , end_callback (end_callback) + {} + bool parse (class ply_parser& ply_parser, + format_type format, + std::istream& istream) override + { + return ply_parser.parse_list_property (format, + istream, + begin_callback, + element_callback, + end_callback); + } + begin_callback_type begin_callback; + element_callback_type element_callback; + end_callback_type end_callback; + }; + + struct element + { + element (const std::string& name, + std::size_t count, + const begin_element_callback_type& begin_element_callback, + const end_element_callback_type& end_element_callback) + : name (name) + , count (count) + , begin_element_callback (begin_element_callback) + , end_element_callback (end_element_callback) + {} + std::string name; + std::size_t count; + begin_element_callback_type begin_element_callback; + end_element_callback_type end_element_callback; + std::vector> properties; + }; + + info_callback_type info_callback_ = [](std::size_t, const std::string&){}; + warning_callback_type warning_callback_ = [](std::size_t, const std::string&){}; + error_callback_type error_callback_ = [](std::size_t, const std::string&){}; + + magic_callback_type magic_callback_ = [](){}; + format_callback_type format_callback_ = [](format_type, const std::string&){}; + comment_callback_type comment_callback_ = [](const std::string&){}; + obj_info_callback_type obj_info_callback_ = [](const std::string&){}; + end_header_callback_type end_header_callback_ = [](){return true;}; + + element_definition_callback_type element_definition_callbacks_ = + [](const std::string&, std::size_t) + { + return std::make_tuple([](){}, [](){}); + }; + scalar_property_definition_callbacks_type scalar_property_definition_callbacks_; + list_property_definition_callbacks_type list_property_definition_callbacks_; + + template inline void + parse_scalar_property_definition (const std::string& property_name); + + template inline void + parse_list_property_definition (const std::string& property_name); + + template inline bool + parse_scalar_property (format_type format, + std::istream& istream, + const typename scalar_property_callback_type::type& scalar_property_callback); + + template inline bool + parse_list_property (format_type format, + std::istream& istream, + const typename list_property_begin_callback_type::type& list_property_begin_callback, + const typename list_property_element_callback_type::type& list_property_element_callback, + const typename list_property_end_callback_type::type& list_property_end_callback); + + std::size_t line_number_; + element* current_element_; + }; + } // namespace ply + } // namespace io +} // namespace pcl + +/* inline bool pcl::io::ply::ply_parser::parse (const std::string& filename) */ +/* { */ +/* std::ifstream ifstream (filename.c_str ()); */ +/* return (parse (ifstream)); */ +/* } */ + +inline void pcl::io::ply::ply_parser::info_callback (const info_callback_type& info_callback) +{ + info_callback_ = info_callback; +} + +inline void pcl::io::ply::ply_parser::warning_callback (const warning_callback_type& warning_callback) +{ + warning_callback_ = warning_callback; +} + +inline void pcl::io::ply::ply_parser::error_callback (const error_callback_type& error_callback) +{ + error_callback_ = error_callback; +} + +inline void pcl::io::ply::ply_parser::magic_callback (const magic_callback_type& magic_callback) +{ + magic_callback_ = magic_callback; +} + +inline void pcl::io::ply::ply_parser::format_callback (const format_callback_type& format_callback) +{ + format_callback_ = format_callback; +} + +inline void pcl::io::ply::ply_parser::element_definition_callback (const element_definition_callback_type& element_definition_callback) +{ + element_definition_callbacks_ = element_definition_callback; +} + +inline void pcl::io::ply::ply_parser::scalar_property_definition_callbacks (const scalar_property_definition_callbacks_type& scalar_property_definition_callbacks) +{ + scalar_property_definition_callbacks_ = scalar_property_definition_callbacks; +} + +inline void pcl::io::ply::ply_parser::list_property_definition_callbacks (const list_property_definition_callbacks_type& list_property_definition_callbacks) +{ + list_property_definition_callbacks_ = list_property_definition_callbacks; +} + +inline void pcl::io::ply::ply_parser::comment_callback (const comment_callback_type& comment_callback) +{ + comment_callback_ = comment_callback; +} + +inline void pcl::io::ply::ply_parser::obj_info_callback (const obj_info_callback_type& obj_info_callback) +{ + obj_info_callback_ = obj_info_callback; +} + +inline void pcl::io::ply::ply_parser::end_header_callback (const end_header_callback_type& end_header_callback) +{ + end_header_callback_ = end_header_callback; +} + +template +inline void pcl::io::ply::ply_parser::parse_scalar_property_definition (const std::string& property_name) +{ + using scalar_type = ScalarType; + typename scalar_property_definition_callback_type::type& scalar_property_definition_callback = + scalar_property_definition_callbacks_.get (); + typename scalar_property_callback_type::type scalar_property_callback; + if (scalar_property_definition_callback) + { + scalar_property_callback = scalar_property_definition_callback (current_element_->name, property_name); + } + if (!scalar_property_callback) + { + if (warning_callback_) + { + warning_callback_ (line_number_, + "property '" + std::string (type_traits::name ()) + " " + + property_name + "' of element '" + current_element_->name + "' is not handled"); + } + } + current_element_->properties.emplace_back (new scalar_property (property_name, scalar_property_callback)); +} + +template +inline void pcl::io::ply::ply_parser::parse_list_property_definition (const std::string& property_name) +{ + using size_type = SizeType; + using scalar_type = ScalarType; + using list_property_definition_callback_type = typename list_property_definition_callback_type::type; + list_property_definition_callback_type& list_property_definition_callback = list_property_definition_callbacks_.get (); + using list_property_begin_callback_type = typename list_property_begin_callback_type::type; + using list_property_element_callback_type = typename list_property_element_callback_type::type; + using list_property_end_callback_type = typename list_property_end_callback_type::type; + std::tuple list_property_callbacks; + if (list_property_definition_callback) + { + list_property_callbacks = list_property_definition_callback (current_element_->name, property_name); + } + if (!std::get<0> (list_property_callbacks) || !std::get<1> (list_property_callbacks) || !std::get<2> (list_property_callbacks)) + { + if (warning_callback_) + { + warning_callback_ (line_number_, + "property 'list " + std::string (type_traits::name ()) + " " + + std::string (type_traits::name ()) + " " + + property_name + "' of element '" + + current_element_->name + "' is not handled"); + } + } + current_element_->properties.emplace_back (new list_property ( + property_name, + std::get<0> (list_property_callbacks), + std::get<1> (list_property_callbacks), + std::get<2> (list_property_callbacks))); +} + +template +inline bool pcl::io::ply::ply_parser::parse_scalar_property (format_type format, + std::istream& istream, + const typename scalar_property_callback_type::type& scalar_property_callback) +{ + using namespace io_operators; + using scalar_type = ScalarType; + if (format == ascii_format) + { + std::string value_s; + scalar_type value; + char space = ' '; + istream >> value_s; + try + { + value = static_cast (boost::lexical_cast::parse_type> (value_s)); + } + catch (boost::bad_lexical_cast &) + { + value = std::numeric_limits::quiet_NaN (); + } + + if (!istream.eof ()) + istream >> space >> std::ws; + if (!istream || !isspace (space)) + { + if (error_callback_) + error_callback_ (line_number_, "parse error"); + return (false); + } + if (scalar_property_callback) + scalar_property_callback (value); + return (true); + } + scalar_type value = std::numeric_limits::quiet_NaN (); + istream.read (reinterpret_cast (&value), sizeof (scalar_type)); + if (!istream) + { + if (error_callback_) + error_callback_ (line_number_, "parse error"); + return (false); + } + if (((format == binary_big_endian_format) && (host_byte_order == little_endian_byte_order)) || + ((format == binary_little_endian_format) && (host_byte_order == big_endian_byte_order))) + swap_byte_order (value); + if (scalar_property_callback) + scalar_property_callback (value); + return (true); +} + +template +inline bool pcl::io::ply::ply_parser::parse_list_property (format_type format, std::istream& istream, + const typename list_property_begin_callback_type::type& list_property_begin_callback, + const typename list_property_element_callback_type::type& list_property_element_callback, + const typename list_property_end_callback_type::type& list_property_end_callback) +{ + using namespace io_operators; + using size_type = SizeType; + using scalar_type = ScalarType; + if (format == ascii_format) + { + size_type size = std::numeric_limits::infinity (); + char space = ' '; + istream >> size; + if (!istream.eof ()) + { + istream >> space >> std::ws; + } + if (!istream || !isspace (space)) + { + if (error_callback_) + { + error_callback_ (line_number_, "parse error"); + } + return (false); + } + if (list_property_begin_callback) + { + list_property_begin_callback (size); + } + for (std::size_t index = 0; index < size; ++index) + { + std::string value_s; + scalar_type value; + char space = ' '; + istream >> value_s; + try + { + value = static_cast (boost::lexical_cast::parse_type> (value_s)); + } + catch (boost::bad_lexical_cast &) + { + value = std::numeric_limits::quiet_NaN (); + } + + if (!istream.eof ()) + { + istream >> space >> std::ws; + } + if (!istream || !isspace (space)) + { + if (error_callback_) + { + error_callback_ (line_number_, "parse error"); + } + return (false); + } + if (list_property_element_callback) + { + list_property_element_callback (value); + } + } + if (list_property_end_callback) + { + list_property_end_callback (); + } + return (true); + } + size_type size = std::numeric_limits::infinity (); + istream.read (reinterpret_cast (&size), sizeof (size_type)); + if (((format == binary_big_endian_format) && (host_byte_order == little_endian_byte_order)) || + ((format == binary_little_endian_format) && (host_byte_order == big_endian_byte_order))) + { + swap_byte_order (size); + } + if (!istream) + { + if (error_callback_) + { + error_callback_ (line_number_, "parse error"); + } + return (false); + } + if (list_property_begin_callback) + { + list_property_begin_callback (size); + } + for (std::size_t index = 0; index < size; ++index) { + scalar_type value = std::numeric_limits::quiet_NaN (); + istream.read (reinterpret_cast (&value), sizeof (scalar_type)); + if (!istream) { + if (error_callback_) { + error_callback_ (line_number_, "parse error"); + } + return (false); + } + if (((format == binary_big_endian_format) && (host_byte_order == little_endian_byte_order)) || + ((format == binary_little_endian_format) && (host_byte_order == big_endian_byte_order))) + { + swap_byte_order (value); + } + if (list_property_element_callback) + { + list_property_element_callback (value); + } + } + if (list_property_end_callback) + { + list_property_end_callback (); + } + return (true); +} + +#ifdef BUILD_Maintainer +# if defined __GNUC__ +# if __GNUC__ == 4 && __GNUC_MINOR__ > 3 +# pragma GCC diagnostic warning "-Weffc++" +# pragma GCC diagnostic warning "-pedantic" +# endif +# elif defined _MSC_VER +# pragma warning(pop) +# endif +#endif diff --git a/deps_install/include/pcl-1.10/pcl/io/ply_io.h b/deps_install/include/pcl-1.10/pcl/io/ply_io.h new file mode 100755 index 0000000..918db54 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/ply_io.h @@ -0,0 +1,894 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +namespace pcl +{ + /** \brief Point Cloud Data (PLY) file format reader. + * + * The PLY data format is organized in the following way: + * lines beginning with "comment" are treated as comments + * - ply + * - format [ascii|binary_little_endian|binary_big_endian] 1.0 + * - element vertex COUNT + * - property float x + * - property float y + * - [property float z] + * - [property float normal_x] + * - [property float normal_y] + * - [property float normal_z] + * - [property uchar red] + * - [property uchar green] + * - [property uchar blue] ... + * - ascii/binary point coordinates + * - [element camera 1] + * - [property float view_px] ... + * - [element range_grid COUNT] + * - [property list uchar int vertex_indices] + * - end header + * + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS PLYReader : public FileReader + { + public: + enum + { + PLY_V0 = 0, + PLY_V1 = 1 + }; + + PLYReader () + : origin_ (Eigen::Vector4f::Zero ()) + , orientation_ (Eigen::Matrix3f::Zero ()) + , cloud_ () + , vertex_count_ (0) + , vertex_offset_before_ (0) + , range_grid_ (nullptr) + , rgb_offset_before_ (0) + , do_resize_ (false) + , polygons_ (nullptr) + , r_(0), g_(0), b_(0) + , a_(0), rgba_(0) + {} + + PLYReader (const PLYReader &p) + : origin_ (Eigen::Vector4f::Zero ()) + , orientation_ (Eigen::Matrix3f::Zero ()) + , cloud_ () + , vertex_count_ (0) + , vertex_offset_before_ (0) + , range_grid_ (nullptr) + , rgb_offset_before_ (0) + , do_resize_ (false) + , polygons_ (nullptr) + , r_(0), g_(0), b_(0) + , a_(0), rgba_(0) + { + *this = p; + } + + PLYReader& + operator = (const PLYReader &p) + { + origin_ = p.origin_; + orientation_ = p.orientation_; + range_grid_ = p.range_grid_; + polygons_ = p.polygons_; + return (*this); + } + + ~PLYReader () { delete range_grid_; } + /** \brief Read a point cloud data header from a PLY file. + * + * Load only the meta information (number of points, their types, etc), + * and not the points themselves, from a given PLY file. Useful for fast + * evaluation of the underlying data structure. + * + * Returns: + * * < 0 (-1) on error + * * > 0 on success + * \param[in] file_name the name of the file to load + * \param[out] cloud the resultant point cloud dataset (only the header will be filled) + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[out] ply_version the PLY version read from the file + * \param[out] data_type the type of PLY data stored in the file + * \param[out] data_idx the data index + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int &ply_version, int &data_type, unsigned int &data_idx, const int offset = 0) override; + + /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[out] ply_version the PLY version read from the file + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int& ply_version, const int offset = 0) override; + + /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2. + * \note This function is provided for backwards compatibility only + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + inline int + read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0) + { + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + int ply_version; + return read (file_name, cloud, origin, orientation, ply_version, offset); + } + + /** \brief Read a point cloud data from any PLY file, and convert it to the given template format. + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] cloud the resultant PointCloud message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + template inline int + read (const std::string &file_name, pcl::PointCloud &cloud, const int offset = 0) + { + pcl::PCLPointCloud2 blob; + int ply_version; + int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, + ply_version, offset); + + // Exit in case of error + if (res < 0) + return (res); + pcl::fromPCLPointCloud2 (blob, cloud); + return (0); + } + + /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] mesh the resultant PolygonMesh message read from disk + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[out] ply_version the PLY version read from the file + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, + int& ply_version, const int offset = 0); + + /** \brief Read a point cloud data from a PLY file and store it into a pcl/PolygonMesh. + * + * \param[in] file_name the name of the file containing the actual PointCloud data + * \param[out] mesh the resultant PolygonMesh message read from disk + * \param[in] offset the offset in the file where to expect the true header to begin. + * One usage example for setting the offset parameter is for reading + * data from a TAR "archive containing multiple files: TAR files always + * add a 512 byte header in front of the actual file, so set the offset + * to the next byte after the header (e.g., 513). + */ + int + read (const std::string &file_name, pcl::PolygonMesh &mesh, const int offset = 0); + + private: + ::pcl::io::ply::ply_parser parser_; + + bool + parse (const std::string& istream_filename); + + /** \brief Info callback function + * \param[in] filename PLY file read + * \param[in] line_number line triggering the callback + * \param[in] message information message + */ + void + infoCallback (const std::string& filename, std::size_t line_number, const std::string& message) + { + PCL_DEBUG ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); + } + + /** \brief Warning callback function + * \param[in] filename PLY file read + * \param[in] line_number line triggering the callback + * \param[in] message warning message + */ + void + warningCallback (const std::string& filename, std::size_t line_number, const std::string& message) + { + PCL_WARN ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); + } + + /** \brief Error callback function + * \param[in] filename PLY file read + * \param[in] line_number line triggering the callback + * \param[in] message error message + */ + void + errorCallback (const std::string& filename, std::size_t line_number, const std::string& message) + { + PCL_ERROR ("[pcl::PLYReader] %s:%lu: %s\n", filename.c_str (), line_number, message.c_str ()); + } + + /** \brief function called when the keyword element is parsed + * \param[in] element_name element name + * \param[in] count number of instances + */ + std::tuple, std::function > + elementDefinitionCallback (const std::string& element_name, std::size_t count); + + bool + endHeaderCallback (); + + /** \brief function called when a scalar property is parsed + * \param[in] element_name element name to which the property belongs + * \param[in] property_name property name + */ + template std::function + scalarPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name); + + /** \brief function called when a list property is parsed + * \param[in] element_name element name to which the property belongs + * \param[in] property_name list property name + */ + template + std::tuple, std::function, std::function > + listPropertyDefinitionCallback (const std::string& element_name, const std::string& property_name); + + /** \brief function called at the beginning of a list property parsing. + * \param[in] size number of elements in the list + */ + template void + vertexListPropertyBeginCallback (const std::string& property_name, SizeType size); + + /** \brief function called when a list element is parsed. + * \param[in] value the list's element value + */ + template void + vertexListPropertyContentCallback (ContentType value); + + /** \brief function called at the end of a list property parsing */ + inline void + vertexListPropertyEndCallback (); + + /** Callback function for an anonymous vertex scalar property. + * Writes down a double value in cloud data. + * param[in] value double value parsed + */ + template void + vertexScalarPropertyCallback (Scalar value); + + /** Callback function for vertex RGB color. + * This callback is in charge of packing red green and blue in a single int + * before writing it down in cloud data. + * param[in] color_name color name in {red, green, blue} + * param[in] color value of {red, green, blue} property + */ + inline void + vertexColorCallback (const std::string& color_name, pcl::io::ply::uint8 color); + + /** Callback function for vertex intensity. + * converts intensity from int to float before writing it down in cloud data. + * param[in] intensity + */ + inline void + vertexIntensityCallback (pcl::io::ply::uint8 intensity); + + /** Callback function for vertex alpha. + * extracts RGB value, append alpha and put it back + * param[in] alpha + */ + inline void + vertexAlphaCallback (pcl::io::ply::uint8 alpha); + + /** Callback function for origin x component. + * param[in] value origin x value + */ + inline void + originXCallback (const float& value) { origin_[0] = value; } + + /** Callback function for origin y component. + * param[in] value origin y value + */ + inline void + originYCallback (const float& value) { origin_[1] = value; } + + /** Callback function for origin z component. + * param[in] value origin z value + */ + inline void + originZCallback (const float& value) { origin_[2] = value; } + + /** Callback function for orientation x axis x component. + * param[in] value orientation x axis x value + */ + inline void + orientationXaxisXCallback (const float& value) { orientation_ (0,0) = value; } + + /** Callback function for orientation x axis y component. + * param[in] value orientation x axis y value + */ + inline void + orientationXaxisYCallback (const float& value) { orientation_ (0,1) = value; } + + /** Callback function for orientation x axis z component. + * param[in] value orientation x axis z value + */ + inline void + orientationXaxisZCallback (const float& value) { orientation_ (0,2) = value; } + + /** Callback function for orientation y axis x component. + * param[in] value orientation y axis x value + */ + inline void + orientationYaxisXCallback (const float& value) { orientation_ (1,0) = value; } + + /** Callback function for orientation y axis y component. + * param[in] value orientation y axis y value + */ + inline void + orientationYaxisYCallback (const float& value) { orientation_ (1,1) = value; } + + /** Callback function for orientation y axis z component. + * param[in] value orientation y axis z value + */ + inline void + orientationYaxisZCallback (const float& value) { orientation_ (1,2) = value; } + + /** Callback function for orientation z axis x component. + * param[in] value orientation z axis x value + */ + inline void + orientationZaxisXCallback (const float& value) { orientation_ (2,0) = value; } + + /** Callback function for orientation z axis y component. + * param[in] value orientation z axis y value + */ + inline void + orientationZaxisYCallback (const float& value) { orientation_ (2,1) = value; } + + /** Callback function for orientation z axis z component. + * param[in] value orientation z axis z value + */ + inline void + orientationZaxisZCallback (const float& value) { orientation_ (2,2) = value; } + + /** Callback function to set the cloud height + * param[in] height cloud height + */ + inline void + cloudHeightCallback (const int &height) { cloud_->height = height; } + + /** Callback function to set the cloud width + * param[in] width cloud width + */ + inline void + cloudWidthCallback (const int &width) { cloud_->width = width; } + + /** Append a scalar property to the cloud fields. + * param[in] name property name + * param[in] count property count: 1 for scalar properties and higher for a + * list property. + */ + template void + appendScalarProperty (const std::string& name, const std::size_t& count = 1); + + /** Amend property from cloud fields identified by \a old_name renaming + * it \a new_name. + * param[in] old_name property old name + * param[in] new_name property new name + */ + void + amendProperty (const std::string& old_name, const std::string& new_name, std::uint8_t datatype = 0); + + /** Callback function for the begin of vertex line */ + void + vertexBeginCallback (); + + /** Callback function for the end of vertex line */ + void + vertexEndCallback (); + + /** Callback function for the begin of range_grid line */ + void + rangeGridBeginCallback (); + + /** Callback function for the begin of range_grid vertex_indices property + * param[in] size vertex_indices list size + */ + void + rangeGridVertexIndicesBeginCallback (pcl::io::ply::uint8 size); + + /** Callback function for each range_grid vertex_indices element + * param[in] vertex_index index of the vertex in vertex_indices + */ + void + rangeGridVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index); + + /** Callback function for the end of a range_grid vertex_indices property */ + void + rangeGridVertexIndicesEndCallback (); + + /** Callback function for the end of a range_grid element end */ + void + rangeGridEndCallback (); + + /** Callback function for obj_info */ + void + objInfoCallback (const std::string& line); + + /** Callback function for the begin of face line */ + void + faceBeginCallback (); + + /** Callback function for the begin of face vertex_indices property + * param[in] size vertex_indices list size + */ + void + faceVertexIndicesBeginCallback (pcl::io::ply::uint8 size); + + /** Callback function for each face vertex_indices element + * param[in] vertex_index index of the vertex in vertex_indices + */ + void + faceVertexIndicesElementCallback (pcl::io::ply::int32 vertex_index); + + /** Callback function for the end of a face vertex_indices property */ + void + faceVertexIndicesEndCallback (); + + /** Callback function for the end of a face element end */ + void + faceEndCallback (); + + /// origin + Eigen::Vector4f origin_; + + /// orientation + Eigen::Matrix3f orientation_; + + //vertex element artifacts + pcl::PCLPointCloud2 *cloud_; + std::size_t vertex_count_; + int vertex_offset_before_; + //range element artifacts + std::vector > *range_grid_; + std::size_t rgb_offset_before_; + bool do_resize_; + //face element artifact + std::vector *polygons_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + + private: + // RGB values stored by vertexColorCallback() + std::int32_t r_, g_, b_; + // Color values stored by vertexAlphaCallback() + std::uint32_t a_, rgba_; + }; + + /** \brief Point Cloud Data (PLY) file format writer. + * \author Nizar Sallem + * \ingroup io + */ + class PCL_EXPORTS PLYWriter : public FileWriter + { + public: + ///Constructor + PLYWriter () {}; + + ///Destructor + ~PLYWriter () {}; + + /** \brief Generate the header of a PLY v.7 file format + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[in] valid_points number of valid points (finite ones for range_grid and + * all of them for camer) + * \param[in] use_camera if set to true then PLY file will use element camera else + * element range_grid will be used. + */ + inline std::string + generateHeaderBinary (const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation, + int valid_points, + bool use_camera = true) + { + return (generateHeader (cloud, origin, orientation, true, use_camera, valid_points)); + } + + /** \brief Generate the header of a PLY v.7 file format + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[in] valid_points number of valid points (finite ones for range_grid and + * all of them for camer) + * \param[in] use_camera if set to true then PLY file will use element camera else + * element range_grid will be used. + */ + inline std::string + generateHeaderASCII (const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation, + int valid_points, + bool use_camera = true) + { + return (generateHeader (cloud, origin, orientation, false, use_camera, valid_points)); + } + + /** \brief Save point cloud data to a PLY file containing n-D points, in ASCII format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[in] precision the specified output numeric stream precision (default: 8) + * \param[in] use_camera if set to true then PLY file will use element camera else + * element range_grid will be used. + */ + int + writeASCII (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + int precision = 8, + bool use_camera = true); + + /** \brief Save point cloud data to a PLY file containing n-D points, in BINARY format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[in] use_camera if set to true then PLY file will use element camera else + * element range_grid will be used + */ + int + writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + bool use_camera = true); + + /** \brief Save point cloud data to a PLY file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \param[in] binary set to true if the file is to be written in a binary + * PLY format, false (default) for ASCII + */ + inline int + write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + const bool binary = false) override + { + if (binary) + return (this->writeBinary (file_name, cloud, origin, orientation, true)); + return (this->writeASCII (file_name, cloud, origin, orientation, 8, true)); + } + + /** \brief Save point cloud data to a PLY file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \param[in] binary set to true if the file is to be written in a binary + * PLY format, false (default) for ASCII + * \param[in] use_camera set to true to use camera element and false to + * use range_grid element + */ + inline int + write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + bool binary = false, + bool use_camera = true) + { + if (binary) + return (this->writeBinary (file_name, cloud, origin, orientation, use_camera)); + return (this->writeASCII (file_name, cloud, origin, orientation, 8, use_camera)); + } + + /** \brief Save point cloud data to a PLY file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message (boost shared pointer) + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + * \param[in] binary set to true if the file is to be written in a binary + * PLY format, false (default) for ASCII + * \param[in] use_camera set to true to use camera element and false to + * use range_grid element + */ + inline int + write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + bool binary = false, + bool use_camera = true) + { + return (write (file_name, *cloud, origin, orientation, binary, use_camera)); + } + + /** \brief Save point cloud data to a PLY file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the pcl::PointCloud data + * \param[in] binary set to true if the file is to be written in a binary + * PLY format, false (default) for ASCII + * \param[in] use_camera set to true to use camera element and false to + * use range_grid element + */ + template inline int + write (const std::string &file_name, + const pcl::PointCloud &cloud, + bool binary = false, + bool use_camera = true) + { + Eigen::Vector4f origin = cloud.sensor_origin_; + Eigen::Quaternionf orientation = cloud.sensor_orientation_; + + pcl::PCLPointCloud2 blob; + pcl::toPCLPointCloud2 (cloud, blob); + + // Save the data + return (this->write (file_name, blob, origin, orientation, binary, use_camera)); + } + + private: + /** \brief Generate a PLY header. + * \param[in] cloud the input point cloud + * \param[in] binary whether the PLY file should be saved as binary data (true) or ascii (false) + */ + std::string + generateHeader (const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation, + bool binary, + bool use_camera, + int valid_points); + + void + writeContentWithCameraASCII (int nr_points, + int point_size, + const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, + const Eigen::Quaternionf &orientation, + std::ofstream& fs); + + void + writeContentWithRangeGridASCII (int nr_points, + int point_size, + const pcl::PCLPointCloud2 &cloud, + std::ostringstream& fs, + int& nb_valid_points); + }; + + namespace io + { + /** \brief Load a PLY v.6 file into a templated PointCloud type. + * + * Any PLY files containing sensor data will generate a warning as a + * pcl/PCLPointCloud2 message cannot hold the sensor origin. + * + * \param[in] file_name the name of the file to load + * \param[in] cloud the resultant templated point cloud + * \ingroup io + */ + inline int + loadPLYFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) + { + pcl::PLYReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load any PLY file into a templated PointCloud type. + * \param[in] file_name the name of the file to load + * \param[in] cloud the resultant templated point cloud + * \param[in] origin the sensor acquisition origin (only for > PLY_V7 - null if not present) + * \param[in] orientation the sensor acquisition orientation if available, + * identity if not present + * \ingroup io + */ + inline int + loadPLYFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) + { + pcl::PLYReader p; + int ply_version; + return (p.read (file_name, cloud, origin, orientation, ply_version)); + } + + /** \brief Load any PLY file into a templated PointCloud type + * \param[in] file_name the name of the file to load + * \param[in] cloud the resultant templated point cloud + * \ingroup io + */ + template inline int + loadPLYFile (const std::string &file_name, pcl::PointCloud &cloud) + { + pcl::PLYReader p; + return (p.read (file_name, cloud)); + } + + /** \brief Load a PLY file into a PolygonMesh type. + * + * Any PLY files containing sensor data will generate a warning as a + * pcl/PolygonMesh message cannot hold the sensor origin. + * + * \param[in] file_name the name of the file to load + * \param[in] mesh the resultant polygon mesh + * \ingroup io + */ + inline int + loadPLYFile (const std::string &file_name, pcl::PolygonMesh &mesh) + { + pcl::PLYReader p; + return (p.read (file_name, mesh)); + } + + /** \brief Save point cloud data to a PLY file containing n-D points + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor data acquisition origin (translation) + * \param[in] orientation the sensor data acquisition origin (rotation) + * \param[in] binary_mode true for binary mode, false (default) for ASCII + * \param[in] use_camera + * \ingroup io + */ + inline int + savePLYFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), + bool binary_mode = false, bool use_camera = true) + { + PLYWriter w; + return (w.write (file_name, cloud, origin, orientation, binary_mode, use_camera)); + } + + /** \brief Templated version for saving point cloud data to a PLY file + * containing a specific given cloud format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] binary_mode true for binary mode, false (default) for ASCII + * \ingroup io + */ + template inline int + savePLYFile (const std::string &file_name, const pcl::PointCloud &cloud, bool binary_mode = false) + { + PLYWriter w; + return (w.write (file_name, cloud, binary_mode)); + } + + /** \brief Templated version for saving point cloud data to a PLY file + * containing a specific given cloud format. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \ingroup io + */ + template inline int + savePLYFileASCII (const std::string &file_name, const pcl::PointCloud &cloud) + { + PLYWriter w; + return (w.write (file_name, cloud, false)); + } + + /** \brief Templated version for saving point cloud data to a PLY file containing a specific given cloud format. + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \ingroup io + */ + template inline int + savePLYFileBinary (const std::string &file_name, const pcl::PointCloud &cloud) + { + PLYWriter w; + return (w.write (file_name, cloud, true)); + } + + /** \brief Templated version for saving point cloud data to a PLY file containing a specific given cloud format + * \param[in] file_name the output file name + * \param[in] cloud the point cloud data message + * \param[in] indices the set of indices to save + * \param[in] binary_mode true for binary mode, false (default) for ASCII + * \ingroup io + */ + template int + savePLYFile (const std::string &file_name, const pcl::PointCloud &cloud, + const std::vector &indices, bool binary_mode = false) + { + // Copy indices to a new point cloud + pcl::PointCloud cloud_out; + copyPointCloud (cloud, indices, cloud_out); + // Save the data + PLYWriter w; + return (w.write (file_name, cloud_out, binary_mode)); + } + + /** \brief Saves a PolygonMesh in ascii PLY format. + * \param[in] file_name the name of the file to write to disk + * \param[in] mesh the polygonal mesh to save + * \param[in] precision the output ASCII precision default 5 + * \ingroup io + */ + PCL_EXPORTS int + savePLYFile (const std::string &file_name, const pcl::PolygonMesh &mesh, unsigned precision = 5); + + /** \brief Saves a PolygonMesh in binary PLY format. + * \param[in] file_name the name of the file to write to disk + * \param[in] mesh the polygonal mesh to save + * \ingroup io + */ + PCL_EXPORTS int + savePLYFileBinary (const std::string &file_name, const pcl::PolygonMesh &mesh); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/point_cloud_image_extractors.h b/deps_install/include/pcl-1.10/pcl/io/point_cloud_image_extractors.h new file mode 100755 index 0000000..a83e6da --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/point_cloud_image_extractors.h @@ -0,0 +1,438 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace io + { + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Base Image Extractor class for organized point clouds. + * + * This is an abstract class that declares an interface for image extraction from + * organized point clouds. The family of its subclasses provide functionality to + * extract images from particular fields. + * + * The following piece of code demonstrates typical usage of a PointCloudImageExtractor + * subclass. Here the data are extracted from the "label" field and are saved as a + * PNG image file. + * + * \code + * // Source point cloud (needs to be filled with data of course) + * pcl::PointCloud cloud; + * // Target image + * pcl::PCLImage image; + * // Create PointCloudImageExtractor subclass that can handle "label" field + * pcl::io::PointCloudImageExtractorFromLabelField pcie; + * // Set it up if not happy with the defaults + * pcie.setColorMode(pcie.COLORS_RGB_RANDOM); + * // Try to extract an image + * bool success = pcie.extract(cloud, image); + * // Save to file if succeeded + * if (success) + * pcl::io::saveImage ("filename.png", image); + * \endcode + * + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractor + { + public: + using PointCloud = pcl::PointCloud; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + PointCloudImageExtractor () + : paint_nans_with_black_ (false) + {} + + /** \brief Destructor. */ + virtual ~PointCloudImageExtractor () {} + + /** \brief Obtain the image from the given cloud. + * \param[in] cloud organized point cloud to extract image from + * \param[out] image the output image + * \return true if the operation was successful, false otherwise + */ + bool + extract (const PointCloud& cloud, pcl::PCLImage& image) const; + + /** \brief Set a flag that controls if image pixels corresponding to + * NaN (infinite) points should be painted black. + */ + inline void + setPaintNaNsWithBlack (bool flag) + { + paint_nans_with_black_ = flag; + } + + protected: + + /** \brief Implementation of the extract() function, has to be + * implemented in deriving classes. + */ + virtual bool + extractImpl (const PointCloud& cloud, pcl::PCLImage& image) const = 0; + + /// A flag that controls if image pixels corresponding to NaN (infinite) + /// points should be painted black. + bool paint_nans_with_black_; + }; + + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Image Extractor extension which provides functionality to apply scaling to + * the values extracted from a field. + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractorWithScaling : public PointCloudImageExtractor + { + using PointCloud = typename PointCloudImageExtractor::PointCloud; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Different scaling methods. + *
    + *
  • SCALING_NO - no scaling.
    • + *
  • SCALING_FULL_RANGE - scales to full range of the output value.
    • + *
  • SCASING_FIXED_FACTOR - scales by a given fixed factor.
    • + *
+ */ + enum ScalingMethod + { + SCALING_NO, + SCALING_FULL_RANGE, + SCALING_FIXED_FACTOR + }; + + /** \brief Constructor. */ + PointCloudImageExtractorWithScaling (const std::string& field_name, const ScalingMethod scaling_method) + : field_name_ (field_name) + , scaling_method_ (scaling_method) + , scaling_factor_ (1.0f) + { + } + + /** \brief Constructor. */ + PointCloudImageExtractorWithScaling (const std::string& field_name, const float scaling_factor) + : field_name_ (field_name) + , scaling_method_ (SCALING_FIXED_FACTOR) + , scaling_factor_ (scaling_factor) + { + } + + /** \brief Destructor. */ + ~PointCloudImageExtractorWithScaling () {} + + /** \brief Set scaling method. */ + inline void + setScalingMethod (const ScalingMethod scaling_method) + { + scaling_method_ = scaling_method; + } + + /** \brief Set fixed scaling factor. */ + inline void + setScalingFactor (const float scaling_factor) + { + scaling_factor_ = scaling_factor; + } + + protected: + + bool + extractImpl (const PointCloud& cloud, pcl::PCLImage& image) const override; + + std::string field_name_; + ScalingMethod scaling_method_; + float scaling_factor_; + }; + + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Image Extractor which uses the data present in the "normal" field. Normal + * vector components (x, y, z) are mapped to color channels (r, g, b respectively). + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractorFromNormalField : public PointCloudImageExtractor + { + using PointCloud = typename PointCloudImageExtractor::PointCloud; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + PointCloudImageExtractorFromNormalField () {} + + /** \brief Destructor. */ + ~PointCloudImageExtractorFromNormalField () {} + + protected: + + bool + extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const override; + }; + + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Image Extractor which uses the data present in the "rgb" or "rgba" fields + * to produce a color image with rgb8 encoding. + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractorFromRGBField : public PointCloudImageExtractor + { + using PointCloud = typename PointCloudImageExtractor::PointCloud; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + PointCloudImageExtractorFromRGBField () {} + + /** \brief Destructor. */ + ~PointCloudImageExtractorFromRGBField () {} + + protected: + + bool + extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const override; + }; + + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Image Extractor which uses the data present in the "label" field to produce + * either monochrome or RGB image where different labels correspond to different + * colors. + * See the documentation for ColorMode to learn about available coloring options. + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractorFromLabelField : public PointCloudImageExtractor + { + using PointCloud = typename PointCloudImageExtractor::PointCloud; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Different modes for color mapping. */ + enum ColorMode + { + /// Shades of gray (according to label id) + /// \note Labels using more than 16 bits will cause problems + COLORS_MONO, + /// Randomly generated RGB colors + COLORS_RGB_RANDOM, + /// Fixed RGB colors from the [Glasbey lookup table](http://fiji.sc/Glasbey), + /// assigned in the ascending order of label id + COLORS_RGB_GLASBEY, + }; + + /** \brief Constructor. */ + PointCloudImageExtractorFromLabelField (const ColorMode color_mode = COLORS_MONO) + : color_mode_ (color_mode) + { + } + + /** \brief Destructor. */ + ~PointCloudImageExtractorFromLabelField () {} + + /** \brief Set color mapping mode. */ + inline void + setColorMode (const ColorMode color_mode) + { + color_mode_ = color_mode; + } + + protected: + + bool + extractImpl (const PointCloud& cloud, pcl::PCLImage& img) const override; + + // Members derived from the base class + using PointCloudImageExtractor::paint_nans_with_black_; + + private: + + ColorMode color_mode_; + }; + + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Image Extractor which uses the data present in the "z" field to produce a + * depth map (as a monochrome image with mono16 encoding). + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractorFromZField : public PointCloudImageExtractorWithScaling + { + using PointCloud = typename PointCloudImageExtractor::PointCloud; + using ScalingMethod = typename PointCloudImageExtractorWithScaling::ScalingMethod; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + * \param[in] scaling_factor a scaling factor to apply to each depth value (default 10000) + */ + PointCloudImageExtractorFromZField (const float scaling_factor = 10000) + : PointCloudImageExtractorWithScaling ("z", scaling_factor) + { + } + + /** \brief Constructor. + * \param[in] scaling_method a scaling method to use + */ + PointCloudImageExtractorFromZField (const ScalingMethod scaling_method) + : PointCloudImageExtractorWithScaling ("z", scaling_method) + { + } + + /** \brief Destructor. */ + ~PointCloudImageExtractorFromZField () {} + + protected: + // Members derived from the base class + using PointCloudImageExtractorWithScaling::field_name_; + using PointCloudImageExtractorWithScaling::scaling_method_; + using PointCloudImageExtractorWithScaling::scaling_factor_; + }; + + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Image Extractor which uses the data present in the "curvature" field to + * produce a curvature map (as a monochrome image with mono16 encoding). + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractorFromCurvatureField : public PointCloudImageExtractorWithScaling + { + using PointCloud = typename PointCloudImageExtractor::PointCloud; + using ScalingMethod = typename PointCloudImageExtractorWithScaling::ScalingMethod; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + * \param[in] scaling_method a scaling method to use (default SCALING_FULL_RANGE) + */ + PointCloudImageExtractorFromCurvatureField (const ScalingMethod scaling_method = PointCloudImageExtractorWithScaling::SCALING_FULL_RANGE) + : PointCloudImageExtractorWithScaling ("curvature", scaling_method) + { + } + + /** \brief Constructor. + * \param[in] scaling_factor a scaling factor to apply to each curvature value + */ + PointCloudImageExtractorFromCurvatureField (const float scaling_factor) + : PointCloudImageExtractorWithScaling ("curvature", scaling_factor) + { + } + + /** \brief Destructor. */ + ~PointCloudImageExtractorFromCurvatureField () {} + + protected: + // Members derived from the base class + using PointCloudImageExtractorWithScaling::field_name_; + using PointCloudImageExtractorWithScaling::scaling_method_; + using PointCloudImageExtractorWithScaling::scaling_factor_; + }; + + ////////////////////////////////////////////////////////////////////////////////////// + /** \brief Image Extractor which uses the data present in the "intensity" field to produce a + * monochrome intensity image (with mono16 encoding). + * \author Sergey Alexandrov + * \ingroup io + */ + template + class PointCloudImageExtractorFromIntensityField : public PointCloudImageExtractorWithScaling + { + using PointCloud = typename PointCloudImageExtractor::PointCloud; + using ScalingMethod = typename PointCloudImageExtractorWithScaling::ScalingMethod; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. + * \param[in] scaling_method a scaling method to use (default SCALING_NO) + */ + PointCloudImageExtractorFromIntensityField (const ScalingMethod scaling_method = PointCloudImageExtractorWithScaling::SCALING_NO) + : PointCloudImageExtractorWithScaling ("intensity", scaling_method) + { + } + + /** \brief Constructor. + * \param[in] scaling_factor a scaling factor to apply to each intensity value + */ + PointCloudImageExtractorFromIntensityField (const float scaling_factor) + : PointCloudImageExtractorWithScaling ("intensity", scaling_factor) + { + } + + /** \brief Destructor. */ + ~PointCloudImageExtractorFromIntensityField () {} + + protected: + // Members derived from the base class + using PointCloudImageExtractorWithScaling::field_name_; + using PointCloudImageExtractorWithScaling::scaling_method_; + using PointCloudImageExtractorWithScaling::scaling_factor_; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/io/pxc_grabber.h b/deps_install/include/pcl-1.10/pcl/io/pxc_grabber.h new file mode 100755 index 0000000..245e64f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/pxc_grabber.h @@ -0,0 +1 @@ +#error "PXCGrabber was deprecated and removed, please use DepthSenseGrabber instead" diff --git a/deps_install/include/pcl-1.10/pcl/io/robot_eye_grabber.h b/deps_install/include/pcl-1.10/pcl/io/robot_eye_grabber.h new file mode 100755 index 0000000..92487f8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/robot_eye_grabber.h @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "pcl/pcl_config.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace pcl +{ + + /** \brief Grabber for the Ocular Robotics RobotEye sensor. + * \ingroup io + */ + class PCL_EXPORTS RobotEyeGrabber : public Grabber + { + public: + + /** \brief Signal used for the point cloud callback. + * This signal is sent when the accumulated number of points reaches + * the limit specified by setSignalPointCloudSize(). + */ + using sig_cb_robot_eye_point_cloud_xyzi = void (const pcl::PointCloud::ConstPtr &); + + /** \brief RobotEyeGrabber default constructor. */ + RobotEyeGrabber (); + + /** \brief RobotEyeGrabber constructor taking a specified IP address and data port. */ + RobotEyeGrabber (const boost::asio::ip::address& ipAddress, unsigned short port=443); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + ~RobotEyeGrabber () noexcept; + + /** \brief Starts the RobotEye grabber. + * The grabber runs on a separate thread, this call will return without blocking. */ + void start () override; + + /** \brief Stops the RobotEye grabber. */ + void stop () override; + + /** \brief Obtains the name of this I/O Grabber + * \return The name of the grabber + */ + std::string getName () const override; + + /** \brief Check if the grabber is still running. + * \return TRUE if the grabber is running, FALSE otherwise + */ + bool isRunning () const override; + + /** \brief Returns the number of frames per second. + */ + float getFramesPerSecond () const override; + + /** \brief Set/get ip address of the sensor that sends the data. + * The default is address_v4::any (). + */ + void setSensorAddress (const boost::asio::ip::address& ipAddress); + const boost::asio::ip::address& getSensorAddress () const; + + /** \brief Set/get the port number which receives data from the sensor. + * The default is 443. + */ + void setDataPort (unsigned short port); + unsigned short getDataPort () const; + + /** \brief Set/get the number of points to accumulate before the grabber + * callback is signaled. The default is 1000. + */ + void setSignalPointCloudSize (std::size_t numerOfPoints); + std::size_t getSignalPointCloudSize () const; + + /** \brief Returns the point cloud with point accumulated by the grabber. + * It is not safe to access this point cloud except if the grabber is + * stopped or during the grabber callback. + */ + pcl::PointCloud::Ptr getPointCloud() const; + + private: + + bool terminate_thread_; + std::size_t signal_point_cloud_size_; + unsigned short data_port_; + enum { MAX_LENGTH = 65535 }; + unsigned char receive_buffer_[MAX_LENGTH]; + unsigned int data_size_; + + boost::asio::ip::address sensor_address_; + boost::asio::ip::udp::endpoint sender_endpoint_; + boost::asio::io_service io_service_; + std::shared_ptr socket_; + std::shared_ptr socket_thread_; + std::shared_ptr consumer_thread_; + + pcl::SynchronizedQueue > packet_queue_; + pcl::PointCloud::Ptr point_cloud_xyzi_; + boost::signals2::signal* point_cloud_signal_; + + void consumerThreadLoop (); + void socketThreadLoop (); + void asyncSocketReceive (); + void resetPointCloud (); + void socketCallback (const boost::system::error_code& error, std::size_t number_of_bytes); + void convertPacketData (unsigned char *data_packet, std::size_t length); + void computeXYZI (pcl::PointXYZI& point_XYZI, unsigned char* point_data); + void computeTimestamp (std::uint32_t& timestamp, unsigned char* point_data); + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/io/tar.h b/deps_install/include/pcl-1.10/pcl/io/tar.h new file mode 100755 index 0000000..210ecc9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/tar.h @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace io + { + /** \brief A TAR file's header, as described on + * http://en.wikipedia.org/wiki/Tar_%28file_format%29. + */ + struct TARHeader + { + char file_name[100]; + char file_mode[8]; + char uid[8]; + char gid[8]; + char file_size[12]; + char mtime[12]; + char chksum[8]; + char file_type[1]; + char link_file_name[100]; + char ustar[6]; + char ustar_version[2]; + char uname[32]; + char gname[32]; + char dev_major[8]; + char dev_minor[8]; + char file_name_prefix[155]; + char _padding[12]; + + /** \brief get file size */ + unsigned int + getFileSize () + { + unsigned int output = 0; + char *str = file_size; + for (int i = 0; i < 11; i++) + { + output = output * 8 + *str - '0'; + str++; + } + return (output); + } + }; + + /** \brief Save a PointCloud dataset into a TAR file. + * Append if the file exists, or create a new one if not. + * \remark till implemented will return FALSE + */ + // param[in] tar_filename the name of the TAR file to save the cloud to + // param[in] cloud the point cloud dataset to save + // param[in] pcd_filename the internal name of the PCD file that should be stored in the TAR header + template bool + saveTARPointCloud (const std::string& /*tar_filename*/, + const PointCloud& /*cloud*/, + const std::string& /*pcd_filename*/) + { + return (false); + } + } +} diff --git a/deps_install/include/pcl-1.10/pcl/io/vlp_grabber.h b/deps_install/include/pcl-1.10/pcl/io/vlp_grabber.h new file mode 100755 index 0000000..da863b5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/vlp_grabber.h @@ -0,0 +1,125 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 2015 The MITRE Corporation + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "pcl/pcl_config.h" + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + + /** \brief Grabber for the Velodyne LiDAR (VLP), based on the Velodyne High Definition Laser (HDL) + * \author Keven Ring + * \ingroup io + */ + class PCL_EXPORTS VLPGrabber : public HDLGrabber + { + public: + /** \brief Constructor taking an optional path to an vlp corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368] + * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional + */ + VLPGrabber (const std::string& pcapFile = ""); + + /** \brief Constructor taking a specified IP/port + * \param[in] ipAddress IP Address that should be used to listen for VLP packets + * \param[in] port UDP Port that should be used to listen for VLP packets + */ + VLPGrabber (const boost::asio::ip::address& ipAddress, + const std::uint16_t port); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + + ~VLPGrabber () noexcept; + + /** \brief Obtains the name of this I/O Grabber + * \return The name of the grabber + */ + std::string + getName () const override; + + /** \brief Allows one to customize the colors used by each laser. + * \param[in] color RGB color to set + * \param[in] laserNumber Number of laser to set color + */ + void + setLaserColorRGB (const pcl::RGB& color, + const std::uint8_t laserNumber); + + /** \brief Allows one to customize the colors used for each of the lasers. + * \param[in] begin begin iterator of RGB color array + * \param[in] end end iterator of RGB color array + */ + template void + setLaserColorRGB (const IterT& begin, const IterT& end) + { + std::copy (begin, end, laser_rgb_mapping_); + } + + /** \brief Returns the maximum number of lasers + */ + std::uint8_t + getMaximumNumberOfLasers () const override; + + protected: + static const std::uint8_t VLP_MAX_NUM_LASERS = 16; + static const std::uint8_t VLP_DUAL_MODE = 0x39; + + private: + pcl::RGB laser_rgb_mapping_[VLP_MAX_NUM_LASERS]; + + void + toPointClouds (HDLDataPacket *dataPacket) override; + + boost::asio::ip::address + getDefaultNetworkAddress () override; + + void + initializeLaserMapping (); + + void + loadVLP16Corrections (); + + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/io/vtk_io.h b/deps_install/include/pcl-1.10/pcl/io/vtk_io.h new file mode 100755 index 0000000..a97654e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/io/vtk_io.h @@ -0,0 +1,72 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +// Please do not add any functions that depend on VTK structures to this file! +// Use vtk_io_lib.h instead. + +namespace pcl +{ + namespace io + { + /** \brief Saves a PolygonMesh in ascii VTK format. + * \param[in] file_name the name of the file to write to disk + * \param[in] triangles the polygonal mesh to save + * \param[in] precision the output ASCII precision + * \ingroup io + */ + PCL_EXPORTS int + saveVTKFile (const std::string &file_name, const pcl::PolygonMesh &triangles, unsigned precision = 5); + + /** \brief Saves a PointCloud in ascii VTK format. + * \param[in] file_name the name of the file to write to disk + * \param[in] cloud the point cloud to save + * \param[in] precision the output ASCII precision + * \ingroup io + */ + PCL_EXPORTS int + saveVTKFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, unsigned precision = 5); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/kdtree/flann.h b/deps_install/include/pcl-1.10/pcl/kdtree/flann.h new file mode 100755 index 0000000..cd944a2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/kdtree/flann.h @@ -0,0 +1,53 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +PCL_PRAGMA_WARNING("This header is deprecated and will be removed in an upcoming release.") + +#if defined _MSC_VER +# pragma warning(disable: 4267 4244) +#endif + +#include + +#if defined _MSC_VER +# pragma warning(default: 4267) +#endif diff --git a/deps_install/include/pcl-1.10/pcl/kdtree/impl/io.hpp b/deps_install/include/pcl-1.10/pcl/kdtree/impl/io.hpp new file mode 100755 index 0000000..fe24823 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/kdtree/impl/io.hpp @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_KDTREE_IO_IMPL_HPP_ +#define PCL_KDTREE_IO_IMPL_HPP_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getApproximateIndices ( + const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, + std::vector &indices) +{ + pcl::KdTreeFLANN tree; + tree.setInputCloud (cloud_ref); + + std::vector nn_idx (1); + std::vector nn_dists (1); + indices.resize (cloud_in->points.size ()); + for (std::size_t i = 0; i < cloud_in->points.size (); ++i) + { + tree.nearestKSearchT ((*cloud_in)[i], 1, nn_idx, nn_dists); + indices[i] = nn_idx[0]; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getApproximateIndices ( + const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, + std::vector &indices) +{ + pcl::KdTreeFLANN tree; + tree.setInputCloud (cloud_ref); + + std::vector nn_idx (1); + std::vector nn_dists (1); + indices.resize (cloud_in->points.size ()); + for (std::size_t i = 0; i < cloud_in->points.size (); ++i) + { + tree.nearestKSearch (*cloud_in, i, 1, nn_idx, nn_dists); + indices[i] = nn_idx[0]; + } +} + +#endif // PCL_KDTREE_IO_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/kdtree/impl/kdtree_flann.hpp b/deps_install/include/pcl-1.10/pcl/kdtree/impl/kdtree_flann.hpp new file mode 100755 index 0000000..7d76b18 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/kdtree/impl/kdtree_flann.hpp @@ -0,0 +1,301 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_ +#define PCL_KDTREE_KDTREE_IMPL_FLANN_H_ + +#include + +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::KdTreeFLANN::KdTreeFLANN (bool sorted) + : pcl::KdTree (sorted) + , flann_index_ () + , identity_mapping_ (false) + , dim_ (0), total_nr_points_ (0) + , param_k_ (::flann::SearchParams (-1 , epsilon_)) + , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted)) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::KdTreeFLANN::KdTreeFLANN (const KdTreeFLANN &k) + : pcl::KdTree (false) + , flann_index_ () + , identity_mapping_ (false) + , dim_ (0), total_nr_points_ (0) + , param_k_ (::flann::SearchParams (-1 , epsilon_)) + , param_radius_ (::flann::SearchParams (-1, epsilon_, false)) +{ + *this = k; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeFLANN::setEpsilon (float eps) +{ + epsilon_ = eps; + param_k_ = ::flann::SearchParams (-1 , epsilon_); + param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeFLANN::setSortedResults (bool sorted) +{ + sorted_ = sorted; + param_k_ = ::flann::SearchParams (-1, epsilon_); + param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeFLANN::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) +{ + cleanup (); // Perform an automatic cleanup of structures + + epsilon_ = 0.0f; // default error bound value + dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz + + input_ = cloud; + indices_ = indices; + + // Allocate enough data + if (!input_) + { + PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n"); + return; + } + if (indices != nullptr) + { + convertCloudToArray (*input_, *indices_); + } + else + { + convertCloudToArray (*input_); + } + total_nr_points_ = static_cast (index_mapping_.size ()); + if (total_nr_points_ == 0) + { + PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n"); + return; + } + + flann_index_.reset (new FLANNIndex (::flann::Matrix (cloud_.get (), + index_mapping_.size (), + dim_), + ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf + flann_index_->buildIndex (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::KdTreeFLANN::nearestKSearch (const PointT &point, int k, + std::vector &k_indices, + std::vector &k_distances) const +{ + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + if (k > total_nr_points_) + k = total_nr_points_; + + k_indices.resize (k); + k_distances.resize (k); + + std::vector query (dim_); + point_representation_->vectorize (static_cast (point), query); + + ::flann::Matrix k_indices_mat (&k_indices[0], 1, k); + ::flann::Matrix k_distances_mat (&k_distances[0], 1, k); + // Wrap the k_indices and k_distances vectors (no data copy) + flann_index_->knnSearch (::flann::Matrix (&query[0], 1, dim_), + k_indices_mat, k_distances_mat, + k, param_k_); + + // Do mapping to original point cloud + if (!identity_mapping_) + { + for (std::size_t i = 0; i < static_cast (k); ++i) + { + int& neighbor_index = k_indices[i]; + neighbor_index = index_mapping_[neighbor_index]; + } + } + + return (k); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::KdTreeFLANN::radiusSearch (const PointT &point, double radius, std::vector &k_indices, + std::vector &k_sqr_dists, unsigned int max_nn) const +{ + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); + + std::vector query (dim_); + point_representation_->vectorize (static_cast (point), query); + + // Has max_nn been set properly? + if (max_nn == 0 || max_nn > static_cast (total_nr_points_)) + max_nn = total_nr_points_; + + std::vector > indices(1); + std::vector > dists(1); + + ::flann::SearchParams params (param_radius_); + if (max_nn == static_cast(total_nr_points_)) + params.max_neighbors = -1; // return all neighbors in radius + else + params.max_neighbors = max_nn; + + int neighbors_in_radius = flann_index_->radiusSearch (::flann::Matrix (&query[0], 1, dim_), + indices, + dists, + static_cast (radius * radius), + params); + + k_indices = indices[0]; + k_sqr_dists = dists[0]; + + // Do mapping to original point cloud + if (!identity_mapping_) + { + for (int i = 0; i < neighbors_in_radius; ++i) + { + int& neighbor_index = k_indices[i]; + neighbor_index = index_mapping_[neighbor_index]; + } + } + + return (neighbors_in_radius); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeFLANN::cleanup () +{ + // Data array cleanup + index_mapping_.clear (); + + if (indices_) + indices_.reset (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeFLANN::convertCloudToArray (const PointCloud &cloud) +{ + // No point in doing anything if the array is empty + if (cloud.points.empty ()) + { + cloud_.reset (); + return; + } + + int original_no_of_points = static_cast (cloud.points.size ()); + + cloud_.reset (new float[original_no_of_points * dim_], std::default_delete ()); + float* cloud_ptr = cloud_.get (); + index_mapping_.reserve (original_no_of_points); + identity_mapping_ = true; + + for (int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index) + { + // Check if the point is invalid + if (!point_representation_->isValid (cloud.points[cloud_index])) + { + identity_mapping_ = false; + continue; + } + + index_mapping_.push_back (cloud_index); + + point_representation_->vectorize (cloud.points[cloud_index], cloud_ptr); + cloud_ptr += dim_; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeFLANN::convertCloudToArray (const PointCloud &cloud, const std::vector &indices) +{ + // No point in doing anything if the array is empty + if (cloud.points.empty ()) + { + cloud_.reset (); + return; + } + + int original_no_of_points = static_cast (indices.size ()); + + cloud_.reset (new float[original_no_of_points * dim_], std::default_delete ()); + float* cloud_ptr = cloud_.get (); + index_mapping_.reserve (original_no_of_points); + // its a subcloud -> false + // true only identity: + // - indices size equals cloud size + // - indices only contain values between 0 and cloud.size - 1 + // - no index is multiple times in the list + // => index is complete + // But we can not guarantee that => identity_mapping_ = false + identity_mapping_ = false; + + for (const int &index : indices) + { + // Check if the point is invalid + if (!point_representation_->isValid (cloud.points[index])) + continue; + + // map from 0 - N -> indices [0] - indices [N] + index_mapping_.push_back (index); // If the returned index should be for the indices vector + + point_representation_->vectorize (cloud.points[index], cloud_ptr); + cloud_ptr += dim_; + } +} + +#define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN; + +#endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/kdtree/io.h b/deps_install/include/pcl-1.10/pcl/kdtree/io.h new file mode 100755 index 0000000..a3ebb21 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/kdtree/io.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: io.h 2413 2011-09-07 07:01:00Z rusu $ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Get a set of approximate indices for a given point cloud into a reference point cloud. + * The coordinates of the two point clouds can differ. The method uses an internal KdTree for + * finding the closest neighbors from \a cloud_in in \a cloud_ref. + * + * \param[in] cloud_in the input point cloud dataset + * \param[in] cloud_ref the reference point cloud dataset + * \param[out] indices the resultant set of nearest neighbor indices of \a cloud_in in \a cloud_ref + * \ingroup kdtree + */ + template void + getApproximateIndices (const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, + std::vector &indices); + + /** \brief Get a set of approximate indices for a given point cloud into a reference point cloud. + * The coordinates of the two point clouds can differ. The method uses an internal KdTree for + * finding the closest neighbors from \a cloud_in in \a cloud_ref. + * + * \param[in] cloud_in the input point cloud dataset + * \param[in] cloud_ref the reference point cloud dataset + * \param[out] indices the resultant set of nearest neighbor indices of \a cloud_in in \a cloud_ref + * \ingroup kdtree + */ + template void + getApproximateIndices (const typename pcl::PointCloud::ConstPtr &cloud_in, + const typename pcl::PointCloud::ConstPtr &cloud_ref, + std::vector &indices); +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/kdtree/kdtree.h b/deps_install/include/pcl-1.10/pcl/kdtree/kdtree.h new file mode 100755 index 0000000..9161873 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/kdtree/kdtree.h @@ -0,0 +1,357 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief KdTree represents the base spatial locator class for kd-tree implementations. + * \author Radu B Rusu, Bastian Steder, Michael Dixon + * \ingroup kdtree + */ + template + class KdTree + { + public: + using IndicesPtr = shared_ptr >; + using IndicesConstPtr = shared_ptr >; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointRepresentation = pcl::PointRepresentation; + using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; + + // Boost shared pointers + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor for KdTree. Sets some internal values to their defaults. + * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. + */ + KdTree (bool sorted = true) : input_(), + epsilon_(0.0f), min_pts_(1), sorted_(sorted), + point_representation_ (new DefaultPointRepresentation) + { + }; + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used + */ + virtual void + setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) + { + input_ = cloud; + indices_ = indices; + } + + /** \brief Get a pointer to the vector of indices used. */ + inline IndicesConstPtr + getIndices () const + { + return (indices_); + } + + /** \brief Get a pointer to the input point cloud dataset. */ + inline PointCloudConstPtr + getInputCloud () const + { + return (input_); + } + + /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. + * \param[in] point_representation the const boost shared pointer to a PointRepresentation + */ + inline void + setPointRepresentation (const PointRepresentationConstPtr &point_representation) + { + point_representation_ = point_representation; + if (!input_) return; + setInputCloud (input_, indices_); // Makes sense in derived classes to reinitialize the tree + } + + /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */ + inline PointRepresentationConstPtr + getPointRepresentation () const + { + return (point_representation_); + } + + /** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */ + virtual ~KdTree () {}; + + /** \brief Search for k-nearest neighbors for the given query point. + * \param[in] p_q the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + virtual int + nearestKSearch (const PointT &p_q, int k, + std::vector &k_indices, std::vector &k_sqr_distances) const = 0; + + /** \brief Search for k-nearest neighbors for the given query point. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] cloud the point cloud data + * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * + * \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + nearestKSearch (const PointCloud &cloud, int index, int k, + std::vector &k_indices, std::vector &k_sqr_distances) const + { + assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!"); + return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances)); + } + + /** \brief Search for k-nearest neighbors for the given query point. + * This method accepts a different template parameter for the point type. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + template inline int + nearestKSearchT (const PointTDiff &point, int k, + std::vector &k_indices, std::vector &k_sqr_distances) const + { + PointT p; + copyPoint (point, p); + return (nearestKSearch (p, k, k_indices, k_sqr_distances)); + } + + /** \brief Search for k-nearest neighbors for the given query point (zero-copy). + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] index a \a valid index representing a \a valid query point in the dataset given + * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in + * the indices vector. + * + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + nearestKSearch (int index, int k, + std::vector &k_indices, std::vector &k_sqr_distances) const + { + if (indices_ == nullptr) + { + assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!"); + return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances)); + } + assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in nearestKSearch!"); + return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances)); + } + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] p_q the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + virtual int + radiusSearch (const PointT &p_q, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) const = 0; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] cloud the point cloud data + * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + radiusSearch (const PointCloud &cloud, int index, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn = 0) const + { + assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!"); + return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); + } + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + template inline int + radiusSearchT (const PointTDiff &point, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) const + { + PointT p; + copyPoint (point, p); + return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn)); + } + + /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy). + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] index a \a valid index representing a \a valid query point in the dataset given + * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in + * the indices vector. + * + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + radiusSearch (int index, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) const + { + if (indices_ == nullptr) + { + assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in radiusSearch!"); + return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn)); + } + assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in radiusSearch!"); + return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn)); + } + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. + * \param[in] eps precision (error bound) for nearest neighbors searches + */ + virtual inline void + setEpsilon (float eps) + { + epsilon_ = eps; + } + + /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ + inline float + getEpsilon () const + { + return (epsilon_); + } + + /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. + * \param[in] min_pts the minimum number of neighbors in a viable neighborhood + */ + inline void + setMinPts (int min_pts) + { + min_pts_ = min_pts; + } + + /** \brief Get the minimum allowed number of k nearest neighbors points that a viable result must contain. */ + inline int + getMinPts () const + { + return (min_pts_); + } + + protected: + /** \brief The input point cloud dataset containing the points we need to use. */ + PointCloudConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + IndicesConstPtr indices_; + + /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ + float epsilon_; + + /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. */ + int min_pts_; + + /** \brief Return the radius search neighbours sorted **/ + bool sorted_; + + /** \brief For converting different point structures into k-dimensional vectors for nearest-neighbor search. */ + PointRepresentationConstPtr point_representation_; + + /** \brief Class getName method. */ + virtual std::string + getName () const = 0; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/kdtree/kdtree_flann.h b/deps_install/include/pcl-1.10/pcl/kdtree/kdtree_flann.h new file mode 100755 index 0000000..a72a0e8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/kdtree/kdtree_flann.h @@ -0,0 +1,237 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: kdtree_flann.h 36261 2011-02-26 01:34:42Z mariusm $ + * + */ + +#pragma once + +#include +#include + +#include + +// Forward declarations +namespace flann +{ + template struct L2_Simple; + template class Index; +} + +namespace pcl +{ + // Forward declarations + template class PointRepresentation; + + /** \brief KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures. The class is making use of + * the FLANN (Fast Library for Approximate Nearest Neighbor) project by Marius Muja and David Lowe. + * + * \author Radu B. Rusu, Marius Muja + * \ingroup kdtree + */ + template > + class KdTreeFLANN : public pcl::KdTree + { + public: + using KdTree::input_; + using KdTree::indices_; + using KdTree::epsilon_; + using KdTree::sorted_; + using KdTree::point_representation_; + using KdTree::nearestKSearch; + using KdTree::radiusSearch; + + using PointCloud = typename KdTree::PointCloud; + using PointCloudConstPtr = typename KdTree::PointCloudConstPtr; + + using IndicesPtr = shared_ptr >; + using IndicesConstPtr = shared_ptr >; + + using FLANNIndex = ::flann::Index; + + // Boost shared pointers + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Default Constructor for KdTreeFLANN. + * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. + * + * By setting sorted to false, the \ref radiusSearch operations will be faster. + */ + KdTreeFLANN (bool sorted = true); + + /** \brief Copy constructor + * \param[in] k the tree to copy into this + */ + KdTreeFLANN (const KdTreeFLANN &k); + + /** \brief Copy operator + * \param[in] k the tree to copy into this + */ + inline KdTreeFLANN& + operator = (const KdTreeFLANN& k) + { + KdTree::operator=(k); + flann_index_ = k.flann_index_; + cloud_ = k.cloud_; + index_mapping_ = k.index_mapping_; + identity_mapping_ = k.identity_mapping_; + dim_ = k.dim_; + total_nr_points_ = k.total_nr_points_; + param_k_ = k.param_k_; + param_radius_ = k.param_radius_; + return (*this); + } + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. + * \param[in] eps precision (error bound) for nearest neighbors searches + */ + void + setEpsilon (float eps) override; + + void + setSortedResults (bool sorted); + + inline Ptr makeShared () { return Ptr (new KdTreeFLANN (*this)); } + + /** \brief Destructor for KdTreeFLANN. + * Deletes all allocated data arrays and destroys the kd-tree structures. + */ + ~KdTreeFLANN () + { + cleanup (); + } + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used + */ + void + setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) override; + + /** \brief Search for k-nearest neighbors for the given query point. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] point a given \a valid (i.e., finite) query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + int + nearestKSearch (const PointT &point, int k, + std::vector &k_indices, std::vector &k_sqr_distances) const override; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] point a given \a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + int + radiusSearch (const PointT &point, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) const override; + + private: + /** \brief Internal cleanup method. */ + void + cleanup (); + + /** \brief Converts a PointCloud to the internal FLANN point array representation. Returns the number + * of points. + * \param cloud the PointCloud + */ + void + convertCloudToArray (const PointCloud &cloud); + + /** \brief Converts a PointCloud with a given set of indices to the internal FLANN point array + * representation. Returns the number of points. + * \param[in] cloud the PointCloud data + * \param[in] indices the point cloud indices + */ + void + convertCloudToArray (const PointCloud &cloud, const std::vector &indices); + + private: + /** \brief Class getName method. */ + std::string + getName () const override { return ("KdTreeFLANN"); } + + /** \brief A FLANN index object. */ + std::shared_ptr flann_index_; + + /** \brief Internal pointer to data. TODO: replace with std::shared_ptr with C++17*/ + std::shared_ptr cloud_; + + /** \brief mapping between internal and external indices. */ + std::vector index_mapping_; + + /** \brief whether the mapping between internal and external indices is identity */ + bool identity_mapping_; + + /** \brief Tree dimensionality (i.e. the number of dimensions per point). */ + int dim_; + + /** \brief The total size of the data (either equal to the number of points in the input cloud or to the number of indices - if passed). */ + int total_nr_points_; + + /** \brief The KdTree search parameters for K-nearest neighbors. */ + ::flann::SearchParams param_k_; + + /** \brief The KdTree search parameters for radius search. */ + ::flann::SearchParams param_radius_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/agast_2d.h b/deps_install/include/pcl-1.10/pcl/keypoints/agast_2d.h new file mode 100755 index 0000000..159272d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/agast_2d.h @@ -0,0 +1,806 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include + +namespace pcl +{ + namespace keypoints + { + namespace agast + { + + /** \brief Abstract detector class for AGAST corner point detectors. + * + * Adapted from the C++ implementation of Elmar Mair + * (http://www6.in.tum.de/Main/ResearchAgast). + * + * \author Stefan Holzer + * \ingroup keypoints + */ + class PCL_EXPORTS AbstractAgastDetector + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Constructor. + * \param[in] width the width of the image to process + * \param[in] height the height of the image to process + * \param[in] threshold the corner detection threshold + * \param[in] bmax the max image value (default: 255) + */ + AbstractAgastDetector (const std::size_t width, + const std::size_t height, + const double threshold, + const double bmax) + : width_ (width) + , height_ (height) + , threshold_ (threshold) + , nr_max_keypoints_ (std::numeric_limits::max ()) + , bmax_ (bmax) + {} + + /** \brief Destructor. */ + virtual ~AbstractAgastDetector () {} + + /** \brief Detects corner points. + * \param intensity_data + * \param output + */ + void + detectKeypoints (const std::vector &intensity_data, + pcl::PointCloud &output); + + /** \brief Detects corner points. + * \param intensity_data + * \param output + */ + void + detectKeypoints (const std::vector &intensity_data, + pcl::PointCloud &output); + + /** \brief Applies non-max-suppression. + * \param[in] intensity_data the image data + * \param[in] input the keypoint positions + * \param[out] output the resultant keypoints after non-max-supression + */ + void + applyNonMaxSuppression (const std::vector& intensity_data, + const pcl::PointCloud &input, + pcl::PointCloud &output); + + /** \brief Applies non-max-suppression. + * \param[in] intensity_data the image data + * \param[in] input the keypoint positions + * \param[out] output the resultant keypoints after non-max-supression + */ + void + applyNonMaxSuppression (const std::vector& intensity_data, + const pcl::PointCloud &input, + pcl::PointCloud &output); + + /** \brief Computes corner score. + * \param[in] im the pixels to compute the score at + */ + virtual int + computeCornerScore (const unsigned char* im) const = 0; + + /** \brief Computes corner score. + * \param[in] im the pixels to compute the score at + */ + virtual int + computeCornerScore (const float* im) const = 0; + + /** \brief Sets the threshold for corner detection. + * \param[in] threshold the threshold used for corner detection. + */ + inline void + setThreshold (const double threshold) + { + threshold_ = threshold; + } + + /** \brief Get the threshold for corner detection, as set by the user. */ + inline double + getThreshold () + { + return (threshold_); + } + + /** \brief Sets the maximum number of keypoints to return. The + * estimated keypoints are sorted by their internal score. + * \param[in] nr_max_keypoints set the maximum number of keypoints to return + */ + inline void + setMaxKeypoints (const unsigned int nr_max_keypoints) + { + nr_max_keypoints_ = nr_max_keypoints; + } + + /** \brief Get the maximum number of keypoints to return, as set by the user. */ + inline unsigned int + getMaxKeypoints () + { + return (nr_max_keypoints_); + } + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + * \param[out] corners_all the resultant set of keypoints detected + */ + virtual void + detect (const unsigned char* im, + std::vector > &corners_all) const = 0; + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + */ + virtual void + detect (const float* im, + std::vector > &) const = 0; + + protected: + + /** \brief Structure holding an index and the associated keypoint score. */ + struct ScoreIndex + { + int idx; + int score; + }; + + /** \brief Score index comparator. */ + struct CompareScoreIndex + { + /** \brief Comparator + * \param[in] i1 the first score index + * \param[in] i2 the second score index + */ + inline bool + operator() (const ScoreIndex &i1, const ScoreIndex &i2) + { + return (i1.score > i2.score); + } + }; + + /** \brief Initializes the sample pattern. */ + virtual void + initPattern () = 0; + + /** \brief Non-max-suppression helper method. + * \param[in] input the keypoint positions + * \param[in] scores the keypoint scores computed on the image data + * \param[out] output the resultant keypoints after non-max-supression + */ + void + applyNonMaxSuppression (const pcl::PointCloud &input, + const std::vector& scores, + pcl::PointCloud &output); + + /** \brief Computes corner scores for the specified points. + * \param im + * \param corners_all + * \param scores + */ + void + computeCornerScores (const unsigned char* im, + const std::vector > & corners_all, + std::vector & scores); + + /** \brief Computes corner scores for the specified points. + * \param im + * \param corners_all + * \param scores + */ + void + computeCornerScores (const float* im, + const std::vector > & corners_all, + std::vector & scores); + + /** \brief Width of the image to process. */ + std::size_t width_; + /** \brief Height of the image to process. */ + std::size_t height_; + + /** \brief Threshold for corner detection. */ + double threshold_; + + /** \brief The maximum number of keypoints to return. */ + unsigned int nr_max_keypoints_; + + /** \brief Max image value. */ + double bmax_; + }; + + /** \brief Detector class for AGAST corner point detector (7_12s). + * + * Adapted from the C++ implementation of Elmar Mair + * (http://www6.in.tum.de/Main/ResearchAgast). + * + * \author Stefan Holzer + * \ingroup keypoints + */ + class PCL_EXPORTS AgastDetector7_12s : public AbstractAgastDetector + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Constructor. + * \param[in] width the width of the image to process + * \param[in] height the height of the image to process + * \param[in] threshold the corner detection threshold + * \param[in] bmax the max image value (default: 255) + */ + AgastDetector7_12s (const std::size_t width, + const std::size_t height, + const double threshold, + const double bmax = 255) + : AbstractAgastDetector (width, height, threshold, bmax) + { + initPattern (); + } + + /** \brief Destructor. */ + ~AgastDetector7_12s () {} + + /** \brief Computes corner score. + * \param im + */ + int + computeCornerScore (const unsigned char* im) const override; + + /** \brief Computes corner score. + * \param im + */ + int + computeCornerScore (const float* im) const override; + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + * \param[out] corners_all the resultant set of keypoints detected + */ + void + detect (const unsigned char* im, std::vector > &corners_all) const override; + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + * \param[out] corners_all the resultant set of keypoints detected + */ + void + detect (const float* im, std::vector > &corners_all) const override; + + protected: + /** \brief Initializes the sample pattern. */ + void + initPattern () override; + + private: + /** \brief Border width. */ + static const int border_width_ = 2; + + // offsets defining the sample pattern + std::array offset_; + }; + + /** \brief Detector class for AGAST corner point detector (5_8). + * + * Adapted from the C++ implementation of Elmar Mair + * (http://www6.in.tum.de/Main/ResearchAgast). + * + * \author Stefan Holzer + * \ingroup keypoints + */ + class PCL_EXPORTS AgastDetector5_8 : public AbstractAgastDetector + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Constructor. + * \param[in] width the width of the image to process + * \param[in] height the height of the image to process + * \param[in] threshold the corner detection threshold + * \param[in] bmax the max image value (default: 255) + */ + AgastDetector5_8 (const std::size_t width, + const std::size_t height, + const double threshold, + const double bmax = 255) + : AbstractAgastDetector (width, height, threshold, bmax) + { + initPattern (); + } + + /** \brief Destructor. */ + ~AgastDetector5_8 () {} + + /** \brief Computes corner score. + * \param im + */ + int + computeCornerScore (const unsigned char* im) const override; + + /** \brief Computes corner score. + * \param im + */ + int + computeCornerScore (const float* im) const override; + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + * \param[out] corners_all the resultant set of keypoints detected + */ + void + detect (const unsigned char* im, std::vector > &corners_all) const override; + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + * \param[out] corners_all the resultant set of keypoints detected + */ + void + detect (const float* im, std::vector > &corners_all) const override; + + protected: + /** \brief Initializes the sample pattern. */ + void + initPattern () override; + + private: + /** \brief Border width. */ + static const int border_width_ = 1; + + // offsets defining the sample pattern + std::array offset_; + }; + + /** \brief Detector class for AGAST corner point detector (OAST 9_16). + * + * Adapted from the C++ implementation of Elmar Mair + * (http://www6.in.tum.de/Main/ResearchAgast). + * + * \author Stefan Holzer + * \ingroup keypoints + */ + class PCL_EXPORTS OastDetector9_16 : public AbstractAgastDetector + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Constructor. + * \param[in] width the width of the image to process + * \param[in] height the height of the image to process + * \param[in] threshold the corner detection threshold + * \param[in] bmax the max image value (default: 255) + */ + OastDetector9_16 (const std::size_t width, + const std::size_t height, + const double threshold, + const double bmax = 255) + : AbstractAgastDetector (width, height, threshold, bmax) + { + initPattern (); + } + + /** \brief Destructor. */ + ~OastDetector9_16 () {} + + /** \brief Computes corner score. + * \param im + */ + int + computeCornerScore (const unsigned char* im) const override; + + /** \brief Computes corner score. + * \param im + */ + int + computeCornerScore (const float* im) const override; + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + * \param[out] corners_all the resultant set of keypoints detected + */ + void + detect (const unsigned char* im, std::vector > &corners_all) const override; + + /** \brief Detects points of interest (i.e., keypoints) in the given image + * \param[in] im the image to detect keypoints in + * \param[out] corners_all the resultant set of keypoints detected + */ + void + detect (const float* im, std::vector > &corners_all) const override; + + protected: + /** \brief Initializes the sample pattern. */ + void + initPattern () override; + + private: + /** \brief Border width. */ + static const int border_width_ = 3; + + // offsets defining the sample pattern + std::array offset_; + }; + } // namespace agast + } // namespace keypoints + + ///////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////// + namespace keypoints + { + namespace internal + { + ///////////////////////////////////////////////////////////////////////////////////// + template + struct AgastApplyNonMaxSuppresion + { + AgastApplyNonMaxSuppresion ( + const std::vector &image_data, + const pcl::PointCloud &tmp_cloud, + const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector, + pcl::PointCloud &output) + { + pcl::PointCloud output_temp; + detector->applyNonMaxSuppression (image_data, tmp_cloud, output_temp); + pcl::copyPointCloud (output_temp, output); + } + }; + + ///////////////////////////////////////////////////////////////////////////////////// + template <> + struct AgastApplyNonMaxSuppresion + { + AgastApplyNonMaxSuppresion ( + const std::vector &image_data, + const pcl::PointCloud &tmp_cloud, + const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector, + pcl::PointCloud &output) + { + detector->applyNonMaxSuppression (image_data, tmp_cloud, output); + } + }; + ///////////////////////////////////////////////////////////////////////////////////// + template + struct AgastDetector + { + AgastDetector ( + const std::vector &image_data, + const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector, + pcl::PointCloud &output) + { + pcl::PointCloud output_temp; + detector->detectKeypoints (image_data, output_temp); + pcl::copyPointCloud (output_temp, output); + } + }; + + ///////////////////////////////////////////////////////////////////////////////////// + template <> + struct AgastDetector + { + AgastDetector ( + const std::vector &image_data, + const pcl::keypoints::agast::AbstractAgastDetector::Ptr &detector, + pcl::PointCloud &output) + { + detector->detectKeypoints (image_data, output); + } + }; + } // namespace agast + } // namespace keypoints + + ///////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////////////////// + /** \brief Detects 2D AGAST corner points. Based on the original work and + * paper reference by + * + * \par + * Elmar Mair, Gregory D. Hager, Darius Burschka, Michael Suppa, and Gerhard Hirzinger. + * Adaptive and generic corner detection based on the accelerated segment test. + * In Proceedings of the European Conference on Computer Vision (ECCV'10), September 2010. + * + * \note This is an abstract base class. All children must implement a detectKeypoints method, based on the type of AGAST keypoint to be used. + * + * \author Stefan Holzer, Radu B. Rusu + * \ingroup keypoints + */ + template > + class AgastKeypoint2DBase : public Keypoint + { + public: + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using KdTree = typename Keypoint::KdTree; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using AgastDetectorPtr = pcl::keypoints::agast::AbstractAgastDetector::Ptr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::k_; + + /** \brief Constructor */ + AgastKeypoint2DBase () + : threshold_ (10) + , apply_non_max_suppression_ (true) + , bmax_ (255) + , nr_max_keypoints_ (std::numeric_limits::max ()) + { + k_ = 1; + } + + /** \brief Destructor. */ + ~AgastKeypoint2DBase () + { + } + + /** \brief Sets the threshold for corner detection. + * \param[in] threshold the threshold used for corner detection. + */ + inline void + setThreshold (const double threshold) + { + threshold_ = threshold; + } + + /** \brief Get the threshold for corner detection, as set by the user. */ + inline double + getThreshold () + { + return (threshold_); + } + + /** \brief Sets the maximum number of keypoints to return. The + * estimated keypoints are sorted by their internal score. + * \param[in] nr_max_keypoints set the maximum number of keypoints to return + */ + inline void + setMaxKeypoints (const unsigned int nr_max_keypoints) + { + nr_max_keypoints_ = nr_max_keypoints; + } + + /** \brief Get the maximum number of keypoints to return, as set by the user. */ + inline unsigned int + getMaxKeypoints () + { + return (nr_max_keypoints_); + } + + /** \brief Sets the max image data value (affects how many iterations AGAST does) + * \param[in] bmax the max image data value + */ + inline void + setMaxDataValue (const double bmax) + { + bmax_ = bmax; + } + + /** \brief Get the bmax image value, as set by the user. */ + inline double + getMaxDataValue () + { + return (bmax_); + } + + /** \brief Sets whether non-max-suppression is applied or not. + * \param[in] enabled determines whether non-max-suppression is enabled. + */ + inline void + setNonMaxSuppression (const bool enabled) + { + apply_non_max_suppression_ = enabled; + } + + /** \brief Returns whether non-max-suppression is applied or not. */ + inline bool + getNonMaxSuppression () + { + return (apply_non_max_suppression_); + } + + inline void + setAgastDetector (const AgastDetectorPtr &detector) + { + detector_ = detector; + } + + inline AgastDetectorPtr + getAgastDetector () + { + return (detector_); + } + protected: + + /** \brief Initializes everything and checks whether input data is fine. */ + bool + initCompute () override; + + /** \brief Detects the keypoints. + * \param[out] output the resultant keypoints + */ + void + detectKeypoints (PointCloudOut &output) override = 0; + + /** \brief Intensity field accessor. */ + IntensityT intensity_; + + /** \brief Threshold for corner detection. */ + double threshold_; + + /** \brief Determines whether non-max-suppression is activated. */ + bool apply_non_max_suppression_; + + /** \brief Max image value. */ + double bmax_; + + /** \brief The Agast detector to use. */ + AgastDetectorPtr detector_; + + /** \brief The maximum number of keypoints to return. */ + unsigned int nr_max_keypoints_; + }; + + /** \brief Detects 2D AGAST corner points. Based on the original work and + * paper reference by + * + * \par + * Elmar Mair, Gregory D. Hager, Darius Burschka, Michael Suppa, and Gerhard Hirzinger. + * Adaptive and generic corner detection based on the accelerated segment test. + * In Proceedings of the European Conference on Computer Vision (ECCV'10), September 2010. + * + * Code example: + * + * \code + * pcl::PointCloud cloud; + * pcl::AgastKeypoint2D agast; + * agast.setThreshold (30); + * agast.setInputCloud (cloud); + * + * PointCloud keypoints; + * agast.compute (keypoints); + * \endcode + * + * \note The AGAST keypoint type used is 7_12s. + * + * \author Stefan Holzer, Radu B. Rusu + * \ingroup keypoints + */ + template + class AgastKeypoint2D : public AgastKeypoint2DBase > + { + public: + using PointCloudOut = typename Keypoint::PointCloudOut; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::k_; + using AgastKeypoint2DBase >::intensity_; + using AgastKeypoint2DBase >::threshold_; + using AgastKeypoint2DBase >::bmax_; + using AgastKeypoint2DBase >::apply_non_max_suppression_; + using AgastKeypoint2DBase >::detector_; + using AgastKeypoint2DBase >::nr_max_keypoints_; + + /** \brief Constructor */ + AgastKeypoint2D () + { + name_ = "AgastKeypoint2D"; + } + + /** \brief Destructor. */ + ~AgastKeypoint2D () + { + } + + protected: + /** \brief Detects the keypoints. + * \param[out] output the resultant keypoints + */ + void + detectKeypoints (PointCloudOut &output) override; + }; + + /** \brief Detects 2D AGAST corner points. Based on the original work and + * paper reference by + * + * \par + * Elmar Mair, Gregory D. Hager, Darius Burschka, Michael Suppa, and Gerhard Hirzinger. + * Adaptive and generic corner detection based on the accelerated segment test. + * In Proceedings of the European Conference on Computer Vision (ECCV'10), September 2010. + * + * Code example: + * + * \code + * pcl::PointCloud cloud; + * pcl::AgastKeypoint2D agast; + * agast.setThreshold (30); + * agast.setInputCloud (cloud); + * + * PointCloud keypoints; + * agast.compute (keypoints); + * \endcode + * + * \note This is a specialized version for PointXYZ clouds, and operates on depth (z) as float. The output keypoints are of the PointXY type. + * \note The AGAST keypoint type used is 7_12s. + * + * \author Stefan Holzer, Radu B. Rusu + * \ingroup keypoints + */ + template <> + class AgastKeypoint2D + : public AgastKeypoint2DBase > + { + public: + /** \brief Constructor */ + AgastKeypoint2D () + { + name_ = "AgastKeypoint2D"; + bmax_ = 4; // max data value for an OpenNI camera + } + + /** \brief Destructor. */ + ~AgastKeypoint2D () + { + } + + protected: + /** \brief Detects the keypoints. + * \param[out] output the resultant keypoints + */ + void + detectKeypoints (pcl::PointCloud &output) override; + }; + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/brisk_2d.h b/deps_install/include/pcl-1.10/pcl/keypoints/brisk_2d.h new file mode 100755 index 0000000..0d4412a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/brisk_2d.h @@ -0,0 +1,484 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (C) 2011, The Autonomous Systems Lab (ASL), ETH Zurich, + * Stefan Leutenegger, Simon Lynen and Margarita Chli. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Detects BRISK interest points based on the original code and paper + * reference by + * + * \par + * Stefan Leutenegger,Margarita Chli and Roland Siegwart, + * BRISK: Binary Robust Invariant Scalable Keypoints, + * in Proceedings of the IEEE International Conference on Computer Vision (ICCV2011). + * + * Code example: + * + * \code + * pcl::PointCloud cloud; + * pcl::BriskKeypoint2D brisk; + * brisk.setThreshold (60); + * brisk.setOctaves (4); + * brisk.setInputCloud (cloud); + * + * PointCloud keypoints; + * brisk.compute (keypoints); + * \endcode + * + * \author Radu B. Rusu, Stefan Holzer + * \ingroup keypoints + */ + template > + class BriskKeypoint2D: public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using KdTree = typename Keypoint::KdTree; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::k_; + + /** \brief Constructor */ + BriskKeypoint2D (int octaves = 4, int threshold = 60) + : threshold_ (threshold) + , octaves_ (octaves) + , remove_invalid_3D_keypoints_ (false) + { + k_ = 1; + name_ = "BriskKeypoint2D"; + } + + /** \brief Destructor. */ + ~BriskKeypoint2D () + { + } + + /** \brief Sets the threshold for corner detection. + * \param[in] threshold the threshold used for corner detection. + */ + inline void + setThreshold (const int threshold) + { + threshold_ = threshold; + } + + /** \brief Get the threshold for corner detection, as set by the user. */ + inline std::size_t + getThreshold () + { + return (threshold_); + } + + /** \brief Set the number of octaves to use + * \param[in] octaves the number of octaves to use + */ + inline void + setOctaves (const int octaves) + { + octaves_ = octaves; + } + + /** \brief Returns the number of octaves used. */ + inline int + getOctaves () + { + return (octaves_); + } + + /** \brief Specify whether we should do a 2nd pass through the list of keypoints + * found, and remove the ones that do not have a valid 3D (x-y-z) position + * (i.e., are NaN or Inf). + * \param[in] remove set to true whether we want the invalid 3D keypoints removed + */ + inline void + setRemoveInvalid3DKeypoints (bool remove) + { + remove_invalid_3D_keypoints_ = remove; + } + + /** \brief Specify whether the keypoints that do not have a valid 3D position are + * kept (false) or removed (true). + */ + inline bool + getRemoveInvalid3DKeypoints () + { + return (remove_invalid_3D_keypoints_); + } + + ///////////////////////////////////////////////////////////////////////// + inline void + bilinearInterpolation (const PointCloudInConstPtr &cloud, + float x, float y, + PointOutT &pt) + { + int u = int (x); + int v = int (y); + + pt.x = pt.y = pt.z = 0; + + const PointInT &p1 = (*cloud)(u, v); + const PointInT &p2 = (*cloud)(u+1, v); + const PointInT &p3 = (*cloud)(u, v+1); + const PointInT &p4 = (*cloud)(u+1, v+1); + + float fx = x - float (u), fy = y - float (v); + float fx1 = 1.0f - fx, fy1 = 1.0f - fy; + + float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy; + float weight = 0; + + if (pcl::isFinite (p1)) + { + pt.x += p1.x * w1; + pt.y += p1.y * w1; + pt.z += p1.z * w1; + weight += w1; + } + if (pcl::isFinite (p2)) + { + pt.x += p2.x * w2; + pt.y += p2.y * w2; + pt.z += p2.z * w2; + weight += w2; + } + if (pcl::isFinite (p3)) + { + pt.x += p3.x * w3; + pt.y += p3.y * w3; + pt.z += p3.z * w3; + weight += w3; + } + if (pcl::isFinite (p4)) + { + pt.x += p4.x * w4; + pt.y += p4.y * w4; + pt.z += p4.z * w4; + weight += w4; + } + + if (weight == 0) + pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); + else + { + weight = 1.0f / weight; + pt.x *= weight; pt.y *= weight; pt.z *= weight; + } + } + + protected: + /** \brief Initializes everything and checks whether input data is fine. */ + bool + initCompute () override; + + /** \brief Detects the keypoints. */ + void + detectKeypoints (PointCloudOut &output) override; + + private: + /** \brief Intensity field accessor. */ + IntensityT intensity_; + + /** \brief Threshold for corner detection. */ + int threshold_; + + int octaves_; + + /** \brief Specify whether the keypoints that do not have a valid 3D position are + * kept (false) or removed (true). + */ + bool remove_invalid_3D_keypoints_; + }; + + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + namespace keypoints + { + namespace brisk + { + /** \brief A layer in the BRISK detector pyramid. */ + class PCL_EXPORTS Layer + { + public: + // constructor arguments + struct CommonParams + { + static const int HALFSAMPLE; + static const int TWOTHIRDSAMPLE; + }; + + /** \brief Constructor. + * \param[in] img input image + * \param[in] width image width + * \param[in] height image height + * \param[in] scale scale + * \param[in] offset offset + */ + Layer (const std::vector& img, + int width, int height, + float scale = 1.0f, float offset = 0.0f); + + /** \brief Copy constructor for deriving a layer. + * \param[in] layer layer to derive from + * \param[in] mode deriving mode + */ + Layer (const Layer& layer, int mode); + + /** \brief AGAST keypoints without non-max suppression. + * \param[in] threshold the keypoints threshold + * \param[out] keypoints the AGAST keypoints + */ + void + getAgastPoints (std::uint8_t threshold, std::vector > &keypoints); + + // get scores - attention, this is in layer coordinates, not scale=1 coordinates! + /** \brief Get the AGAST keypoint score for a given pixel using a threshold + * \param[in] x the U coordinate of the pixel + * \param[in] y the V coordinate of the pixel + * \param[in] threshold the threshold to use for cutting the response + */ + std::uint8_t + getAgastScore (int x, int y, std::uint8_t threshold); + /** \brief Get the AGAST keypoint score for a given pixel using a threshold + * \param[in] x the U coordinate of the pixel + * \param[in] y the V coordinate of the pixel + * \param[in] threshold the threshold to use for cutting the response + */ + std::uint8_t + getAgastScore_5_8 (int x, int y, std::uint8_t threshold); + /** \brief Get the AGAST keypoint score for a given pixel using a threshold + * \param[in] xf the X coordinate of the pixel + * \param[in] yf the Y coordinate of the pixel + * \param[in] threshold the threshold to use for cutting the response + * \param[in] scale the scale + */ + std::uint8_t + getAgastScore (float xf, float yf, std::uint8_t threshold, float scale = 1.0f); + + /** \brief Access gray values (smoothed/interpolated) + * \param[in] mat the image + * \param[in] width the image width + * \param[in] height the image height + * \param[in] xf the x coordinate + * \param[in] yf the y coordinate + * \param[in] scale the scale + */ + std::uint8_t + getValue (const std::vector& mat, + int width, int height, float xf, float yf, float scale); + + /** \brief Get the image used. */ + const std::vector& + getImage () const + { + return (img_); + } + + /** \brief Get the width of the image used. */ + int + getImageWidth () const + { + return (img_width_); + } + + /** \brief Get the height of the image used. */ + int + getImageHeight () const + { + return (img_height_); + } + + /** \brief Get the scale used. */ + float + getScale () const + { + return (scale_); + } + + /** \brief Get the offset used. */ + inline float + getOffset () const + { + return (offset_); + } + + /** \brief Get the scores obtained. */ + inline const std::vector& + getScores () const + { + return (scores_); + } + + private: + // half sampling + inline void + halfsample (const std::vector& srcimg, + int srcwidth, int srcheight, + std::vector& dstimg, + int dstwidth, int dstheight); + + // two third sampling + inline void + twothirdsample (const std::vector& srcimg, + int srcwidth, int srcheight, + std::vector& dstimg, + int dstwidth, int dstheight); + + /** the image */ + std::vector img_; + int img_width_; + int img_height_; + + /** its Fast scores */ + std::vector scores_; + + /** coordinate transformation */ + float scale_; + float offset_; + + /** agast */ + pcl::keypoints::agast::OastDetector9_16::Ptr oast_detector_; + pcl::keypoints::agast::AgastDetector5_8::Ptr agast_detector_5_8_; + }; + + /** BRISK Scale Space helper. */ + class PCL_EXPORTS ScaleSpace + { + public: + /** \brief Constructor. Specify the number of octaves. + * \param[in] octaves the number of octaves (default: 3) + */ + ScaleSpace (int octaves = 3); + ~ScaleSpace (); + + /** \brief Construct the image pyramids. + * \param[in] image the image to construct pyramids for + * \param[in] width the image width + * \param[in] height the image height + */ + void + constructPyramid (const std::vector& image, + int width, int height); + + /** \brief Get the keypoints for the associated image and threshold. + * \param[in] threshold the threshold for the keypoints + * \param[out] keypoints the resultant list of keypoints + */ + void + getKeypoints (const int threshold, + std::vector > &keypoints); + + protected: + /** Nonmax suppression. */ + inline bool + isMax2D (const std::uint8_t layer, const int x_layer, const int y_layer); + + /** 1D (scale axis) refinement: around octave */ + inline float + refine1D (const float s_05, const float s0, const float s05, float& max); + + /** 1D (scale axis) refinement: around intra */ + inline float + refine1D_1 (const float s_05, const float s0, const float s05, float& max); + + /** 1D (scale axis) refinement: around octave 0 only */ + inline float + refine1D_2 (const float s_05, const float s0, const float s05, float& max); + + /** 2D maximum refinement */ + inline float + subpixel2D (const int s_0_0, const int s_0_1, const int s_0_2, + const int s_1_0, const int s_1_1, const int s_1_2, + const int s_2_0, const int s_2_1, const int s_2_2, + float& delta_x, float& delta_y); + + /** 3D maximum refinement centered around (x_layer,y_layer) */ + inline float + refine3D (const std::uint8_t layer, + const int x_layer, const int y_layer, + float& x, float& y, float& scale, bool& ismax); + + /** interpolated score access with recalculation when needed */ + inline int + getScoreAbove (const std::uint8_t layer, const int x_layer, const int y_layer); + + inline int + getScoreBelow (const std::uint8_t layer, const int x_layer, const int y_layer); + + /** return the maximum of score patches above or below */ + inline float + getScoreMaxAbove (const std::uint8_t layer, + const int x_layer, const int y_layer, + const int threshold, bool& ismax, + float& dx, float& dy); + + inline float + getScoreMaxBelow (const std::uint8_t layer, + const int x_layer, const int y_layer, + const int threshold, bool& ismax, + float& dx, float& dy); + + // the image pyramids + std::uint8_t layers_; + std::vector pyramid_; + + // Agast + std::uint8_t threshold_; + std::uint8_t safe_threshold_; + + // some constant parameters + float safety_factor_; + float basic_size_; + }; + } // namespace brisk + } // namespace keypoints + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/harris_2d.h b/deps_install/include/pcl-1.10/pcl/keypoints/harris_2d.h new file mode 100755 index 0000000..2754e85 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/harris_2d.h @@ -0,0 +1,195 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief HarrisKeypoint2D detects Harris corners family points + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template > + class HarrisKeypoint2D : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using KdTree = typename Keypoint::KdTree; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::keypoints_indices_; + + enum ResponseMethod {HARRIS = 1, NOBLE, LOWE, TOMASI}; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param window_width + * \param window_height + * \param min_distance + * \param[in] threshold the threshold to filter out weak corners + */ + HarrisKeypoint2D (ResponseMethod method = HARRIS, int window_width = 3, int window_height = 3, int min_distance = 5, float threshold = 0.0) + : threshold_ (threshold) + , refine_ (false) + , nonmax_ (true) + , method_ (method) + , threads_ (0) + , response_ (new pcl::PointCloud ()) + , window_width_ (window_width) + , window_height_ (window_height) + , skipped_pixels_ (0) + , min_distance_ (min_distance) + { + name_ = "HarrisKeypoint2D"; + } + + /** \brief set the method of the response to be calculated. + * \param[in] type + */ + void setMethod (ResponseMethod type); + + ///Set window width + void setWindowWidth (int window_width); + + ///Set window height + void setWindowHeight (int window_height); + + ///Set number of pixels to skip + void setSkippedPixels (int skipped_pixels); + + ///Set minimal distance between candidate keypoints + void setMinimalDistance (int min_distance); + + /** \brief set the threshold value for detecting corners. This is only evaluated if non maxima suppression is turned on. + * \brief note non maxima suppression needs to be activated in order to use this feature. + * \param[in] threshold + */ + void setThreshold (float threshold); + + /** \brief whether non maxima suppression should be applied or the response for each point should be returned + * \note this value needs to be turned on in order to apply thresholding and refinement + * \param[in] nonmax default is false + */ + void setNonMaxSupression (bool = false); + + /** \brief whether the detected key points should be refined or not. If turned of, the key points are a subset of + * the original point cloud. Otherwise the key points may be arbitrary. + * \brief note non maxima supression needs to be on in order to use this feature. + * \param[in] do_refine + */ + void setRefine (bool do_refine); + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + protected: + bool + initCompute () override; + void + detectKeypoints (PointCloudOut &output) override; + /** \brief gets the corner response for valid input points*/ + void + responseHarris (PointCloudOut &output) const; + void + responseNoble (PointCloudOut &output) const; + void + responseLowe (PointCloudOut &output) const; + void + responseTomasi (PointCloudOut &output) const; +// void refineCorners (PointCloudOut &corners) const; + /** \brief calculates the upper triangular part of unnormalized + * covariance matrix over intensities given by the 2D coordinates + * and window_width_ and window_height_ + */ + void + computeSecondMomentMatrix (std::size_t pos, float* coefficients) const; + /// threshold for non maxima suppression + float threshold_; + /// corner refinement + bool refine_; + /// non maximas suppression + bool nonmax_; + /// cornerness computation method + ResponseMethod method_; + /// number of threads to be used + unsigned int threads_; + + private: + Eigen::MatrixXf derivatives_rows_; + Eigen::MatrixXf derivatives_cols_; + /// intermediate holder for computed responses + typename pcl::PointCloud::Ptr response_; + /// comparator for responses intensity + bool + greaterIntensityAtIndices (int a, int b) const + { + return (response_->at (a).intensity > response_->at (b).intensity); + } + /// Window width + int window_width_; + /// Window height + int window_height_; + /// half window width + int half_window_width_; + /// half window height + int half_window_height_; + /// number of pixels to skip within search window + int skipped_pixels_; + /// minimum distance between two keypoints + int min_distance_; + /// intensity field accessor + IntensityT intensity_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/harris_3d.h b/deps_install/include/pcl-1.10/pcl/keypoints/harris_3d.h new file mode 100755 index 0000000..c406d30 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/harris_3d.h @@ -0,0 +1,182 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief HarrisKeypoint3D uses the idea of 2D Harris keypoints, but instead of using image gradients, it uses + * surface normals. + * + * \author Suat Gedikli + * \ingroup keypoints + */ + template + class HarrisKeypoint3D : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using KdTree = typename Keypoint::KdTree; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::surface_; + using Keypoint::tree_; + using Keypoint::k_; + using Keypoint::search_radius_; + using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; + using Keypoint::initCompute; + using PCLBase::setInputCloud; + + enum ResponseMethod {HARRIS = 1, NOBLE, LOWE, TOMASI, CURVATURE}; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param[in] radius the radius for normal estimation as well as for non maxima suppression + * \param[in] threshold the threshold to filter out weak corners + */ + HarrisKeypoint3D (ResponseMethod method = HARRIS, float radius = 0.01f, float threshold = 0.0f) + : threshold_ (threshold) + , refine_ (true) + , nonmax_ (true) + , method_ (method) + , threads_ (0) + { + name_ = "HarrisKeypoint3D"; + search_radius_ = radius; + } + + /** \brief Empty destructor */ + ~HarrisKeypoint3D () {} + + /** \brief Provide a pointer to the input dataset + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + void + setInputCloud (const PointCloudInConstPtr &cloud) override; + + /** \brief Set the method of the response to be calculated. + * \param[in] type + */ + void + setMethod (ResponseMethod type); + + /** \brief Set the radius for normal estimation and non maxima supression. + * \param[in] radius + */ + void + setRadius (float radius); + + /** \brief Set the threshold value for detecting corners. This is only evaluated if non maxima suppression is turned on. + * \brief note non maxima suppression needs to be activated in order to use this feature. + * \param[in] threshold + */ + void + setThreshold (float threshold); + + /** \brief Whether non maxima suppression should be applied or the response for each point should be returned + * \note this value needs to be turned on in order to apply thresholding and refinement + * \param[in] nonmax default is false + */ + void + setNonMaxSupression (bool = false); + + /** \brief Whether the detected key points should be refined or not. If turned of, the key points are a subset of the original point cloud. Otherwise the key points may be arbitrary. + * \brief note non maxima supression needs to be on in order to use this feature. + * \param[in] do_refine + */ + void + setRefine (bool do_refine); + + /** \brief Set normals if precalculated normals are available. + * \param normals + */ + void + setNormals (const PointCloudNConstPtr &normals); + + /** \brief Provide a pointer to a dataset to add additional information + * to estimate the features for every point in the input dataset. This + * is optional, if this is not set, it will only use the data in the + * input cloud to estimate the features. This is useful when you only + * need to compute the features for a downsampled cloud. + * \param[in] cloud a pointer to a PointCloud message + */ + void + setSearchSurface (const PointCloudInConstPtr &cloud) override { surface_ = cloud; normals_.reset(); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + protected: + bool + initCompute () override; + void detectKeypoints (PointCloudOut &output) override; + /** \brief gets the corner response for valid input points*/ + void responseHarris (PointCloudOut &output) const; + void responseNoble (PointCloudOut &output) const; + void responseLowe (PointCloudOut &output) const; + void responseTomasi (PointCloudOut &output) const; + void responseCurvature (PointCloudOut &output) const; + void refineCorners (PointCloudOut &corners) const; + /** \brief calculates the upper triangular part of unnormalized covariance matrix over the normals given by the indices.*/ + void calculateNormalCovar (const std::vector& neighbors, float* coefficients) const; + private: + float threshold_; + bool refine_; + bool nonmax_; + ResponseMethod method_; + PointCloudNConstPtr normals_; + unsigned int threads_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/harris_6d.h b/deps_install/include/pcl-1.10/pcl/keypoints/harris_6d.h new file mode 100755 index 0000000..838fe5f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/harris_6d.h @@ -0,0 +1,140 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @author Suat Gedikli + */ + +#pragma once + +#include + +namespace pcl +{ + + /** \brief Keypoint detector for detecting corners in 3D (XYZ), 2D (intensity) AND mixed versions of these. + * \author Suat Gedikli + * \ingroup keypoints + */ + template + class HarrisKeypoint6D : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using KdTree = typename Keypoint::KdTree; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::surface_; + using Keypoint::tree_; + using Keypoint::k_; + using Keypoint::search_radius_; + using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; + + /** + * @brief Constructor + * @param radius the radius for normal estimation as well as for non maxima suppression + * @param threshold the threshold to filter out weak corners + */ + HarrisKeypoint6D (float radius = 0.01, float threshold = 0.0) + : threshold_ (threshold) + , refine_ (true) + , nonmax_ (true) + , threads_ (0) + , normals_ (new pcl::PointCloud) + , intensity_gradients_ (new pcl::PointCloud) + { + name_ = "HarrisKeypoint6D"; + search_radius_ = radius; + } + + /** \brief Empty destructor */ + virtual ~HarrisKeypoint6D () {} + + /** + * @brief set the radius for normal estimation and non maxima supression. + * @param radius + */ + void setRadius (float radius); + + /** + * @brief set the threshold value for detecting corners. This is only evaluated if non maxima suppression is turned on. + * @brief note non maxima suppression needs to be activated in order to use this feature. + * @param threshold + */ + void setThreshold (float threshold); + + /** + * @brief whether non maxima suppression should be applied or the response for each point should be returned + * @note this value needs to be turned on in order to apply thresholding and refinement + * @param nonmax default is false + */ + void setNonMaxSupression (bool = false); + + /** + * @brief whether the detected key points should be refined or not. If turned of, the key points are a subset of the original point cloud. Otherwise the key points may be arbitrary. + * @brief note non maxima supression needs to be on in order to use this feature. + * @param do_refine + */ + void setRefine (bool do_refine); + + virtual void + setSearchSurface (const PointCloudInConstPtr &cloud) { surface_ = cloud; normals_->clear (); intensity_gradients_->clear ();} + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + protected: + void detectKeypoints (PointCloudOut &output); + void responseTomasi (PointCloudOut &output) const; + void refineCorners (PointCloudOut &corners) const; + void calculateCombinedCovar (const std::vector& neighbors, float* coefficients) const; + private: + float threshold_; + bool refine_; + bool nonmax_; + unsigned int threads_; + typename pcl::PointCloud::Ptr normals_; + pcl::PointCloud::Ptr intensity_gradients_; + } ; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/agast_2d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/agast_2d.hpp new file mode 100755 index 0000000..71eaffc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/agast_2d.hpp @@ -0,0 +1,102 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_KEYPOINTS_AGAST_KEYPOINT_2D_IMPL_H_ +#define PCL_KEYPOINTS_AGAST_KEYPOINT_2D_IMPL_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::AgastKeypoint2DBase::initCompute () +{ + if (!pcl::Keypoint::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ()); + return (false); + } + + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::AgastKeypoint2D::detectKeypoints (PointCloudOut &output) +{ + // image size + const std::size_t width = input_->width; + const std::size_t height = input_->height; + + // destination for intensity data; will be forwarded to AGAST + std::vector image_data (width*height); + + for (std::size_t i = 0; i < image_data.size (); ++i) + image_data[i] = static_cast (intensity_ ((*input_)[i])); + + if (!detector_) + detector_.reset (new pcl::keypoints::agast::AgastDetector7_12s (width, height, threshold_, bmax_)); + + detector_->setMaxKeypoints (nr_max_keypoints_); + + if (apply_non_max_suppression_) + { + pcl::PointCloud tmp_cloud; + detector_->detectKeypoints (image_data, tmp_cloud); + + pcl::keypoints::internal::AgastApplyNonMaxSuppresion anms ( + image_data, tmp_cloud, detector_, output); + } + else + { + pcl::keypoints::internal::AgastDetector dec ( + image_data, detector_, output); + } + + // we do not change the denseness + output.is_dense = true; +} + + +#define AgastKeypoint2D(T,I) template class PCL_EXPORTS pcl::AgastKeypoint2D; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/brisk_2d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/brisk_2d.hpp new file mode 100755 index 0000000..1e00027 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/brisk_2d.hpp @@ -0,0 +1,108 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (C) 2011, The Autonomous Systems Lab (ASL), ETH Zurich, + * Stefan Leutenegger, Simon Lynen and Margarita Chli. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_KEYPOINTS_BRISK_KEYPOINT_2D_IMPL_H_ +#define PCL_KEYPOINTS_BRISK_KEYPOINT_2D_IMPL_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::BriskKeypoint2D::initCompute () +{ + if (!pcl::Keypoint::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ()); + return (false); + } + + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BriskKeypoint2D::detectKeypoints (PointCloudOut &output) +{ + // image size + const int width = int (input_->width); + const int height = int (input_->height); + + // destination for intensity data; will be forwarded to BRISK + std::vector image_data (width*height); + + for (std::size_t i = 0; i < image_data.size (); ++i) + image_data[i] = static_cast (intensity_ ((*input_)[i])); + + pcl::keypoints::brisk::ScaleSpace brisk_scale_space (octaves_); + brisk_scale_space.constructPyramid (image_data, width, height); + pcl::PointCloud output_temp; + brisk_scale_space.getKeypoints (threshold_, output_temp.points); + pcl::copyPointCloud (output_temp, output); + + // we do not change the denseness + output.width = int (output.points.size ()); + output.height = 1; + output.is_dense = false; // set to false to be sure + + // 2nd pass to remove the invalid set of 3D keypoints + if (remove_invalid_3D_keypoints_) + { + PointCloudOut output_clean; + for (std::size_t i = 0; i < output.size (); ++i) + { + PointOutT pt; + // Interpolate its position in 3D, as the "u" and "v" are subpixel accurate + bilinearInterpolation (input_, output[i].x, output[i].y, pt); + + // Check if the point is finite + if (pcl::isFinite (pt)) + output_clean.push_back (output[i]); + } + output = output_clean; + output.is_dense = true; // set to true as there's no keypoint at an invalid XYZ + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_2d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_2d.hpp new file mode 100755 index 0000000..3bf6111 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_2d.hpp @@ -0,0 +1,475 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_ +#define PCL_HARRIS_KEYPOINT_2D_IMPL_H_ + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setMethod (ResponseMethod method) +{ + method_ = method; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setThreshold (float threshold) +{ + threshold_= threshold; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setRefine (bool do_refine) +{ + refine_ = do_refine; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setNonMaxSupression (bool nonmax) +{ + nonmax_ = nonmax; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setWindowWidth (int window_width) +{ + window_width_= window_width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setWindowHeight (int window_height) +{ + window_height_= window_height; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setSkippedPixels (int skipped_pixels) +{ + skipped_pixels_= skipped_pixels; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::setMinimalDistance (int min_distance) +{ + min_distance_ = min_distance; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::computeSecondMomentMatrix (std::size_t index, float* coefficients) const +{ + static const int width = static_cast (input_->width); + static const int height = static_cast (input_->height); + + int x = static_cast (index % input_->width); + int y = static_cast (index / input_->width); + // indices 0 1 2 + // coefficients: ixix ixiy iyiy + memset (coefficients, 0, sizeof (float) * 3); + + int endx = std::min (width, x + half_window_width_); + int endy = std::min (height, y + half_window_height_); + for (int xx = std::max (0, x - half_window_width_); xx < endx; ++xx) + for (int yy = std::max (0, y - half_window_height_); yy < endy; ++yy) + { + const float& ix = derivatives_rows_ (xx,yy); + const float& iy = derivatives_cols_ (xx,yy); + coefficients[0]+= ix * ix; + coefficients[1]+= ix * iy; + coefficients[2]+= iy * iy; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::HarrisKeypoint2D::initCompute () +{ + if (!pcl::Keypoint::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ()); + return (false); + } + + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); + return (false); + } + + if (indices_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); + return (false); + } + + if ((window_height_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ()); + return (false); + } + + if ((window_width_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ()); + return (false); + } + + if (window_height_ < 3 || window_width_ < 3) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_width_ = window_width_ / 2; + half_window_height_ = window_height_ / 2; + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::detectKeypoints (PointCloudOut &output) +{ + derivatives_cols_.resize (input_->width, input_->height); + derivatives_rows_.resize (input_->width, input_->height); + //Compute cloud intensities first derivatives along columns and rows + //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan + int w = static_cast (input_->width) - 1; + int h = static_cast (input_->height) - 1; + // j = 0 --> j-1 out of range ; use 0 + // i = 0 --> i-1 out of range ; use 0 + derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5; + derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5; + + for(int i = 1; i < w; ++i) + { + derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5; + } + + derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5; + derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5; + + for(int j = 1; j < h; ++j) + { + // i = 0 --> i-1 out of range ; use 0 + derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5; + for(int i = 1; i < w; ++i) + { + // derivative with respect to rows + derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5; + + // derivative with respect to cols + derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5; + } + // i = w --> w+1 out of range ; use w + derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5; + } + + // j = h --> j+1 out of range use h + derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5; + derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5; + + for(int i = 1; i < w; ++i) + { + derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5; + } + derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5; + derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5; + + switch (method_) + { + case HARRIS: + responseHarris(*response_); + break; + case NOBLE: + responseNoble(*response_); + break; + case LOWE: + responseLowe(*response_); + break; + case TOMASI: + responseTomasi(*response_); + break; + } + + if (!nonmax_) + { + output = *response_; + for (std::size_t i = 0; i < response_->size (); ++i) + keypoints_indices_->indices.push_back (i); + } + else + { + std::sort (indices_->begin (), indices_->end (), [this] (int p1, int p2) { return greaterIntensityAtIndices (p1, p2); }); + float threshold = threshold_ * response_->points[indices_->front ()].intensity; + output.clear (); + output.reserve (response_->size()); + std::vector occupency_map (response_->size (), false); + int width (response_->width); + int height (response_->height); + const int occupency_map_size (occupency_map.size ()); + +#ifdef _OPENMP +#pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_) +#endif + for (int i = 0; i < occupency_map_size; ++i) + { + int idx = indices_->at (i); + const PointOutT& point_out = response_->points [idx]; + if (occupency_map[idx] || point_out.intensity < threshold || !isFinite (point_out)) + continue; + +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.push_back (point_out); + keypoints_indices_->indices.push_back (idx); + } + + int u_end = std::min (width, idx % width + min_distance_); + int v_end = std::min (height, idx / width + min_distance_); + for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u) + for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v) + occupency_map[v*input_->width+u] = true; + } + + // if (refine_) + // refineCorners (output); + + output.height = 1; + output.width = static_cast (output.size()); + } + + // we don not change the denseness + output.is_dense = input_->is_dense; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::responseHarris (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [3]; + output.clear (); + output.resize (input_->size ()); + const int output_size (output.size ()); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#endif + for (int index = 0; index < output_size; ++index) + { + PointOutT& out_point = output.points [index]; + const PointInT &in_point = (*input_).points [index]; + out_point.intensity = 0; + out_point.x = in_point.x; + out_point.y = in_point.y; + out_point.z = in_point.z; + if (isFinite (in_point)) + { + computeSecondMomentMatrix (index, covar); + float trace = covar [0] + covar [2]; + if (trace != 0.f) + { + float det = covar[0] * covar[2] - covar[1] * covar[1]; + out_point.intensity = 0.04f + det - 0.04f * trace * trace; + } + } + } + + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::responseNoble (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [3]; + output.clear (); + output.resize (input_->size ()); + const int output_size (output.size ()); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#endif + for (int index = 0; index < output_size; ++index) + { + PointOutT &out_point = output.points [index]; + const PointInT &in_point = input_->points [index]; + out_point.x = in_point.x; + out_point.y = in_point.y; + out_point.z = in_point.z; + out_point.intensity = 0; + if (isFinite (in_point)) + { + computeSecondMomentMatrix (index, covar); + float trace = covar [0] + covar [2]; + if (trace != 0) + { + float det = covar[0] * covar[2] - covar[1] * covar[1]; + out_point.intensity = det / trace; + } + } + } + + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::responseLowe (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [3]; + output.clear (); + output.resize (input_->size ()); + const int output_size (output.size ()); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#endif + for (int index = 0; index < output_size; ++index) + { + PointOutT &out_point = output.points [index]; + const PointInT &in_point = input_->points [index]; + out_point.x = in_point.x; + out_point.y = in_point.y; + out_point.z = in_point.z; + out_point.intensity = 0; + if (isFinite (in_point)) + { + computeSecondMomentMatrix (index, covar); + float trace = covar [0] + covar [2]; + if (trace != 0) + { + float det = covar[0] * covar[2] - covar[1] * covar[1]; + out_point.intensity = det / (trace * trace); + } + } + } + + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint2D::responseTomasi (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [3]; + output.clear (); + output.resize (input_->size ()); + const int output_size (output.size ()); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#endif + for (int index = 0; index < output_size; ++index) + { + PointOutT &out_point = output.points [index]; + const PointInT &in_point = input_->points [index]; + out_point.x = in_point.x; + out_point.y = in_point.y; + out_point.z = in_point.z; + out_point.intensity = 0; + if (isFinite (in_point)) + { + computeSecondMomentMatrix (index, covar); + // min egenvalue + out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f); + } + } + + output.height = input_->height; + output.width = input_->width; +} + +// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// template void +// pcl::HarrisKeypoint2D::refineCorners (PointCloudOut &corners) const +// { +// std::vector nn_indices; +// std::vector nn_dists; + +// Eigen::Matrix2f nnT; +// Eigen::Matrix2f NNT; +// Eigen::Matrix2f NNTInv; +// Eigen::Vector2f NNTp; +// float diff; +// const unsigned max_iterations = 10; +// #ifdef _OPENMP +// #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff, nn_indices, nn_dists) num_threads(threads_) +// #endif +// for (int cIdx = 0; cIdx < static_cast (corners.size ()); ++cIdx) +// { +// unsigned iterations = 0; +// do { +// NNT.setZero(); +// NNTp.setZero(); +// PointInT corner; +// corner.x = corners[cIdx].x; +// corner.y = corners[cIdx].y; +// corner.z = corners[cIdx].z; +// tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists); +// for (std::vector::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) +// { +// if (!std::isfinite (normals_->points[*iIt].normal_x)) +// continue; + +// nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose(); +// NNT += nnT; +// NNTp += nnT * surface_->points[*iIt].getVector3fMap (); +// } +// if (invert3x3SymMatrix (NNT, NNTInv) != 0) +// corners[cIdx].getVector3fMap () = NNTInv * NNTp; + +// diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm (); +// } while (diff > 1e-6 && ++iterations < max_iterations); +// } +// } + +#define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D; +#endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_3d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_3d.hpp new file mode 100755 index 0000000..14e2c2d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_3d.hpp @@ -0,0 +1,541 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_ +#define PCL_HARRIS_KEYPOINT_3D_IMPL_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef __SSE__ +#include +#endif + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setInputCloud (const PointCloudInConstPtr &cloud) +{ + if (normals_ && input_ && (cloud != input_)) + normals_.reset (); + input_ = cloud; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setMethod (ResponseMethod method) +{ + method_ = method; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setThreshold (float threshold) +{ + threshold_= threshold; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setRadius (float radius) +{ + search_radius_ = radius; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setRefine (bool do_refine) +{ + refine_ = do_refine; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setNonMaxSupression (bool nonmax) +{ + nonmax_ = nonmax; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::setNormals (const PointCloudNConstPtr &normals) +{ + normals_ = normals; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::calculateNormalCovar (const std::vector& neighbors, float* coefficients) const +{ + unsigned count = 0; + // indices 0 1 2 3 4 5 6 7 + // coefficients: xx xy xz ?? yx yy yz ?? +#ifdef __SSE__ + // accumulator for xx, xy, xz + __m128 vec1 = _mm_setzero_ps(); + // accumulator for yy, yz, zz + __m128 vec2 = _mm_setzero_ps(); + + __m128 norm1; + + __m128 norm2; + + float zz = 0; + + for (const int &neighbor : neighbors) + { + if (std::isfinite (normals_->points[neighbor].normal_x)) + { + // nx, ny, nz, h + norm1 = _mm_load_ps (&(normals_->points[neighbor].normal_x)); + + // nx, nx, nx, nx + norm2 = _mm_set1_ps (normals_->points[neighbor].normal_x); + + // nx * nx, nx * ny, nx * nz, nx * h + norm2 = _mm_mul_ps (norm1, norm2); + + // accumulate + vec1 = _mm_add_ps (vec1, norm2); + + // ny, ny, ny, ny + norm2 = _mm_set1_ps (normals_->points[neighbor].normal_y); + + // ny * nx, ny * ny, ny * nz, ny * h + norm2 = _mm_mul_ps (norm1, norm2); + + // accumulate + vec2 = _mm_add_ps (vec2, norm2); + + zz += normals_->points[neighbor].normal_z * normals_->points[neighbor].normal_z; + ++count; + } + } + if (count > 0) + { + norm2 = _mm_set1_ps (float(count)); + vec1 = _mm_div_ps (vec1, norm2); + vec2 = _mm_div_ps (vec2, norm2); + _mm_store_ps (coefficients, vec1); + _mm_store_ps (coefficients + 4, vec2); + coefficients [7] = zz / float(count); + } + else + memset (coefficients, 0, sizeof (float) * 8); +#else + memset (coefficients, 0, sizeof (float) * 8); + for (std::vector::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt) + { + if (std::isfinite (normals_->points[*iIt].normal_x)) + { + coefficients[0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x; + coefficients[1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y; + coefficients[2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z; + + coefficients[5] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y; + coefficients[6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z; + coefficients[7] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z; + + ++count; + } + } + if (count > 0) + { + float norm = 1.0 / float (count); + coefficients[0] *= norm; + coefficients[1] *= norm; + coefficients[2] *= norm; + coefficients[5] *= norm; + coefficients[6] *= norm; + coefficients[7] *= norm; + } +#endif +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::HarrisKeypoint3D::initCompute () +{ + if (!Keypoint::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ()); + return (false); + } + + if (method_ < 1 || method_ > 5) + { + PCL_ERROR ("[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_); + return (false); + } + + if (!normals_) + { + PointCloudNPtr normals (new PointCloudN ()); + normals->reserve (normals->size ()); + if (!surface_->isOrganized ()) + { + pcl::NormalEstimation normal_estimation; + normal_estimation.setInputCloud (surface_); + normal_estimation.setRadiusSearch (search_radius_); + normal_estimation.compute (*normals); + } + else + { + IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (surface_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normals); + } + normals_ = normals; + } + if (normals_->size () != surface_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::detectKeypoints (PointCloudOut &output) +{ + typename pcl::PointCloud::Ptr response (new pcl::PointCloud); + + response->points.reserve (input_->points.size()); + + switch (method_) + { + case HARRIS: + responseHarris(*response); + break; + case NOBLE: + responseNoble(*response); + break; + case LOWE: + responseLowe(*response); + break; + case CURVATURE: + responseCurvature(*response); + break; + case TOMASI: + responseTomasi(*response); + break; + } + + if (!nonmax_) + { + output = *response; + // we do not change the denseness in this case + output.is_dense = input_->is_dense; + for (std::size_t i = 0; i < response->size (); ++i) + keypoints_indices_->indices.push_back (i); + } + else + { + output.points.clear (); + output.points.reserve (response->points.size()); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads(threads_) +#endif + for (int idx = 0; idx < static_cast (response->points.size ()); ++idx) + { + if (!isFinite (response->points[idx]) || + !std::isfinite (response->points[idx].intensity) || + response->points[idx].intensity < threshold_) + continue; + + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists); + bool is_maxima = true; + for (std::vector::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) + { + if (response->points[idx].intensity < response->points[*iIt].intensity) + { + is_maxima = false; + break; + } + } + if (is_maxima) +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.points.push_back (response->points[idx]); + keypoints_indices_->indices.push_back (idx); + } + } + + if (refine_) + refineCorners (output); + + output.height = 1; + output.width = static_cast (output.points.size()); + output.is_dense = true; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::responseHarris (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [8]; + output.resize (input_->size ()); +#ifdef _OPENMP + #pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#endif + for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) + { + const PointInT& pointIn = input_->points [pIdx]; + output [pIdx].intensity = 0.0; //std::numeric_limits::quiet_NaN (); + if (isFinite (pointIn)) + { + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); + calculateNormalCovar (nn_indices, covar); + + float trace = covar [0] + covar [5] + covar [7]; + if (trace != 0) + { + float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6] + - covar [2] * covar [2] * covar [5] + - covar [1] * covar [1] * covar [7] + - covar [6] * covar [6] * covar [0]; + + output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace; + } + } + output [pIdx].x = pointIn.x; + output [pIdx].y = pointIn.y; + output [pIdx].z = pointIn.z; + } + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::responseNoble (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [8]; + output.resize (input_->size ()); +#ifdef _OPENMP + #pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#endif + for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) + { + const PointInT& pointIn = input_->points [pIdx]; + output [pIdx].intensity = 0.0; + if (isFinite (pointIn)) + { + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); + calculateNormalCovar (nn_indices, covar); + float trace = covar [0] + covar [5] + covar [7]; + if (trace != 0) + { + float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6] + - covar [2] * covar [2] * covar [5] + - covar [1] * covar [1] * covar [7] + - covar [6] * covar [6] * covar [0]; + + output [pIdx].intensity = det / trace; + } + } + output [pIdx].x = pointIn.x; + output [pIdx].y = pointIn.y; + output [pIdx].z = pointIn.z; + } + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::responseLowe (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [8]; + output.resize (input_->size ()); +#ifdef _OPENMP + #pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#endif + for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) + { + const PointInT& pointIn = input_->points [pIdx]; + output [pIdx].intensity = 0.0; + if (isFinite (pointIn)) + { + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); + calculateNormalCovar (nn_indices, covar); + float trace = covar [0] + covar [5] + covar [7]; + if (trace != 0) + { + float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6] + - covar [2] * covar [2] * covar [5] + - covar [1] * covar [1] * covar [7] + - covar [6] * covar [6] * covar [0]; + + output [pIdx].intensity = det / (trace * trace); + } + } + output [pIdx].x = pointIn.x; + output [pIdx].y = pointIn.y; + output [pIdx].z = pointIn.z; + } + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::responseCurvature (PointCloudOut &output) const +{ + PointOutT point; + for (std::size_t idx = 0; idx < input_->points.size(); ++idx) + { + point.x = input_->points[idx].x; + point.y = input_->points[idx].y; + point.z = input_->points[idx].z; + point.intensity = normals_->points[idx].curvature; + output.points.push_back(point); + } + // does not change the order + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::responseTomasi (PointCloudOut &output) const +{ + PCL_ALIGN (16) float covar [8]; + Eigen::Matrix3f covariance_matrix; + output.resize (input_->size ()); +#ifdef _OPENMP + #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_) +#endif + for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) + { + const PointInT& pointIn = input_->points [pIdx]; + output [pIdx].intensity = 0.0; + if (isFinite (pointIn)) + { + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); + calculateNormalCovar (nn_indices, covar); + float trace = covar [0] + covar [5] + covar [7]; + if (trace != 0) + { + covariance_matrix.coeffRef (0) = covar [0]; + covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1]; + covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2]; + covariance_matrix.coeffRef (4) = covar [5]; + covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6]; + covariance_matrix.coeffRef (8) = covar [7]; + + EIGEN_ALIGN16 Eigen::Vector3f eigen_values; + pcl::eigen33(covariance_matrix, eigen_values); + output [pIdx].intensity = eigen_values[0]; + } + } + output [pIdx].x = pointIn.x; + output [pIdx].y = pointIn.y; + output [pIdx].z = pointIn.z; + } + output.height = input_->height; + output.width = input_->width; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint3D::refineCorners (PointCloudOut &corners) const +{ + Eigen::Matrix3f nnT; + Eigen::Matrix3f NNT; + Eigen::Matrix3f NNTInv; + Eigen::Vector3f NNTp; + float diff; + const unsigned max_iterations = 10; +#ifdef _OPENMP + #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_) +#endif + for (int cIdx = 0; cIdx < static_cast (corners.size ()); ++cIdx) + { + unsigned iterations = 0; + do { + NNT.setZero(); + NNTp.setZero(); + PointInT corner; + corner.x = corners[cIdx].x; + corner.y = corners[cIdx].y; + corner.z = corners[cIdx].z; + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists); + for (std::vector::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) + { + if (!std::isfinite (normals_->points[*iIt].normal_x)) + continue; + + nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose(); + NNT += nnT; + NNTp += nnT * surface_->points[*iIt].getVector3fMap (); + } + if (invert3x3SymMatrix (NNT, NNTInv) != 0) + corners[cIdx].getVector3fMap () = NNTInv * NNTp; + + diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm (); + } while (diff > 1e-6 && ++iterations < max_iterations); + } +} + +#define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D; +#endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_6d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_6d.hpp new file mode 100755 index 0000000..4b05db4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/harris_6d.hpp @@ -0,0 +1,407 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_ +#define PCL_HARRIS_KEYPOINT_6D_IMPL_H_ + +#include +#include +#include +#include +#include +//#include +#include +#include + +template void +pcl::HarrisKeypoint6D::setThreshold (float threshold) +{ + threshold_= threshold; +} + +template void +pcl::HarrisKeypoint6D::setRadius (float radius) +{ + search_radius_ = radius; +} + +template void +pcl::HarrisKeypoint6D::setRefine (bool do_refine) +{ + refine_ = do_refine; +} + +template void +pcl::HarrisKeypoint6D::setNonMaxSupression (bool nonmax) +{ + nonmax_ = nonmax; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint6D::calculateCombinedCovar (const std::vector& neighbors, float* coefficients) const +{ + memset (coefficients, 0, sizeof (float) * 21); + unsigned count = 0; + for (const int &neighbor : neighbors) + { + if (std::isfinite (normals_->points[neighbor].normal_x) && std::isfinite (intensity_gradients_->points[neighbor].gradient [0])) + { + coefficients[ 0] += normals_->points[neighbor].normal_x * normals_->points[neighbor].normal_x; + coefficients[ 1] += normals_->points[neighbor].normal_x * normals_->points[neighbor].normal_y; + coefficients[ 2] += normals_->points[neighbor].normal_x * normals_->points[neighbor].normal_z; + coefficients[ 3] += normals_->points[neighbor].normal_x * intensity_gradients_->points[neighbor].gradient [0]; + coefficients[ 4] += normals_->points[neighbor].normal_x * intensity_gradients_->points[neighbor].gradient [1]; + coefficients[ 5] += normals_->points[neighbor].normal_x * intensity_gradients_->points[neighbor].gradient [2]; + + coefficients[ 6] += normals_->points[neighbor].normal_y * normals_->points[neighbor].normal_y; + coefficients[ 7] += normals_->points[neighbor].normal_y * normals_->points[neighbor].normal_z; + coefficients[ 8] += normals_->points[neighbor].normal_y * intensity_gradients_->points[neighbor].gradient [0]; + coefficients[ 9] += normals_->points[neighbor].normal_y * intensity_gradients_->points[neighbor].gradient [1]; + coefficients[10] += normals_->points[neighbor].normal_y * intensity_gradients_->points[neighbor].gradient [2]; + + coefficients[11] += normals_->points[neighbor].normal_z * normals_->points[neighbor].normal_z; + coefficients[12] += normals_->points[neighbor].normal_z * intensity_gradients_->points[neighbor].gradient [0]; + coefficients[13] += normals_->points[neighbor].normal_z * intensity_gradients_->points[neighbor].gradient [1]; + coefficients[14] += normals_->points[neighbor].normal_z * intensity_gradients_->points[neighbor].gradient [2]; + + coefficients[15] += intensity_gradients_->points[neighbor].gradient [0] * intensity_gradients_->points[neighbor].gradient [0]; + coefficients[16] += intensity_gradients_->points[neighbor].gradient [0] * intensity_gradients_->points[neighbor].gradient [1]; + coefficients[17] += intensity_gradients_->points[neighbor].gradient [0] * intensity_gradients_->points[neighbor].gradient [2]; + + coefficients[18] += intensity_gradients_->points[neighbor].gradient [1] * intensity_gradients_->points[neighbor].gradient [1]; + coefficients[19] += intensity_gradients_->points[neighbor].gradient [1] * intensity_gradients_->points[neighbor].gradient [2]; + + coefficients[20] += intensity_gradients_->points[neighbor].gradient [2] * intensity_gradients_->points[neighbor].gradient [2]; + + ++count; + } + } + if (count > 0) + { + float norm = 1.0 / float (count); + coefficients[ 0] *= norm; + coefficients[ 1] *= norm; + coefficients[ 2] *= norm; + coefficients[ 3] *= norm; + coefficients[ 4] *= norm; + coefficients[ 5] *= norm; + coefficients[ 6] *= norm; + coefficients[ 7] *= norm; + coefficients[ 8] *= norm; + coefficients[ 9] *= norm; + coefficients[10] *= norm; + coefficients[11] *= norm; + coefficients[12] *= norm; + coefficients[13] *= norm; + coefficients[14] *= norm; + coefficients[15] *= norm; + coefficients[16] *= norm; + coefficients[17] *= norm; + coefficients[18] *= norm; + coefficients[19] *= norm; + coefficients[20] *= norm; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::HarrisKeypoint6D::detectKeypoints (PointCloudOut &output) +{ + if (normals_->empty ()) + { + normals_->reserve (surface_->size ()); + if (!surface_->isOrganized ()) + { + pcl::NormalEstimation normal_estimation; + normal_estimation.setInputCloud (surface_); + normal_estimation.setRadiusSearch (search_radius_); + normal_estimation.compute (*normals_); + } + else + { + IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (surface_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normals_); + } + } + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->resize (surface_->size ()); +#ifdef _OPENMP + #pragma omp parallel for num_threads(threads_) default(shared) +#endif + for (unsigned idx = 0; idx < surface_->size (); ++idx) + { + cloud->points [idx].x = surface_->points [idx].x; + cloud->points [idx].y = surface_->points [idx].y; + cloud->points [idx].z = surface_->points [idx].z; + //grayscale = 0.2989 * R + 0.5870 * G + 0.1140 * B + + cloud->points [idx].intensity = 0.00390625 * (0.114 * float(surface_->points [idx].b) + 0.5870 * float(surface_->points [idx].g) + 0.2989 * float(surface_->points [idx].r)); + } + pcl::copyPointCloud (*surface_, *cloud); + + IntensityGradientEstimation grad_est; + grad_est.setInputCloud (cloud); + grad_est.setInputNormals (normals_); + grad_est.setRadiusSearch (search_radius_); + grad_est.compute (*intensity_gradients_); + +#ifdef _OPENMP + #pragma omp parallel for num_threads(threads_) default (shared) +#endif + for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx) + { + float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x + + intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y + + intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ; + + // Suat: ToDo: remove this magic number or expose using set/get + if (len > 200.0) + { + len = 1.0 / sqrt (len); + intensity_gradients_->points [idx].gradient_x *= len; + intensity_gradients_->points [idx].gradient_y *= len; + intensity_gradients_->points [idx].gradient_z *= len; + } + else + { + intensity_gradients_->points [idx].gradient_x = 0; + intensity_gradients_->points [idx].gradient_y = 0; + intensity_gradients_->points [idx].gradient_z = 0; + } + } + + typename pcl::PointCloud::Ptr response (new pcl::PointCloud); + response->points.reserve (input_->points.size()); + responseTomasi(*response); + + // just return the response + if (!nonmax_) + { + output = *response; + // we do not change the denseness in this case + output.is_dense = input_->is_dense; + for (std::size_t i = 0; i < response->size (); ++i) + keypoints_indices_->indices.push_back (i); + } + else + { + output.points.clear (); + output.points.reserve (response->points.size()); + +#ifdef _OPENMP + #pragma omp parallel for num_threads(threads_) default(shared) +#endif + for (std::size_t idx = 0; idx < response->points.size (); ++idx) + { + if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_) + continue; + + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists); + bool is_maxima = true; + for (std::vector::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) + { + if (response->points[idx].intensity < response->points[*iIt].intensity) + { + is_maxima = false; + break; + } + } + if (is_maxima) +#ifdef _OPENMP + #pragma omp critical +#endif + { + output.points.push_back (response->points[idx]); + keypoints_indices_->indices.push_back (idx); + } + } + + if (refine_) + refineCorners (output); + + output.height = 1; + output.width = static_cast (output.points.size()); + output.is_dense = true; + } +} + +template void +pcl::HarrisKeypoint6D::responseTomasi (PointCloudOut &output) const +{ + // get the 6x6 covar-mat + PointOutT pointOut; + PCL_ALIGN (16) float covar [21]; + Eigen::SelfAdjointEigenSolver > solver; + Eigen::Matrix covariance; + +#ifdef _OPENMP + #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_) +#endif + for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx) + { + const PointInT& pointIn = input_->points [pIdx]; + pointOut.intensity = 0.0; //std::numeric_limits::quiet_NaN (); + if (isFinite (pointIn)) + { + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); + calculateCombinedCovar (nn_indices, covar); + + float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20]; + if (trace != 0) + { + covariance.coeffRef ( 0) = covar [ 0]; + covariance.coeffRef ( 1) = covar [ 1]; + covariance.coeffRef ( 2) = covar [ 2]; + covariance.coeffRef ( 3) = covar [ 3]; + covariance.coeffRef ( 4) = covar [ 4]; + covariance.coeffRef ( 5) = covar [ 5]; + + covariance.coeffRef ( 7) = covar [ 6]; + covariance.coeffRef ( 8) = covar [ 7]; + covariance.coeffRef ( 9) = covar [ 8]; + covariance.coeffRef (10) = covar [ 9]; + covariance.coeffRef (11) = covar [10]; + + covariance.coeffRef (14) = covar [11]; + covariance.coeffRef (15) = covar [12]; + covariance.coeffRef (16) = covar [13]; + covariance.coeffRef (17) = covar [14]; + + covariance.coeffRef (21) = covar [15]; + covariance.coeffRef (22) = covar [16]; + covariance.coeffRef (23) = covar [17]; + + covariance.coeffRef (28) = covar [18]; + covariance.coeffRef (29) = covar [19]; + + covariance.coeffRef (35) = covar [20]; + + covariance.coeffRef ( 6) = covar [ 1]; + + covariance.coeffRef (12) = covar [ 2]; + covariance.coeffRef (13) = covar [ 7]; + + covariance.coeffRef (18) = covar [ 3]; + covariance.coeffRef (19) = covar [ 8]; + covariance.coeffRef (20) = covar [12]; + + covariance.coeffRef (24) = covar [ 4]; + covariance.coeffRef (25) = covar [ 9]; + covariance.coeffRef (26) = covar [13]; + covariance.coeffRef (27) = covar [16]; + + covariance.coeffRef (30) = covar [ 5]; + covariance.coeffRef (31) = covar [10]; + covariance.coeffRef (32) = covar [14]; + covariance.coeffRef (33) = covar [17]; + covariance.coeffRef (34) = covar [19]; + + solver.compute (covariance); + pointOut.intensity = solver.eigenvalues () [3]; + } + } + + pointOut.x = pointIn.x; + pointOut.y = pointIn.y; + pointOut.z = pointIn.z; +#ifdef _OPENMP + #pragma omp critical +#endif + + output.points.push_back(pointOut); + } + output.height = input_->height; + output.width = input_->width; +} + +template void +pcl::HarrisKeypoint6D::refineCorners (PointCloudOut &corners) const +{ + pcl::search::KdTree search; + search.setInputCloud(surface_); + + Eigen::Matrix3f nnT; + Eigen::Matrix3f NNT; + Eigen::Vector3f NNTp; + const Eigen::Vector3f* normal; + const Eigen::Vector3f* point; + float diff; + const unsigned max_iterations = 10; + for (typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt) + { + unsigned iterations = 0; + do { + NNT.setZero(); + NNTp.setZero(); + PointInT corner; + corner.x = cornerIt->x; + corner.y = cornerIt->y; + corner.z = cornerIt->z; + std::vector nn_indices; + std::vector nn_dists; + search.radiusSearch (corner, search_radius_, nn_indices, nn_dists); + for (std::vector::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) + { + normal = reinterpret_cast (&(normals_->points[*iIt].normal_x)); + point = reinterpret_cast (&(surface_->points[*iIt].x)); + nnT = (*normal) * (normal->transpose()); + NNT += nnT; + NNTp += nnT * (*point); + } + if (NNT.determinant() != 0) + *(reinterpret_cast(&(cornerIt->x))) = NNT.inverse () * NNTp; + + diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) + + (cornerIt->y - corner.y) * (cornerIt->y - corner.y) + + (cornerIt->z - corner.z) * (cornerIt->z - corner.z); + + } while (diff > 1e-6 && ++iterations < max_iterations); + } +} + +#define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D; +#endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/iss_3d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/iss_3d.hpp new file mode 100755 index 0000000..2ee9f0c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/iss_3d.hpp @@ -0,0 +1,465 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_ISS_KEYPOINT3D_IMPL_H_ +#define PCL_ISS_KEYPOINT3D_IMPL_H_ + +#include +#include +#include + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setSalientRadius (double salient_radius) +{ + salient_radius_ = salient_radius; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setNonMaxRadius (double non_max_radius) +{ + non_max_radius_ = non_max_radius; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setNormalRadius (double normal_radius) +{ + normal_radius_ = normal_radius; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setBorderRadius (double border_radius) +{ + border_radius_ = border_radius; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setThreshold21 (double gamma_21) +{ + gamma_21_ = gamma_21; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setThreshold32 (double gamma_32) +{ + gamma_32_ = gamma_32; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setMinNeighbors (int min_neighbors) +{ + min_neighbors_ = min_neighbors; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::setNormals (const PointCloudNConstPtr &normals) +{ + normals_ = normals; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool* +pcl::ISSKeypoint3D::getBoundaryPoints (PointCloudIn &input, double border_radius, float angle_threshold) +{ + bool* edge_points = new bool [input.size ()]; + + Eigen::Vector4f u = Eigen::Vector4f::Zero (); + Eigen::Vector4f v = Eigen::Vector4f::Zero (); + + pcl::BoundaryEstimation boundary_estimator; + boundary_estimator.setInputCloud (input_); + +#ifdef _OPENMP +#pragma omp parallel for private(u, v) num_threads(threads_) +#endif + for (int index = 0; index < int (input.points.size ()); index++) + { + edge_points[index] = false; + PointInT current_point = input.points[index]; + + if (pcl::isFinite(current_point)) + { + std::vector nn_indices; + std::vector nn_distances; + int n_neighbors; + + this->searchForNeighbors (static_cast (index), border_radius, nn_indices, nn_distances); + + n_neighbors = static_cast (nn_indices.size ()); + + if (n_neighbors >= min_neighbors_) + { + boundary_estimator.getCoordinateSystemOnPlane (normals_->points[index], u, v); + + if (boundary_estimator.isBoundaryPoint (input, static_cast (index), nn_indices, u, v, angle_threshold)) + edge_points[index] = true; + } + } + } + + return (edge_points); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::getScatterMatrix (const int& current_index, Eigen::Matrix3d &cov_m) +{ + const PointInT& current_point = (*input_).points[current_index]; + + double central_point[3]; + memset(central_point, 0, sizeof(double) * 3); + + central_point[0] = current_point.x; + central_point[1] = current_point.y; + central_point[2] = current_point.z; + + cov_m = Eigen::Matrix3d::Zero (); + + std::vector nn_indices; + std::vector nn_distances; + int n_neighbors; + + this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances); + + n_neighbors = static_cast (nn_indices.size ()); + + if (n_neighbors < min_neighbors_) + return; + + double cov[9]; + memset(cov, 0, sizeof(double) * 9); + + for (int n_idx = 0; n_idx < n_neighbors; n_idx++) + { + const PointInT& n_point = (*input_).points[nn_indices[n_idx]]; + + double neigh_point[3]; + memset(neigh_point, 0, sizeof(double) * 3); + + neigh_point[0] = n_point.x; + neigh_point[1] = n_point.y; + neigh_point[2] = n_point.z; + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]); + } + + cov_m << cov[0], cov[1], cov[2], + cov[3], cov[4], cov[5], + cov[6], cov[7], cov[8]; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ISSKeypoint3D::initCompute () +{ + if (!Keypoint::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ()); + return (false); + } + if (salient_radius_ <= 0) + { + PCL_ERROR ("[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n", + name_.c_str (), salient_radius_); + return (false); + } + if (non_max_radius_ <= 0) + { + PCL_ERROR ("[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n", + name_.c_str (), non_max_radius_); + return (false); + } + if (gamma_21_ <= 0) + { + PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n", + name_.c_str (), gamma_21_); + return (false); + } + if (gamma_32_ <= 0) + { + PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n", + name_.c_str (), gamma_32_); + return (false); + } + if (min_neighbors_ <= 0) + { + PCL_ERROR ("[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n", + name_.c_str (), min_neighbors_); + return (false); + } + + if (third_eigen_value_) + delete[] third_eigen_value_; + + third_eigen_value_ = new double[input_->size ()]; + memset(third_eigen_value_, 0, sizeof(double) * input_->size ()); + + if (edge_points_) + delete[] edge_points_; + + if (border_radius_ > 0.0) + { + if (normals_->empty ()) + { + if (normal_radius_ <= 0.) + { + PCL_ERROR ("[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n", + name_.c_str (), normal_radius_); + return (false); + } + + PointCloudNPtr normal_ptr (new PointCloudN ()); + if (input_->height == 1 ) + { + pcl::NormalEstimation normal_estimation; + normal_estimation.setInputCloud (surface_); + normal_estimation.setRadiusSearch (normal_radius_); + normal_estimation.compute (*normal_ptr); + } + else + { + pcl::IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (surface_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normal_ptr); + } + normals_ = normal_ptr; + } + if (normals_->size () != surface_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ()); + return (false); + } + } + else if (border_radius_ < 0.0) + { + PCL_ERROR ("[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n", + name_.c_str (), border_radius_); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut &output) +{ + // Make sure the output cloud is empty + output.points.clear (); + + if (border_radius_ > 0.0) + edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_); + + bool* borders = new bool [input_->size()]; + +#ifdef _OPENMP + #pragma omp parallel for num_threads(threads_) +#endif + for (int index = 0; index < int (input_->size ()); index++) + { + borders[index] = false; + PointInT current_point = input_->points[index]; + + if ((border_radius_ > 0.0) && (pcl::isFinite(current_point))) + { + std::vector nn_indices; + std::vector nn_distances; + + this->searchForNeighbors (static_cast (index), border_radius_, nn_indices, nn_distances); + + for (const int &nn_index : nn_indices) + { + if (edge_points_[nn_index]) + { + borders[index] = true; + break; + } + } + } + } + +#ifdef _OPENMP + Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_]; + + for (std::size_t i = 0; i < threads_; i++) + omp_mem[i].setZero (3); +#else + Eigen::Vector3d *omp_mem = new Eigen::Vector3d[1]; + + omp_mem[0].setZero (3); +#endif + + double *prg_local_mem = new double[input_->size () * 3]; + double **prg_mem = new double * [input_->size ()]; + + for (std::size_t i = 0; i < input_->size (); i++) + prg_mem[i] = prg_local_mem + 3 * i; + +#ifdef _OPENMP + #pragma omp parallel for num_threads(threads_) +#endif + for (int index = 0; index < static_cast (input_->size ()); index++) + { +#ifdef _OPENMP + int tid = omp_get_thread_num (); +#else + int tid = 0; +#endif + PointInT current_point = input_->points[index]; + + if ((!borders[index]) && pcl::isFinite(current_point)) + { + //if the considered point is not a border point and the point is "finite", then compute the scatter matrix + Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero (); + getScatterMatrix (static_cast (index), cov_m); + + Eigen::SelfAdjointEigenSolver solver (cov_m); + + const double& e1c = solver.eigenvalues ()[2]; + const double& e2c = solver.eigenvalues ()[1]; + const double& e3c = solver.eigenvalues ()[0]; + + if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c)) + continue; + + if (e3c < 0) + { + PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n", + name_.c_str (), index); + continue; + } + + omp_mem[tid][0] = e2c / e1c; + omp_mem[tid][1] = e3c / e2c;; + omp_mem[tid][2] = e3c; + } + + for (Eigen::Index d = 0; d < omp_mem[tid].size (); d++) + prg_mem[index][d] = omp_mem[tid][d]; + } + + for (int index = 0; index < int (input_->size ()); index++) + { + if (!borders[index]) + { + if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_)) + third_eigen_value_[index] = prg_mem[index][2]; + } + } + + bool* feat_max = new bool [input_->size()]; + bool is_max; + +#ifdef _OPENMP + #pragma omp parallel for private(is_max) num_threads(threads_) +#endif + for (int index = 0; index < int (input_->size ()); index++) + { + feat_max [index] = false; + PointInT current_point = input_->points[index]; + + if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point))) + { + std::vector nn_indices; + std::vector nn_distances; + int n_neighbors; + + this->searchForNeighbors (static_cast (index), non_max_radius_, nn_indices, nn_distances); + + n_neighbors = static_cast (nn_indices.size ()); + + if (n_neighbors >= min_neighbors_) + { + is_max = true; + + for (int j = 0 ; j < n_neighbors; j++) + if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]]) + is_max = false; + if (is_max) + feat_max[index] = true; + } + } + } + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads(threads_) +#endif + for (int index = 0; index < int (input_->size ()); index++) + { + if (feat_max[index]) +#ifdef _OPENMP +#pragma omp critical +#endif + { + PointOutT p; + p.getVector3fMap () = input_->points[index].getVector3fMap (); + output.points.push_back(p); + keypoints_indices_->indices.push_back (index); + } + } + + output.header = input_->header; + output.width = static_cast (output.points.size ()); + output.height = 1; + + // Clear the contents of variables and arrays before the beginning of the next computation. + if (border_radius_ > 0.0) + normals_.reset (new pcl::PointCloud); + + delete[] borders; + delete[] prg_mem; + delete[] prg_local_mem; + delete[] feat_max; + delete[] omp_mem; +} + +#define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D; + +#endif /* PCL_ISS_3D_IMPL_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/keypoint.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/keypoint.hpp new file mode 100755 index 0000000..024534f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/keypoint.hpp @@ -0,0 +1,148 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_KEYPOINT_IMPL_H_ +#define PCL_KEYPOINT_IMPL_H_ + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Keypoint::initCompute () +{ + if (!PCLBase::initCompute ()) + return (false); + + // Initialize the spatial locator + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // If no search surface has been defined, use the input dataset as the search surface itself + if (!surface_) + surface_ = input_; + + // Send the surface dataset to the spatial locator + tree_->setInputCloud (surface_); + + // Do a fast check to see if the search parameters are well defined + if (search_radius_ != 0.0) + { + if (k_ != 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Both radius (%f) and K (%d) defined! Set one of them to zero first and then re-run compute ().\n", getClassName ().c_str (), search_radius_, k_); + return (false); + } + + // Use the radiusSearch () function + search_parameter_ = search_radius_; + if (surface_ == input_) // if the two surfaces are the same + { + // Declare the search locator definition + search_method_ = [this] (int index, double radius, std::vector &k_indices, std::vector &k_distances) + { + return tree_->radiusSearch (index, radius, k_indices, k_distances, 0); + }; + } + else + { + // Declare the search locator definition + search_method_surface_ = [this] (const PointCloudIn &cloud, int index, double radius, std::vector &k_indices, std::vector &k_distances) + { + return tree_->radiusSearch (cloud, index, radius, k_indices, k_distances, 0); + }; + } + } + else + { + if (k_ != 0) // Use the nearestKSearch () function + { + search_parameter_ = k_; + if (surface_ == input_) // if the two surfaces are the same + { + // Declare the search locator definition + search_method_ = [this] (int index, int k, std::vector &k_indices, std::vector &k_distances) + { + return tree_->nearestKSearch (index, k, k_indices, k_distances); + }; + } + else + { + // Declare the search locator definition + search_method_surface_ = [this] (const PointCloudIn &cloud, int index, int k, std::vector &k_indices, std::vector &k_distances) + { + return tree_->nearestKSearch (cloud, index, k, k_indices, k_distances); + }; + } + } + else + { + PCL_ERROR ("[pcl::%s::initCompute] Neither radius nor K defined! Set one of them to a positive number first and then re-run compute ().\n", getClassName ().c_str ()); + return (false); + } + } + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::Keypoint::compute (PointCloudOut &output) +{ + if (!initCompute ()) + { + PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); + return; + } + + // Perform the actual computation + detectKeypoints (output); + + deinitCompute (); + + // Reset the surface + if (input_ == surface_) + surface_.reset (); +} + +#endif //#ifndef PCL_KEYPOINT_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/sift_keypoint.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/sift_keypoint.hpp new file mode 100755 index 0000000..7b06de5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/sift_keypoint.hpp @@ -0,0 +1,325 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SIFT_KEYPOINT_IMPL_H_ +#define PCL_SIFT_KEYPOINT_IMPL_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SIFTKeypoint::setScales (float min_scale, int nr_octaves, int nr_scales_per_octave) +{ + min_scale_ = min_scale; + nr_octaves_ = nr_octaves; + nr_scales_per_octave_ = nr_scales_per_octave; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SIFTKeypoint::setMinimumContrast (float min_contrast) +{ + min_contrast_ = min_contrast; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SIFTKeypoint::initCompute () +{ + if (min_scale_ <= 0) + { + PCL_ERROR ("[pcl::%s::initCompute] : Minimum scale (%f) must be strict positive!\n", + name_.c_str (), min_scale_); + return (false); + } + if (nr_octaves_ < 1) + { + PCL_ERROR ("[pcl::%s::initCompute] : Number of octaves (%d) must be at least 1!\n", + name_.c_str (), nr_octaves_); + return (false); + } + if (nr_scales_per_octave_ < 1) + { + PCL_ERROR ("[pcl::%s::initCompute] : Number of scales per octave (%d) must be at least 1!\n", + name_.c_str (), nr_scales_per_octave_); + return (false); + } + if (min_contrast_ < 0) + { + PCL_ERROR ("[pcl::%s::initCompute] : Minimum contrast (%f) must be non-negative!\n", + name_.c_str (), min_contrast_); + return (false); + } + + this->setKSearch (1); + tree_.reset (new pcl::search::KdTree (true)); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SIFTKeypoint::detectKeypoints (PointCloudOut &output) +{ + if (surface_ && surface_ != input_) + { + PCL_WARN ("[pcl::%s::detectKeypoints] : ", name_.c_str ()); + PCL_WARN ("A search surface has been set by setSearchSurface, but this SIFT keypoint detection algorithm does "); + PCL_WARN ("not support search surfaces other than the input cloud. "); + PCL_WARN ("The cloud provided in setInputCloud is being used instead.\n"); + } + + // Check if the output has a "scale" field + scale_idx_ = pcl::getFieldIndex ("scale", out_fields_); + + // Make sure the output cloud is empty + output.points.clear (); + + // Create a local copy of the input cloud that will be resized for each octave + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud (*input_)); + + VoxelGrid voxel_grid; + // Search for keypoints at each octave + float scale = min_scale_; + for (int i_octave = 0; i_octave < nr_octaves_; ++i_octave) + { + // Downsample the point cloud + const float s = 1.0f * scale; // note: this can be adjusted + voxel_grid.setLeafSize (s, s, s); + voxel_grid.setInputCloud (cloud); + typename pcl::PointCloud::Ptr temp (new pcl::PointCloud); + voxel_grid.filter (*temp); + cloud = temp; + + // Make sure the downsampled cloud still has enough points + const std::size_t min_nr_points = 25; + if (cloud->points.size () < min_nr_points) + break; + + // Update the KdTree with the downsampled points + tree_->setInputCloud (cloud); + + // Detect keypoints for the current scale + detectKeypointsForOctave (*cloud, *tree_, scale, nr_scales_per_octave_, output); + + // Increase the scale by another octave + scale *= 2; + } + + // Set final properties + output.height = 1; + output.width = static_cast (output.points.size ()); + output.header = input_->header; + output.sensor_origin_ = input_->sensor_origin_; + output.sensor_orientation_ = input_->sensor_orientation_; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SIFTKeypoint::detectKeypointsForOctave ( + const PointCloudIn &input, KdTree &tree, float base_scale, int nr_scales_per_octave, + PointCloudOut &output) +{ + // Compute the difference of Gaussians (DoG) scale space + std::vector scales (nr_scales_per_octave + 3); + for (int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale) + { + scales[i_scale] = base_scale * powf (2.0f, (1.0f * static_cast (i_scale) - 1.0f) / static_cast (nr_scales_per_octave)); + } + Eigen::MatrixXf diff_of_gauss; + computeScaleSpace (input, tree, scales, diff_of_gauss); + + // Find extrema in the DoG scale space + std::vector extrema_indices, extrema_scales; + findScaleSpaceExtrema (input, tree, diff_of_gauss, extrema_indices, extrema_scales); + + output.points.reserve (output.points.size () + extrema_indices.size ()); + // Save scale? + if (scale_idx_ != -1) + { + // Add keypoints to output + for (std::size_t i_keypoint = 0; i_keypoint < extrema_indices.size (); ++i_keypoint) + { + PointOutT keypoint; + const int &keypoint_index = extrema_indices[i_keypoint]; + + keypoint.x = input.points[keypoint_index].x; + keypoint.y = input.points[keypoint_index].y; + keypoint.z = input.points[keypoint_index].z; + memcpy (reinterpret_cast (&keypoint) + out_fields_[scale_idx_].offset, + &scales[extrema_scales[i_keypoint]], sizeof (float)); + output.points.push_back (keypoint); + } + } + else + { + // Add keypoints to output + for (const int &keypoint_index : extrema_indices) + { + PointOutT keypoint; + keypoint.x = input.points[keypoint_index].x; + keypoint.y = input.points[keypoint_index].y; + keypoint.z = input.points[keypoint_index].z; + + output.points.push_back (keypoint); + } + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::SIFTKeypoint::computeScaleSpace ( + const PointCloudIn &input, KdTree &tree, const std::vector &scales, + Eigen::MatrixXf &diff_of_gauss) +{ + diff_of_gauss.resize (input.size (), scales.size () - 1); + + // For efficiency, we will only filter over points within 3 standard deviations + const float max_radius = 3.0f * scales.back (); + + for (int i_point = 0; i_point < static_cast (input.size ()); ++i_point) + { + std::vector nn_indices; + std::vector nn_dist; + tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // * + // * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale, + // regardless of the configurable search method specified by the user, so we directly employ tree.radiusSearch + // here instead of using searchForNeighbors. + + // For each scale, compute the Gaussian "filter response" at the current point + float filter_response = 0.0f; + for (std::size_t i_scale = 0; i_scale < scales.size (); ++i_scale) + { + float sigma_sqr = powf (scales[i_scale], 2.0f); + + float numerator = 0.0f; + float denominator = 0.0f; + for (std::size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor) + { + const float &value = getFieldValue_ (input.points[nn_indices[i_neighbor]]); + const float &dist_sqr = nn_dist[i_neighbor]; + if (dist_sqr <= 9*sigma_sqr) + { + float w = std::exp (-0.5f * dist_sqr / sigma_sqr); + numerator += value * w; + denominator += w; + } + else break; // i.e. if dist > 3 standard deviations, then terminate early + } + float previous_filter_response = filter_response; + filter_response = numerator / denominator; + + // Compute the difference between adjacent scales + if (i_scale > 0) + diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SIFTKeypoint::findScaleSpaceExtrema ( + const PointCloudIn &input, KdTree &tree, const Eigen::MatrixXf &diff_of_gauss, + std::vector &extrema_indices, std::vector &extrema_scales) +{ + const int k = 25; + std::vector nn_indices (k); + std::vector nn_dist (k); + + const int nr_scales = static_cast (diff_of_gauss.cols ()); + std::vector min_val (nr_scales), max_val (nr_scales); + + for (int i_point = 0; i_point < static_cast (input.size ()); ++i_point) + { + // Define the local neighborhood around the current point + const std::size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist); //* + // * note: the neighborhood for finding local extrema is best defined as a small fixed-k neighborhood, regardless of + // the configurable search method specified by the user, so we directly employ tree.nearestKSearch here instead + // of using searchForNeighbors + + // At each scale, find the extreme values of the DoG within the current neighborhood + for (int i_scale = 0; i_scale < nr_scales; ++i_scale) + { + min_val[i_scale] = std::numeric_limits::max (); + max_val[i_scale] = -std::numeric_limits::max (); + + for (std::size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor) + { + const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale); + + min_val[i_scale] = (std::min) (min_val[i_scale], d); + max_val[i_scale] = (std::max) (max_val[i_scale], d); + } + } + + // If the current point is an extreme value with high enough contrast, add it as a keypoint + for (int i_scale = 1; i_scale < nr_scales - 1; ++i_scale) + { + const float &val = diff_of_gauss (i_point, i_scale); + + // Does the point have sufficient contrast? + if (std::abs (val) >= min_contrast_) + { + // Is it a local minimum? + if ((val == min_val[i_scale]) && + (val < min_val[i_scale - 1]) && + (val < min_val[i_scale + 1])) + { + extrema_indices.push_back (i_point); + extrema_scales.push_back (i_scale); + } + // Is it a local maximum? + else if ((val == max_val[i_scale]) && + (val > max_val[i_scale - 1]) && + (val > max_val[i_scale + 1])) + { + extrema_indices.push_back (i_point); + extrema_scales.push_back (i_scale); + } + } + } + } +} + +#define PCL_INSTANTIATE_SIFTKeypoint(T,U) template class PCL_EXPORTS pcl::SIFTKeypoint; + +#endif // #ifndef PCL_SIFT_KEYPOINT_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp new file mode 100755 index 0000000..1bf552b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/smoothed_surfaces_keypoint.hpp @@ -0,0 +1,250 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ +#define PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ + +#include + +//#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SmoothedSurfacesKeypoint::addSmoothedPointCloud (const PointCloudTConstPtr &cloud, + const PointCloudNTConstPtr &normals, + KdTreePtr &kdtree, + float &scale) +{ + clouds_.push_back (cloud); + cloud_normals_.push_back (normals); + cloud_trees_.push_back (kdtree); + scales_.push_back (std::pair (scale, scales_.size ())); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SmoothedSurfacesKeypoint::resetClouds () +{ + clouds_.clear (); + cloud_normals_.clear (); + scales_.clear (); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SmoothedSurfacesKeypoint::detectKeypoints (PointCloudT &output) +{ + // Calculate differences for each cloud + std::vector > diffs (scales_.size ()); + + // The cloud with the smallest scale has no differences + std::vector aux_diffs (input_->points.size (), 0.0f); + diffs[scales_[0].second] = aux_diffs; + + cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]); + for (std::size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i) + { + std::size_t cloud_i = scales_[scale_i].second, + cloud_i_minus_one = scales_[scale_i - 1].second; + diffs[cloud_i].resize (input_->points.size ()); + PCL_INFO ("cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one); + for (std::size_t point_i = 0; point_i < input_->points.size (); ++point_i) + diffs[cloud_i][point_i] = cloud_normals_[cloud_i]->points[point_i].getNormalVector3fMap ().dot ( + clouds_[cloud_i]->points[point_i].getVector3fMap () - clouds_[cloud_i_minus_one]->points[point_i].getVector3fMap ()); + + // Setup kdtree for this cloud + cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]); + } + + + // Find minima and maxima in differences inside the input cloud + typename pcl::search::Search::Ptr input_tree = cloud_trees_.back (); + for (int point_i = 0; point_i < static_cast (input_->points.size ()); ++point_i) + { + std::vector nn_indices; + std::vector nn_distances; + input_tree->radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances); + + bool is_min = true, is_max = true; + for (const int &nn_index : nn_indices) + if (nn_index != point_i) + { + if (diffs[input_index_][point_i] < diffs[input_index_][nn_index]) + is_max = false; + else if (diffs[input_index_][point_i] > diffs[input_index_][nn_index]) + is_min = false; + } + + // If the point is a local minimum/maximum, check if it is the same over all the scales + if (is_min || is_max) + { + bool passed_min = true, passed_max = true; + for (std::size_t scale_i = 0; scale_i < scales_.size (); ++scale_i) + { + std::size_t cloud_i = scales_[scale_i].second; + // skip input cloud + if (cloud_i == clouds_.size () - 1) + continue; + + nn_indices.clear (); nn_distances.clear (); + cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances); + + bool is_min_other_scale = true, is_max_other_scale = true; + for (const int &nn_index : nn_indices) + if (nn_index != point_i) + { + if (diffs[input_index_][point_i] < diffs[cloud_i][nn_index]) + is_max_other_scale = false; + else if (diffs[input_index_][point_i] > diffs[cloud_i][nn_index]) + is_min_other_scale = false; + } + + if (is_min && !is_min_other_scale) + passed_min = false; + if (is_max && !is_max_other_scale) + passed_max = false; + + if (!passed_min && !passed_max) + break; + } + + // check if point was minimum/maximum over all the scales + if (passed_min || passed_max) + { + output.points.push_back (input_->points[point_i]); + keypoints_indices_->indices.push_back (point_i); + } + } + } + + output.header = input_->header; + output.width = static_cast (output.points.size ()); + output.height = 1; + + // debug stuff +// for (std::size_t scale_i = 0; scale_i < scales_.size (); ++scale_i) +// { +// PointCloud::Ptr debug_cloud (new PointCloud ()); +// debug_cloud->points.resize (input_->points.size ()); +// debug_cloud->width = input_->width; +// debug_cloud->height = input_->height; +// for (std::size_t point_i = 0; point_i < input_->points.size (); ++point_i) +// { +// debug_cloud->points[point_i].intensity = diffs[scales_[scale_i].second][point_i]; +// debug_cloud->points[point_i].x = input_->points[point_i].x; +// debug_cloud->points[point_i].y = input_->points[point_i].y; +// debug_cloud->points[point_i].z = input_->points[point_i].z; +// } + +// char str[512]; sprintf (str, "diffs_%2d.pcd", scale_i); +// io::savePCDFile (str, *debug_cloud); +// } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SmoothedSurfacesKeypoint::initCompute () +{ + PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n"); + if ( !Keypoint::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_.empty ()) + { + 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 (std::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 (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 (std::size_t i = 0; i < scales_.size (); ++i) + if (scales_[i].second == scales_.size () - 1) + { + input_index_ = i; + break; + } + + PCL_INFO ("Scales: "); + for (std::size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first); + PCL_INFO ("\n"); + + return (true); +} + + +#define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint; + + +#endif /* PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/susan.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/susan.hpp new file mode 100755 index 0000000..11a220b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/susan.hpp @@ -0,0 +1,469 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SUSAN_IMPL_HPP_ +#define PCL_SUSAN_IMPL_HPP_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setNonMaxSupression (bool nonmax) +{ + nonmax_ = nonmax; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setGeometricValidation (bool validate) +{ + geometric_validation_ = validate; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setRadius (float radius) +{ + search_radius_ = radius; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setDistanceThreshold (float distance_threshold) +{ + distance_threshold_ = distance_threshold; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setAngularThreshold (float angular_threshold) +{ + angular_threshold_ = angular_threshold; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setIntensityThreshold (float intensity_threshold) +{ + intensity_threshold_ = intensity_threshold; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setNormals (const PointCloudNConstPtr &normals) +{ + normals_ = normals; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setSearchSurface (const PointCloudInConstPtr &cloud) +{ + surface_ = cloud; + normals_.reset (new pcl::PointCloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::setNumberOfThreads (unsigned int nr_threads) +{ + threads_ = nr_threads; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// template void +// pcl::SUSANKeypoint::USAN (const PointInT& nucleus, +// const NormalT& nucleus_normal, +// const std::vector& neighbors, +// const float& t, +// float& response, +// Eigen::Vector3f& centroid) const +// { +// float area = 0; +// response = 0; +// float x0 = nucleus.x; +// float y0 = nucleus.y; +// float z0 = nucleus.z; +// //xx xy xz yy yz zz +// std::vector coefficients(6); +// memset (&coefficients[0], 0, sizeof (float) * 6); +// for (std::vector::const_iterator index = neighbors.begin(); index != neighbors.end(); ++index) +// { +// if (std::isfinite (normals_->points[*index].normal_x)) +// { +// Eigen::Vector3f diff = normals_->points[*index].getNormal3fMap () - nucleus_normal.getNormal3fMap (); +// float c = diff.norm () / t; +// c = -1 * pow (c, 6.0); +// c = std::exp (c); +// Eigen::Vector3f xyz = surface_->points[*index].getVector3fMap (); +// centroid += c * xyz; +// area += c; +// coefficients[0] += c * (x0 - xyz.x) * (x0 - xyz.x); +// coefficients[1] += c * (x0 - xyz.x) * (y0 - xyz.y); +// coefficients[2] += c * (x0 - xyz.x) * (z0 - xyz.z); +// coefficients[3] += c * (y0 - xyz.y) * (y0 - xyz.y); +// coefficients[4] += c * (y0 - xyz.y) * (z0 - xyz.z); +// coefficients[5] += c * (z0 - xyz.z) * (z0 - xyz.z); +// } +// } + +// if (area > 0) +// { +// centroid /= area; +// if (area < geometric_threshold) +// response = geometric_threshold - area; +// // Look for edge direction +// // X direction +// if ((coefficients[3]/coefficients[0]) < 1 && (coefficients[5]/coefficients[0]) < 1) +// direction = Eigen::Vector3f (1, 0, 0); +// else +// { +// // Y direction +// if ((coefficients[0]/coefficients[3]) < 1 && (coefficients[5]/coefficients[3]) < 1) +// direction = Eigen::Vector3f (0, 1, 0); +// else +// { +// // Z direction +// if ((coefficients[0]/coefficients[5]) < 1 && (coefficients[3]/coefficients[5]) < 1) +// direction = Eigen::Vector3f (0, 1, 0); +// // Diagonal edge +// else +// { +// //XY direction +// if ((coefficients[2]/coeffcients[1]) < 1 && (coeffcients[4]/coeffcients[1]) < 1) +// { +// if (coeffcients[1] > 0) +// direction = Eigen::Vector3f (1,1,0); +// else +// direction = Eigen::Vector3f (-1,1,0); +// } +// else +// { +// //XZ direction +// if ((coefficients[1]/coeffcients[2]) > 1 && (coeffcients[4]/coeffcients[2]) < 1) +// { +// if (coeffcients[2] > 0) +// direction = Eigen::Vector3f (1,0,1); +// else +// direction = Eigen::Vector3f (-1,0,1); +// } +// //YZ direction +// else +// { +// if (coeffcients[4] > 0) +// direction = Eigen::Vector3f (0,1,1); +// else +// direction = Eigen::Vector3f (0,-1,1); +// } +// } +// } +// } +// } + +// // std::size_t max_index = std::distance (coefficients.begin (), max); +// // switch (max_index) +// // { +// // case 0 : direction = Eigen::Vector3f (1, 0, 0); break; +// // case 1 : direction = Eigen::Vector3f (1, 1, 0); break; +// // case 2 : direction = Eigen::Vector3f (1, 0, 1); break; +// // case 3 : direction = Eigen::Vector3f (0, 1, 0); break; +// // case 4 : direction = Eigen::Vector3f (0, 1, 1); break; +// // case 5 : direction = Eigen::Vector3f (0, 0, 1); break; +// // } +// } +// } + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SUSANKeypoint::initCompute () +{ + if (!Keypoint::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ()); + return (false); + } + + if (normals_->empty ()) + { + PointCloudNPtr normals (new PointCloudN ()); + normals->reserve (normals->size ()); + if (!surface_->isOrganized ()) + { + pcl::NormalEstimation normal_estimation; + normal_estimation.setInputCloud (surface_); + normal_estimation.setRadiusSearch (search_radius_); + normal_estimation.compute (*normals); + } + else + { + IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (surface_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normals); + } + normals_ = normals; + } + if (normals_->size () != surface_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ()); + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SUSANKeypoint::isWithinNucleusCentroid (const Eigen::Vector3f& nucleus, + const Eigen::Vector3f& centroid, + const Eigen::Vector3f& nc, + const PointInT& point) const +{ + Eigen::Vector3f pc = centroid - point.getVector3fMap (); + Eigen::Vector3f pn = nucleus - point.getVector3fMap (); + Eigen::Vector3f pc_cross_nc = pc.cross (nc); + return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0)); +} + +// template bool +// pcl::SUSANKeypoint::isValidQueryPoint3D (int point_index) const +// { +// return (isFinite (surface_->points [point_index]) && +// isFinite (normals_->points [point_index])); +// } + +// template bool +// pcl::SUSANKeypoint::isValidQueryPoint2D (int point_index) const +// { +// return (isFinite (surface_->points [point_index])); +// } + +// template bool +// pcl::SUSANKeypoint::isWithinSusan2D (int nucleus, int neighbor) const +// { +// return (std::abs (intensity_ (surface_->points[nucleus]) - +// intensity_ (surface_->points[neighbor])) <= intensity_threshold_); +// } + +// template bool +// pcl::SUSANKeypoint::isWithinSusan3D (int nucleus, int neighbor) const +// { +// Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap (); +// return (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_); +// } + +// template bool +// pcl::SUSANKeypoint::isWithinSusanH (int nucleus, int neighbor) const +// { +// Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap (); +// return ((1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_) || +// (std::abs (intensity_ (surface_->points[nucleus]) - intensity_ (surface_->points[neighbor])) <= intensity_threshold_)); +// } + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SUSANKeypoint::detectKeypoints (PointCloudOut &output) +{ + typename pcl::PointCloud::Ptr response (new pcl::PointCloud); + response->reserve (surface_->size ()); + + // Check if the output has a "label" field + label_idx_ = pcl::getFieldIndex ("label", out_fields_); + + const int input_size = static_cast (input_->size ()); +//#ifdef _OPENMP +//#pragma omp parallel for shared (response) num_threads(threads_) +//#endif + for (int point_index = 0; point_index < input_size; ++point_index) + { + const PointInT& point_in = input_->points [point_index]; + const NormalT& normal_in = normals_->points [point_index]; + if (!isFinite (point_in) || !isFinite (normal_in)) + continue; + + Eigen::Vector3f nucleus = point_in.getVector3fMap (); + Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap (); + float nucleus_intensity = intensity_ (point_in); + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists); + float area = 0; + Eigen::Vector3f centroid = Eigen::Vector3f::Zero (); + // Exclude nucleus from the usan + std::vector usan; usan.reserve (nn_indices.size () - 1); + for (std::vector::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index) + { + if ((*index != point_index) && std::isfinite (normals_->points[*index].normal_x)) + { + // if the point fulfill condition + if ((std::abs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) || + (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_)) + { + ++area; + centroid += input_->points[*index].getVector3fMap (); + usan.push_back (*index); + } + } + } + + float geometric_threshold = 0.5f * (static_cast (nn_indices.size () - 1)); + if ((area > 0) && (area < geometric_threshold)) + { + // if no geometric validation required add the point to the response + if (!geometric_validation_) + { + PointOutT point_out; + point_out.getVector3fMap () = point_in.getVector3fMap (); + //point_out.intensity = geometric_threshold - area; + intensity_out_.set (point_out, geometric_threshold - area); + // if a label field is found use it to save the index + if (label_idx_ != -1) + { + // save the index in the cloud + std::uint32_t label = static_cast (point_index); + memcpy (reinterpret_cast (&point_out) + out_fields_[label_idx_].offset, + &label, sizeof (std::uint32_t)); + } +//#ifdef _OPENMP +//#pragma omp critical +//#endif + response->push_back (point_out); + } + else + { + centroid /= area; + Eigen::Vector3f dist = nucleus - centroid; + // Check the distance <= distance_threshold_ + if (dist.norm () >= distance_threshold_) + { + // point is valid from distance point of view + Eigen::Vector3f nc = centroid - nucleus; + // Check the contiguity + auto usan_it = usan.cbegin (); + for (; usan_it != usan.cend (); ++usan_it) + { + if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it])) + break; + } + // All points within usan lies on the segment [nucleus centroid] + if (usan_it == usan.end ()) + { + PointOutT point_out; + point_out.getVector3fMap () = point_in.getVector3fMap (); + // point_out.intensity = geometric_threshold - area; + intensity_out_.set (point_out, geometric_threshold - area); + // if a label field is found use it to save the index + if (label_idx_ != -1) + { + // save the index in the cloud + std::uint32_t label = static_cast (point_index); + memcpy (reinterpret_cast (&point_out) + out_fields_[label_idx_].offset, + &label, sizeof (std::uint32_t)); + } +//#ifdef _OPENMP +//#pragma omp critical +//#endif + response->push_back (point_out); + } + } + } + } + } + + response->height = 1; + response->width = static_cast (response->size ()); + + if (!nonmax_) + { + output = *response; + for (std::size_t i = 0; i < response->size (); ++i) + keypoints_indices_->indices.push_back (i); + // we don not change the denseness + output.is_dense = input_->is_dense; + } + else + { + output.points.clear (); + output.points.reserve (response->points.size()); + +//#ifdef _OPENMP +//#pragma omp parallel for shared (output) num_threads(threads_) +//#endif + for (int idx = 0; idx < static_cast (response->points.size ()); ++idx) + { + const PointOutT& point_in = response->points [idx]; + const NormalT& normal_in = normals_->points [idx]; + //const float intensity = response->points[idx].intensity; + const float intensity = intensity_out_ (response->points[idx]); + if (!isFinite (point_in) || !isFinite (normal_in) || (intensity == 0)) + continue; + std::vector nn_indices; + std::vector nn_dists; + tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists); + bool is_minima = true; + for (std::vector::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it) + { +// if (intensity > response->points[*nn_it].intensity) + if (intensity > intensity_out_ (response->points[*nn_it])) + { + is_minima = false; + break; + } + } + if (is_minima) +//#ifdef _OPENMP +//#pragma omp critical +//#endif + { + output.points.push_back (response->points[idx]); + keypoints_indices_->indices.push_back (idx); + } + } + + output.height = 1; + output.width = static_cast (output.points.size()); + output.is_dense = true; + } +} + +#define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint; +#endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/trajkovic_2d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/trajkovic_2d.hpp new file mode 100755 index 0000000..b9c83ed --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/trajkovic_2d.hpp @@ -0,0 +1,256 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ +#define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ + +template bool +pcl::TrajkovicKeypoint2D::initCompute () +{ + if (!PCLBase::initCompute ()) + return (false); + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); + return (false); + } + + if (indices_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); + return (false); + } + + if ((window_size_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ()); + return (false); + } + + if (window_size_ < 3) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TrajkovicKeypoint2D::detectKeypoints (PointCloudOut &output) +{ + response_.reset (new pcl::PointCloud (input_->width, input_->height)); + int w = static_cast (input_->width) - half_window_size_; + int h = static_cast (input_->height) - half_window_size_; + + if (method_ == pcl::TrajkovicKeypoint2D::FOUR_CORNERS) + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + float center = intensity_ ((*input_) (i,j)); + float up = intensity_ ((*input_) (i, j-half_window_size_)); + float down = intensity_ ((*input_) (i, j+half_window_size_)); + float left = intensity_ ((*input_) (i-half_window_size_, j)); + float right = intensity_ ((*input_) (i+half_window_size_, j)); + + float up_center = up - center; + float r1 = up_center * up_center; + float down_center = down - center; + r1+= down_center * down_center; + + float right_center = right - center; + float r2 = right_center * right_center; + float left_center = left - center; + r2+= left_center * left_center; + + float d = std::min (r1, r2); + + if (d < first_threshold_) + continue; + + float b1 = (right - up) * up_center; + b1+= (left - down) * down_center; + float b2 = (right - down) * down_center; + b2+= (left - up) * up_center; + float B = std::min (b1, b2); + float A = r2 - r1 - 2*B; + + (*response_) (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; + } + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + float center = intensity_ ((*input_) (i,j)); + float up = intensity_ ((*input_) (i, j-half_window_size_)); + float down = intensity_ ((*input_) (i, j+half_window_size_)); + float left = intensity_ ((*input_) (i-half_window_size_, j)); + float right = intensity_ ((*input_) (i+half_window_size_, j)); + float upleft = intensity_ ((*input_) (i-half_window_size_, j-half_window_size_)); + float upright = intensity_ ((*input_) (i+half_window_size_, j-half_window_size_)); + float downleft = intensity_ ((*input_) (i-half_window_size_, j+half_window_size_)); + float downright = intensity_ ((*input_) (i+half_window_size_, j+half_window_size_)); + std::vector r (4,0); + + float up_center = up - center; + r[0] = up_center * up_center; + float down_center = down - center; + r[0]+= down_center * down_center; + + float upright_center = upright - center; + r[1] = upright_center * upright_center; + float downleft_center = downleft - center; + r[1]+= downleft_center * downleft_center; + + float right_center = right - center; + r[2] = right_center * right_center; + float left_center = left - center; + r[2]+= left_center * left_center; + + float downright_center = downright - center; + r[3] = downright_center * downright_center; + float upleft_center = upleft - center; + r[3]+= upleft_center * upleft_center; + + float d = *(std::min_element (r.begin (), r.end ())); + + if (d < first_threshold_) + continue; + + std::vector B (4,0); + std::vector A (4,0); + std::vector sumAB (4,0); + B[0] = (upright - up) * up_center; + B[0]+= (downleft - down) * down_center; + B[1] = (right - upright) * upright_center; + B[1]+= (left - downleft) * downleft_center; + B[2] = (downright - right) * downright_center; + B[2]+= (upleft - left) * upleft_center; + B[3] = (down - downright) * downright_center; + B[3]+= (up - upleft) * upleft_center; + A[0] = r[1] - r[0] - B[0] - B[0]; + A[1] = r[2] - r[1] - B[1] - B[1]; + A[2] = r[3] - r[2] - B[2] - B[2]; + A[3] = r[0] - r[3] - B[3] - B[3]; + sumAB[0] = A[0] + B[0]; + sumAB[1] = A[1] + B[1]; + sumAB[2] = A[2] + B[2]; + sumAB[3] = A[3] + B[3]; + if ((*std::max_element (B.begin (), B.end ()) < 0) && + (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) + { + std::vector D (4,0); + D[0] = B[0] * B[0] / A[0]; + D[1] = B[1] * B[1] / A[1]; + D[2] = B[2] * B[2] / A[2]; + D[3] = B[3] * B[3] / A[3]; + (*response_) (i,j) = *(std::min (D.begin (), D.end ())); + } + else + (*response_) (i,j) = d; + } + } + } + + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), [this] (int p1, int p2) { return greaterCornernessAtIndices (p1, p2); }); + + output.clear (); + output.reserve (input_->size ()); + + std::vector occupency_map (indices.size (), false); + const int width (input_->width); + const int height (input_->height); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for (std::size_t i = 0; i < indices.size (); ++i) + { + int idx = indices[i]; + if ((response_->points[idx] < second_threshold_) || occupency_map[idx]) + continue; + + PointOutT p; + p.getVector3fMap () = input_->points[idx].getVector3fMap (); + p.intensity = response_->points [idx]; + +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.push_back (p); + keypoints_indices_->indices.push_back (idx); + } + + const int x = idx % width; + const int y = idx / width; + const int u_end = std::min (width, x + half_window_size_); + const int v_end = std::min (height, y + half_window_size_); + for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) + for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) + occupency_map[v*width + u] = true; + } + + output.height = 1; + output.width = static_cast (output.size()); + // we don not change the denseness + output.is_dense = input_->is_dense; +} + +#define PCL_INSTANTIATE_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/impl/trajkovic_3d.hpp b/deps_install/include/pcl-1.10/pcl/keypoints/impl/trajkovic_3d.hpp new file mode 100755 index 0000000..a9a6797 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/impl/trajkovic_3d.hpp @@ -0,0 +1,270 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_TRAJKOVIC_KEYPOINT_3D_IMPL_H_ +#define PCL_TRAJKOVIC_KEYPOINT_3D_IMPL_H_ + +#include + +template bool +pcl::TrajkovicKeypoint3D::initCompute () +{ + if (!PCLBase::initCompute ()) + return (false); + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + if (indices_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); + return (false); + } + + if ((window_size_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ()); + return (false); + } + + if (window_size_ < 3) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + + if (!normals_) + { + NormalsPtr normals (new Normals ()); + pcl::IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (input_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normals); + normals_ = normals; + } + + if (normals_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ()); + return (false); + } + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TrajkovicKeypoint3D::detectKeypoints (PointCloudOut &output) +{ + response_.reset (new pcl::PointCloud (input_->width, input_->height)); + const Normals &normals = *normals_; + const PointCloudIn &input = *input_; + pcl::PointCloud& response = *response_; + const int w = static_cast (input_->width) - half_window_size_; + const int h = static_cast (input_->height) - half_window_size_; + + if (method_ == FOUR_CORNERS) + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + if (!isFinite (input (i,j))) continue; + const NormalT ¢er = normals (i,j); + if (!isFinite (center)) continue; + + int count = 0; + const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); + const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); + const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); + const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); + // Get rid of isolated points + if (!count) continue; + + float sn1 = squaredNormalsDiff (up, center); + float sn2 = squaredNormalsDiff (down, center); + float r1 = sn1 + sn2; + float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center); + + float d = std::min (r1, r2); + if (d < first_threshold_) continue; + + sn1 = std::sqrt (sn1); + sn2 = std::sqrt (sn2); + float b1 = normalsDiff (right, up) * sn1; + b1+= normalsDiff (left, down) * sn2; + float b2 = normalsDiff (right, down) * sn2; + b2+= normalsDiff (left, up) * sn1; + float B = std::min (b1, b2); + float A = r2 - r1 - 2*B; + + response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; + } + } + } + else + { +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + if (!isFinite (input (i,j))) continue; + const NormalT ¢er = normals (i,j); + if (!isFinite (center)) continue; + + int count = 0; + const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); + const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); + const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); + const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); + const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count); + const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count); + const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count); + const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count); + // Get rid of isolated points + if (!count) continue; + + std::vector r (4,0); + + r[0] = squaredNormalsDiff (up, center); + r[0]+= squaredNormalsDiff (down, center); + + r[1] = squaredNormalsDiff (upright, center); + r[1]+= squaredNormalsDiff (downleft, center); + + r[2] = squaredNormalsDiff (right, center); + r[2]+= squaredNormalsDiff (left, center); + + r[3] = squaredNormalsDiff (downright, center); + r[3]+= squaredNormalsDiff (upleft, center); + + float d = *(std::min_element (r.begin (), r.end ())); + + if (d < first_threshold_) continue; + + std::vector B (4,0); + std::vector A (4,0); + std::vector sumAB (4,0); + B[0] = normalsDiff (upright, up) * normalsDiff (up, center); + B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center); + B[1] = normalsDiff (right, upright) * normalsDiff (upright, center); + B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center); + B[2] = normalsDiff (downright, right) * normalsDiff (downright, center); + B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center); + B[3] = normalsDiff (down, downright) * normalsDiff (downright, center); + B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center); + A[0] = r[1] - r[0] - B[0] - B[0]; + A[1] = r[2] - r[1] - B[1] - B[1]; + A[2] = r[3] - r[2] - B[2] - B[2]; + A[3] = r[0] - r[3] - B[3] - B[3]; + sumAB[0] = A[0] + B[0]; + sumAB[1] = A[1] + B[1]; + sumAB[2] = A[2] + B[2]; + sumAB[3] = A[3] + B[3]; + if ((*std::max_element (B.begin (), B.end ()) < 0) && + (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) + { + std::vector D (4,0); + D[0] = B[0] * B[0] / A[0]; + D[1] = B[1] * B[1] / A[1]; + D[2] = B[2] * B[2] / A[2]; + D[3] = B[3] * B[3] / A[3]; + response (i,j) = *(std::min (D.begin (), D.end ())); + } + else + response (i,j) = d; + } + } + } + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), [this] (int p1, int p2) { return greaterCornernessAtIndices (p1, p2); }); + + output.clear (); + output.reserve (input_->size ()); + + std::vector occupency_map (indices.size (), false); + const int width (input_->width); + const int height (input_->height); + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for (int i = 0; i < static_cast(indices.size ()); ++i) + { + int idx = indices[static_cast(i)]; + if ((response_->points[idx] < second_threshold_) || occupency_map[idx]) + continue; + + PointOutT p; + p.getVector3fMap () = input_->points[idx].getVector3fMap (); + p.intensity = response_->points [idx]; + +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.push_back (p); + keypoints_indices_->indices.push_back (idx); + } + + const int x = idx % width; + const int y = idx / width; + const int u_end = std::min (width, x + half_window_size_); + const int v_end = std::min (height, y + half_window_size_); + for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) + for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) + occupency_map[v*width + u] = true; + } + + output.height = 1; + output.width = static_cast (output.size()); + // we don not change the denseness + output.is_dense = true; +} + +#define PCL_INSTANTIATE_TrajkovicKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::TrajkovicKeypoint3D; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/iss_3d.h b/deps_install/include/pcl-1.10/pcl/keypoints/iss_3d.h new file mode 100755 index 0000000..a71a899 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/iss_3d.h @@ -0,0 +1,273 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief ISSKeypoint3D detects the Intrinsic Shape Signatures keypoints for a given + * point cloud. This class is based on a particular implementation made by Federico + * Tombari and Samuele Salti and it has been explicitly adapted to PCL. + * + * For more information about the original ISS detector, see: + * + *\par + * Yu Zhong, “Intrinsic shape signatures: A shape descriptor for 3D object recognition,” + * Computer Vision Workshops (ICCV Workshops), 2009 IEEE 12th International Conference on , + * vol., no., pp.689-696, Sept. 27 2009-Oct. 4 2009 + * + * Code example: + * + * \code + * pcl::PointCloud::Ptr model (new pcl::PointCloud ());; + * pcl::PointCloud::Ptr model_keypoints (new pcl::PointCloud ()); + * pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); + * + * // Fill in the model cloud + * + * double model_resolution; + * + * // Compute model_resolution + * + * pcl::ISSKeypoint3D iss_detector; + * + * iss_detector.setSearchMethod (tree); + * iss_detector.setSalientRadius (6 * model_resolution); + * iss_detector.setNonMaxRadius (4 * model_resolution); + * iss_detector.setThreshold21 (0.975); + * iss_detector.setThreshold32 (0.975); + * iss_detector.setMinNeighbors (5); + * iss_detector.setNumberOfThreads (4); + * iss_detector.setInputCloud (model); + * iss_detector.compute (*model_keypoints); + * \endcode + * + * \author Gioia Ballin + * \ingroup keypoints + */ + + template + class ISSKeypoint3D : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using OctreeSearchIn = pcl::octree::OctreePointCloudSearch; + using OctreeSearchInPtr = typename OctreeSearchIn::Ptr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::surface_; + using Keypoint::tree_; + using Keypoint::search_radius_; + using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; + + /** \brief Constructor. + * \param[in] salient_radius the radius of the spherical neighborhood used to compute the scatter matrix. + */ + ISSKeypoint3D (double salient_radius = 0.0001) + : salient_radius_ (salient_radius) + , non_max_radius_ (0.0) + , normal_radius_ (0.0) + , border_radius_ (0.0) + , gamma_21_ (0.975) + , gamma_32_ (0.975) + , third_eigen_value_ (nullptr) + , edge_points_ (nullptr) + , min_neighbors_ (5) + , normals_ (new pcl::PointCloud) + , angle_threshold_ (static_cast (M_PI) / 2.0f) + , threads_ (0) + { + name_ = "ISSKeypoint3D"; + search_radius_ = salient_radius_; + } + + /** \brief Destructor. */ + ~ISSKeypoint3D () + { + delete[] third_eigen_value_; + delete[] edge_points_; + } + + /** \brief Set the radius of the spherical neighborhood used to compute the scatter matrix. + * \param[in] salient_radius the radius of the spherical neighborhood + */ + void + setSalientRadius (double salient_radius); + + /** \brief Set the radius for the application of the non maxima supression algorithm. + * \param[in] non_max_radius the non maxima suppression radius + */ + void + setNonMaxRadius (double non_max_radius); + + /** \brief Set the radius used for the estimation of the surface normals of the input cloud. If the radius is + * too large, the temporal performances of the detector may degrade significantly. + * \param[in] normal_radius the radius used to estimate surface normals + */ + void + setNormalRadius (double normal_radius); + + /** \brief Set the radius used for the estimation of the boundary points. If the radius is too large, + * the temporal performances of the detector may degrade significantly. + * \param[in] border_radius the radius used to compute the boundary points + */ + void + setBorderRadius (double border_radius); + + /** \brief Set the upper bound on the ratio between the second and the first eigenvalue. + * \param[in] gamma_21 the upper bound on the ratio between the second and the first eigenvalue + */ + void + setThreshold21 (double gamma_21); + + /** \brief Set the upper bound on the ratio between the third and the second eigenvalue. + * \param[in] gamma_32 the upper bound on the ratio between the third and the second eigenvalue + */ + void + setThreshold32 (double gamma_32); + + /** \brief Set the minimum number of neighbors that has to be found while applying the non maxima suppression algorithm. + * \param[in] min_neighbors the minimum number of neighbors required + */ + void + setMinNeighbors (int min_neighbors); + + /** \brief Set the normals if pre-calculated normals are available. + * \param[in] normals the given cloud of normals + */ + void + setNormals (const PointCloudNConstPtr &normals); + + /** \brief Set the decision boundary (angle threshold) that marks points as boundary or regular. + * (default \f$\pi / 2.0\f$) + * \param[in] angle the angle threshold + */ + inline void + setAngleThreshold (float angle) + { + angle_threshold_ = angle; + } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param[in] nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + protected: + + /** \brief Compute the boundary points for the given input cloud. + * \param[in] input the input cloud + * \param[in] border_radius the radius used to compute the boundary points + * \param[in] angle_threshold the decision boundary that marks the points as boundary + * \return the vector of boolean values in which the information about the boundary points is stored + */ + bool* + getBoundaryPoints (PointCloudIn &input, double border_radius, float angle_threshold); + + /** \brief Compute the scatter matrix for a point index. + * \param[in] current_index the index of the point + * \param[out] cov_m the point scatter matrix + */ + void + getScatterMatrix (const int ¤t_index, Eigen::Matrix3d &cov_m); + + /** \brief Perform the initial checks before computing the keypoints. + * \return true if all the checks are passed, false otherwise + */ + bool + initCompute () override; + + /** \brief Detect the keypoints by performing the EVD of the scatter matrix. + * \param[out] output the resultant cloud of keypoints + */ + void + detectKeypoints (PointCloudOut &output) override; + + + /** \brief The radius of the spherical neighborhood used to compute the scatter matrix.*/ + double salient_radius_; + + /** \brief The non maxima suppression radius. */ + double non_max_radius_; + + /** \brief The radius used to compute the normals of the input cloud. */ + double normal_radius_; + + /** \brief The radius used to compute the boundary points of the input cloud. */ + double border_radius_; + + /** \brief The upper bound on the ratio between the second and the first eigenvalue returned by the EVD. */ + double gamma_21_; + + /** \brief The upper bound on the ratio between the third and the second eigenvalue returned by the EVD. */ + double gamma_32_; + + /** \brief Store the third eigen value associated to each point in the input cloud. */ + double *third_eigen_value_; + + /** \brief Store the information about the boundary points of the input cloud. */ + bool *edge_points_; + + /** \brief Minimum number of neighbors that has to be found while applying the non maxima suppression algorithm. */ + int min_neighbors_; + + /** \brief The cloud of normals related to the input surface. */ + PointCloudNConstPtr normals_; + + /** \brief The decision boundary (angle threshold) that marks points as boundary or regular. (default \f$\pi / 2.0\f$) */ + float angle_threshold_; + + /** \brief The number of threads that has to be used by the scheduler. */ + unsigned int threads_; + + }; + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/keypoint.h b/deps_install/include/pcl-1.10/pcl/keypoints/keypoint.h new file mode 100755 index 0000000..f019d76 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/keypoint.h @@ -0,0 +1,205 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +// PCL includes +#include +#include +#include + +#include + +namespace pcl +{ + /** \brief @b Keypoint represents the base class for key points. + * \author Bastian Steder + * \ingroup keypoints + */ + template + class Keypoint : public PCLBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PCLBase::indices_; + using PCLBase::input_; + + using BaseClass = PCLBase; + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + using PointCloudOut = pcl::PointCloud; + using SearchMethod = std::function &, std::vector &)>; + using SearchMethodSurface = std::function &, std::vector &)>; + + public: + /** \brief Empty constructor. */ + Keypoint () : + BaseClass (), + search_method_surface_ (), + surface_ (), + tree_ (), + search_parameter_ (0), + search_radius_ (0), + k_ (0) + {}; + + /** \brief Empty destructor */ + ~Keypoint () {} + + /** \brief Provide a pointer to the input dataset that we need to estimate features at every point for. + * \param cloud the const boost shared pointer to a PointCloud message + */ + virtual void + setSearchSurface (const PointCloudInConstPtr &cloud) { surface_ = cloud; } + + /** \brief Get a pointer to the surface point cloud dataset. */ + inline PointCloudInConstPtr + getSearchSurface () { return (surface_); } + + /** \brief Provide a pointer to the search object. + * \param tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () { return (tree_); } + + /** \brief Get the internal search parameter. */ + inline double + getSearchParameter () { return (search_parameter_); } + + /** \brief Set the number of k nearest neighbors to use for the feature estimation. + * \param k the number of k-nearest neighbors + */ + inline void + setKSearch (int k) { k_ = k; } + + /** \brief get the number of k nearest neighbors used for the feature estimation. */ + inline int + getKSearch () { return (k_); } + + /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the + * key point detection + * \param radius the sphere radius used as the maximum distance to consider a point a neighbor + */ + inline void + setRadiusSearch (double radius) { search_radius_ = radius; } + + /** \brief Get the sphere radius used for determining the neighbors. */ + inline double + getRadiusSearch () { return (search_radius_); } + + /** \brief \return the keypoints indices in the input cloud. + * \note not all the daughter classes populate the keypoints indices so check emptiness before use. + */ + pcl::PointIndicesConstPtr + getKeypointsIndices () { return (keypoints_indices_); } + + /** \brief Base method for key point detection for all points given in using + * the surface in setSearchSurface () and the spatial locator in setSearchMethod () + * \param output the resultant point cloud model dataset containing the estimated features + */ + inline void + compute (PointCloudOut &output); + + /** \brief Search for k-nearest neighbors using the spatial locator from \a setSearchmethod, and the given surface + * from \a setSearchSurface. + * \param index the index of the query point + * \param parameter the search parameter (either k or radius) + * \param indices the resultant vector of indices representing the k-nearest neighbors + * \param distances the resultant vector of distances representing the distances from the query point to the + * k-nearest neighbors + */ + inline int + searchForNeighbors (int index, double parameter, std::vector &indices, std::vector &distances) const + { + if (surface_ == input_) // if the two surfaces are the same + return (search_method_ (index, parameter, indices, distances)); + return (search_method_surface_ (*input_, index, parameter, indices, distances)); + } + + protected: + using PCLBase::deinitCompute; + + virtual bool + initCompute (); + + /** \brief The key point detection method's name. */ + std::string name_; + + /** \brief The search method template for indices. */ + SearchMethod search_method_; + + /** \brief The search method template for points. */ + SearchMethodSurface search_method_surface_; + + /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */ + PointCloudInConstPtr surface_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The actual search parameter (casted from either \a search_radius_ or \a k_). */ + double search_parameter_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief The number of K nearest neighbors to use for each point. */ + int k_; + + /** \brief Indices of the keypoints in the input cloud. */ + pcl::PointIndicesPtr keypoints_indices_; + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const { return (name_); } + + /** \brief Abstract key point detection method. */ + virtual void + detectKeypoints (PointCloudOut &output) = 0; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/narf_keypoint.h b/deps_install/include/pcl-1.10/pcl/keypoints/narf_keypoint.h new file mode 100755 index 0000000..29c72f0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/narf_keypoint.h @@ -0,0 +1,203 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +/* \author Bastian Steder */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl { + +// Forward declarations +class RangeImage; +class RangeImageBorderExtractor; + +/** \brief @b NARF (Normal Aligned Radial Feature) keypoints. Input is a range image, + * output the indices of the keypoints + * See B. Steder, R. B. Rusu, K. Konolige, and W. Burgard + * Point Feature Extraction on 3D Range Scans Taking into Account Object Boundaries + * In Proc. of the IEEE Int. Conf. on Robotics &Automation (ICRA). 2011. + * \author Bastian Steder + * \ingroup keypoints + */ +class PCL_EXPORTS NarfKeypoint : public Keypoint +{ + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + // =====TYPEDEFS===== + using BaseClass = Keypoint; + + using PointCloudOut = Keypoint::PointCloudOut; + + // =====PUBLIC STRUCTS===== + //! Parameters used in this class + struct Parameters + { + Parameters() : support_size(-1.0f), max_no_of_interest_points(-1), min_distance_between_interest_points(0.25f), + optimal_distance_to_high_surface_change(0.25), min_interest_value(0.45f), + min_surface_change_score(0.2f), optimal_range_image_patch_size(10), + distance_for_additional_points(0.0f), add_points_on_straight_edges(false), + do_non_maximum_suppression(true), no_of_polynomial_approximations_per_point(false), + max_no_of_threads(1), use_recursive_scale_reduction(false), + calculate_sparse_interest_image(true) {} + + float support_size; //!< This defines the area 'covered' by an interest point (in meters) + int max_no_of_interest_points; //!< The maximum number of interest points that will be returned + float min_distance_between_interest_points; /**< Minimum distance between maximas + * (this is a factor for support_size, i.e. the distance is + * min_distance_between_interest_points*support_size) */ + float optimal_distance_to_high_surface_change; /**< The distance we want keep between keypoints and areas + * of high surface change + * (this is a factor for support_size, i.e., the distance is + * optimal_distance_to_high_surface_change*support_size) */ + float min_interest_value; //!< The minimum value to consider a point as an interest point + float min_surface_change_score; //!< The minimum value of the surface change score to consider a point + int optimal_range_image_patch_size; /**< The size (in pixels) of the image patches from which the interest value + * should be computed. This influences, which range image is selected from + * the scale space to compute the interest value of a pixel at a certain + * distance. */ + // TODO: + float distance_for_additional_points; /**< All points in this distance to a found maximum, that + * are above min_interest_value are also added as interest points + * (this is a factor for support_size, i.e. the distance is + * distance_for_additional_points*support_size) */ + bool add_points_on_straight_edges; /**< If this is set to true, there will also be interest points on + * straight edges, e.g., just indicating an area of high surface change */ + bool do_non_maximum_suppression; /**< If this is set to false there will be much more points + * (can be used to spread points over the whole scene + * (combined with a low min_interest_value)) */ + bool no_of_polynomial_approximations_per_point; /**< If this is >0, the exact position of the interest point is + determined using bivariate polynomial approximations of the + interest values of the area. */ + int max_no_of_threads; //!< The maximum number of threads this code is allowed to use with OPNEMP + bool use_recursive_scale_reduction; /**< Try to decrease runtime by extracting interest points at lower reolution + * in areas that contain enough points, i.e., have lower range. */ + bool calculate_sparse_interest_image; /**< Use some heuristics to decide which areas of the interest image + can be left out to improve the runtime. */ + }; + + // =====CONSTRUCTOR & DESTRUCTOR===== + NarfKeypoint (RangeImageBorderExtractor* range_image_border_extractor=nullptr, float support_size=-1.0f); + ~NarfKeypoint (); + + // =====PUBLIC METHODS===== + //! Erase all data calculated for the current range image + void + clearData (); + + //! Set the RangeImageBorderExtractor member (required) + void + setRangeImageBorderExtractor (RangeImageBorderExtractor* range_image_border_extractor); + + //! Get the RangeImageBorderExtractor member + RangeImageBorderExtractor* + getRangeImageBorderExtractor () { return range_image_border_extractor_; } + + //! Set the RangeImage member of the RangeImageBorderExtractor + void + setRangeImage (const RangeImage* range_image); + + /** Extract interest value per image point */ + float* + getInterestImage () { calculateInterestImage(); return interest_image_;} + + //! Extract maxima from an interest image + const ::pcl::PointCloud& + getInterestPoints () { calculateInterestPoints(); return *interest_points_;} + + //! Set all points in the image that are interest points to true, the rest to false + const std::vector& + getIsInterestPointImage () { calculateInterestPoints(); return is_interest_point_image_;} + + //! Getter for the parameter struct + Parameters& + getParameters () { return parameters_;} + + //! Getter for the range image of range_image_border_extractor_ + const RangeImage& + getRangeImage (); + + //! Overwrite the compute function of the base class + void + compute (PointCloudOut& output); + + protected: + // =====PROTECTED METHODS===== + void + calculateScaleSpace (); + void + calculateInterestImage (); + void + calculateCompleteInterestImage (); + void + calculateSparseInterestImage (); + void + calculateInterestPoints (); + //void + //blurInterestImage (); + //! Detect key points + void + detectKeypoints (PointCloudOut& output) override; + + // =====PROTECTED MEMBER VARIABLES===== + using BaseClass::name_; + RangeImageBorderExtractor* range_image_border_extractor_; + Parameters parameters_; + float* interest_image_; + ::pcl::PointCloud* interest_points_; + std::vector is_interest_point_image_; + std::vector range_image_scale_space_; + std::vector border_extractor_scale_space_; + std::vector interest_image_scale_space_; +}; + +/** + * \ingroup keypoints + */ +inline std::ostream& + operator << (std::ostream& os, const NarfKeypoint::Parameters& p) +{ + os << PVARC(p.support_size) << PVARC(p.min_distance_between_interest_points) + << PVARC(p.min_interest_value) << PVARN(p.distance_for_additional_points); + return (os); +} + +} // end namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/sift_keypoint.h b/deps_install/include/pcl-1.10/pcl/keypoints/sift_keypoint.h new file mode 100755 index 0000000..4e33d5f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/sift_keypoint.h @@ -0,0 +1,203 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + template + struct SIFTKeypointFieldSelector + { + inline float + operator () (const PointT & p) const + { + return p.intensity; + } + }; + template<> + struct SIFTKeypointFieldSelector + { + inline float + operator () (const PointNormal & p) const + { + return p.curvature; + } + }; + template<> + struct SIFTKeypointFieldSelector + { + inline float + operator () (const PointXYZRGB & p) const + { + return (static_cast (299*p.r + 587*p.g + 114*p.b) / 1000.0f); + } + }; + template<> + struct SIFTKeypointFieldSelector + { + inline float + operator () (const PointXYZRGBA & p) const + { + return (static_cast (299*p.r + 587*p.g + 114*p.b) / 1000.0f); + } + }; + + /** \brief @b SIFTKeypoint detects the Scale Invariant Feature Transform + * keypoints for a given point cloud dataset containing points and intensity. + * This implementation adapts the original algorithm from images to point + * clouds. + * + * For more information about the image-based SIFT interest operator, see: + * + * David G. Lowe, "Distinctive image features from scale-invariant keypoints," + * International Journal of Computer Vision, 60, 2 (2004), pp. 91-110. + * + * \author Michael Dixon + * \ingroup keypoints + */ + template + class SIFTKeypoint : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using KdTree = typename Keypoint::KdTree; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::surface_; + using Keypoint::tree_; + using Keypoint::initCompute; + + /** \brief Empty constructor. */ + SIFTKeypoint () : min_scale_ (0.0), nr_octaves_ (0), nr_scales_per_octave_ (0), + min_contrast_ (-std::numeric_limits::max ()), scale_idx_ (-1), + getFieldValue_ () + { + name_ = "SIFTKeypoint"; + } + + /** \brief Specify the range of scales over which to search for keypoints + * \param min_scale the standard deviation of the smallest scale in the scale space + * \param nr_octaves the number of octaves (i.e. doublings of scale) to compute + * \param nr_scales_per_octave the number of scales to compute within each octave + */ + void + setScales (float min_scale, int nr_octaves, int nr_scales_per_octave); + + /** \brief Provide a threshold to limit detection of keypoints without sufficient contrast + * \param min_contrast the minimum contrast required for detection + */ + void + setMinimumContrast (float min_contrast); + + protected: + bool + initCompute () override; + + /** \brief Detect the SIFT keypoints for a set of points given in setInputCloud () using the spatial locator in + * setSearchMethod (). + * \param output the resultant cloud of keypoints + */ + void + detectKeypoints (PointCloudOut &output) override; + + private: + /** \brief Detect the SIFT keypoints for a given point cloud for a single octave. + * \param input the point cloud to detect keypoints in + * \param tree a k-D tree of the points in \a input + * \param base_scale the first (smallest) scale in the octave + * \param nr_scales_per_octave the number of scales to to compute + * \param output the resultant point cloud containing the SIFT keypoints + */ + void + detectKeypointsForOctave (const PointCloudIn &input, KdTree &tree, + float base_scale, int nr_scales_per_octave, + PointCloudOut &output); + + /** \brief Compute the difference-of-Gaussian (DoG) scale space for the given input and scales + * \param input the point cloud for which the DoG scale space will be computed + * \param tree a k-D tree of the points in \a input + * \param scales a vector containing the scales over which to compute the DoG scale space + * \param diff_of_gauss the resultant DoG scale space (in a number-of-points by number-of-scales matrix) + */ + void + computeScaleSpace (const PointCloudIn &input, KdTree &tree, + const std::vector &scales, + Eigen::MatrixXf &diff_of_gauss); + + /** \brief Find the local minima and maxima in the provided difference-of-Gaussian (DoG) scale space + * \param input the input point cloud + * \param tree a k-D tree of the points in \a input + * \param diff_of_gauss the DoG scale space (in a number-of-points by number-of-scales matrix) + * \param extrema_indices the resultant vector containing the point indices of each keypoint + * \param extrema_scales the resultant vector containing the scale indices of each keypoint + */ + void + findScaleSpaceExtrema (const PointCloudIn &input, KdTree &tree, + const Eigen::MatrixXf &diff_of_gauss, + std::vector &extrema_indices, std::vector &extrema_scales); + + + /** \brief The standard deviation of the smallest scale in the scale space.*/ + float min_scale_; + + /** \brief The number of octaves (i.e. doublings of scale) over which to search for keypoints.*/ + int nr_octaves_; + + /** \brief The number of scales to be computed for each octave.*/ + int nr_scales_per_octave_; + + /** \brief The minimum contrast required for detection.*/ + float min_contrast_; + + /** \brief Set to a value different than -1 if the output cloud has a "scale" field and we have to save + * the keypoints scales. */ + int scale_idx_; + + /** \brief The list of fields present in the output point cloud data. */ + std::vector out_fields_; + + SIFTKeypointFieldSelector getFieldValue_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/smoothed_surfaces_keypoint.h b/deps_install/include/pcl-1.10/pcl/keypoints/smoothed_surfaces_keypoint.h new file mode 100755 index 0000000..bc02196 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/smoothed_surfaces_keypoint.h @@ -0,0 +1,132 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief + * Based on the paper: + * Xinju Li and Igor Guskov + * Multi-scale features for approximate alignment of point-based surfaces + * Proceedings of the third Eurographics symposium on Geometry processing + * July 2005, Vienna, Austria + * + * \author Alexandru-Eugen Ichim + */ + template + class SmoothedSurfacesKeypoint : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PCLBase::input_; + using Keypoint::name_; + using Keypoint::tree_; + using Keypoint::keypoints_indices_; + using Keypoint::initCompute; + + using PointCloudT = pcl::PointCloud; + using PointCloudTConstPtr = typename PointCloudT::ConstPtr; + using PointCloudNT = pcl::PointCloud; + using PointCloudNTConstPtr = typename PointCloudNT::ConstPtr; + using PointCloudTPtr = typename PointCloudT::Ptr; + using KdTreePtr = typename Keypoint::KdTreePtr; + + SmoothedSurfacesKeypoint () + : Keypoint (), + neighborhood_constant_ (0.5f), + clouds_ (), + cloud_normals_ (), + cloud_trees_ (), + normals_ (), + input_scale_ (0.0f), + input_index_ () + { + name_ = "SmoothedSurfacesKeypoint"; + + // hack to pass the initCompute () check of Keypoint - although it is never used in SmoothedSurfacesKeypoint + Keypoint::search_radius_ = 0.1; + } + + void + addSmoothedPointCloud (const PointCloudTConstPtr &cloud, + const PointCloudNTConstPtr &normals, + KdTreePtr &kdtree, + float &scale); + + + void + resetClouds (); + + inline void + setNeighborhoodConstant (float neighborhood_constant) { neighborhood_constant_ = neighborhood_constant; } + + inline float + getNeighborhoodConstant () { return neighborhood_constant_; } + + inline void + setInputNormals (const PointCloudNTConstPtr &normals) { normals_ = normals; } + + inline void + setInputScale (float input_scale) { input_scale_ = input_scale; } + + void + detectKeypoints (PointCloudT &output) override; + + protected: + bool + initCompute () override; + + private: + float neighborhood_constant_; + std::vector clouds_; + std::vector cloud_normals_; + std::vector cloud_trees_; + PointCloudNTConstPtr normals_; + std::vector > scales_; + float input_scale_; + std::size_t input_index_; + + static bool + compareScalesFunction (const std::pair &a, + const std::pair &b) { return a.first < b.first; } + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/susan.h b/deps_install/include/pcl-1.10/pcl/keypoints/susan.h new file mode 100755 index 0000000..9698f64 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/susan.h @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SUSANKeypoint implements a RGB-D extension of the SUSAN detector including normal + * directions variation in top of intensity variation. + * It is different from Harris in that it exploits normals directly so it is faster. + * Original paper "SUSAN — A New Approach to Low Level Image Processing", Smith, + * Stephen M. and Brady, J. Michael + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template > + class SUSANKeypoint : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using KdTree = typename Keypoint::KdTree; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::surface_; + using Keypoint::tree_; + using Keypoint::k_; + using Keypoint::search_radius_; + using Keypoint::search_parameter_; + using Keypoint::keypoints_indices_; + using Keypoint::initCompute; + + /** \brief Constructor + * \param[in] radius the radius for normal estimation as well as for non maxima suppression + * \param[in] distance_threshold to test if the nucleus is far enough from the centroid + * \param[in] angular_threshold to test if normals are parallel + * \param[in] intensity_threshold to test if points are of same color + */ + SUSANKeypoint (float radius = 0.01f, + float distance_threshold = 0.001f, + float angular_threshold = 0.0001f, + float intensity_threshold = 7.0f) + : distance_threshold_ (distance_threshold) + , angular_threshold_ (angular_threshold) + , intensity_threshold_ (intensity_threshold) + , normals_ (new pcl::PointCloud) + , threads_ (0) + , label_idx_ (-1) + { + name_ = "SUSANKeypoint"; + search_radius_ = radius; + geometric_validation_ = false; + tolerance_ = 2 * distance_threshold_; + } + + /** \brief Empty destructor */ + ~SUSANKeypoint () {} + + /** \brief set the radius for normal estimation and non maxima supression. + * \param[in] radius + */ + void + setRadius (float radius); + + void + setDistanceThreshold (float distance_threshold); + + /** \brief set the angular_threshold value for detecting corners. Normals are considered as + * parallel if 1 - angular_threshold <= (Ni.Nj) <= 1 + * \param[in] angular_threshold + */ + void + setAngularThreshold (float angular_threshold); + + /** \brief set the intensity_threshold value for detecting corners. + * \param[in] intensity_threshold + */ + void + setIntensityThreshold (float intensity_threshold); + + /** + * \brief set normals if precalculated normals are available. + * \param normals + */ + void + setNormals (const PointCloudNConstPtr &normals); + + void + setSearchSurface (const PointCloudInConstPtr &cloud) override; + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads); + + /** \brief Apply non maxima suppression to the responses to keep strongest corners. + * \note in SUSAN points with less response or stronger corners + */ + void + setNonMaxSupression (bool nonmax); + + /** \brief Filetr false positive using geometric criteria. + * The nucleus and the centroid should at least distance_threshold_ from each other AND all the + * points belonging to the USAN must be within the segment [nucleus centroid]. + * \param[in] validate + */ + void + setGeometricValidation (bool validate); + + protected: + bool + initCompute () override; + + void + detectKeypoints (PointCloudOut &output) override; + /** \brief return true if a point lies within the line between the nucleus and the centroid + * \param[in] nucleus coordinate of the nucleus + * \param[in] centroid of the SUSAN + * \param[in] nc to centroid vector (used to speed up since it is constant for a given + * neighborhood) + * \param[in] point the query point to test against + * \return true if the point lies within [nucleus centroid] + */ + bool + isWithinNucleusCentroid (const Eigen::Vector3f& nucleus, + const Eigen::Vector3f& centroid, + const Eigen::Vector3f& nc, + const PointInT& point) const; + private: + float distance_threshold_; + float angular_threshold_; + float intensity_threshold_; + float tolerance_; + PointCloudNConstPtr normals_; + unsigned int threads_; + bool geometric_validation_; + bool nonmax_; + /// intensity field accessor + IntensityT intensity_; + /** \brief Set to a value different than -1 if the output cloud has a "label" field and we have + * to save the keypoints indices. + */ + int label_idx_; + /** \brief The list of fields present in the output point cloud data. */ + std::vector out_fields_; + pcl::common::IntensityFieldAccessor intensity_out_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/trajkovic_2d.h b/deps_install/include/pcl-1.10/pcl/keypoints/trajkovic_2d.h new file mode 100755 index 0000000..355cdf5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/trajkovic_2d.h @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief TrajkovicKeypoint2D implements Trajkovic and Hedley corner detector on + * organized pooint cloud using intensity information. + * It uses first order statistics to find variation of intensities in horizontal + * or vertical directions. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template > + class TrajkovicKeypoint2D : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::keypoints_indices_; + + enum ComputationMethod { FOUR_CORNERS, EIGHT_CORNERS }; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param[in] window_size + * \param[in] first_threshold the threshold used in the simple cornerness test. + * \param[in] second_threshold the threshold used to reject weak corners. + */ + TrajkovicKeypoint2D (ComputationMethod method = FOUR_CORNERS, + int window_size = 3, + float first_threshold = 0.1, + float second_threshold = 100.0) + : method_ (method) + , window_size_ (window_size) + , first_threshold_ (first_threshold) + , second_threshold_ (second_threshold) + , threads_ (1) + { + name_ = "TrajkovicKeypoint2D"; + } + + /** \brief set the method of the response to be calculated. + * \param[in] method either 4 corners or 8 corners + */ + inline void + setMethod (ComputationMethod method) { method_ = method; } + + /// \brief \return the computation method + inline ComputationMethod + getMethod () const { return (method_); } + + /// \brief Set window size + inline void + setWindowSize (int window_size) { window_size_= window_size; } + + /// \brief \return window size i.e. window width or height + inline int + getWindowSize () const { return (window_size_); } + + /** \brief set the first_threshold to reject corners in the simple cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setFirstThreshold (float threshold) { first_threshold_= threshold; } + + /// \brief \return first threshold + inline float + getFirstThreshold () const { return (first_threshold_); } + + /** \brief set the second threshold to reject corners in the final cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setSecondThreshold (float threshold) { second_threshold_= threshold; } + + /// \brief \return second threshold + inline float + getSecondThreshold () const { return (second_threshold_); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use, 0 for automatic. + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /// \brief \return the number of threads + inline unsigned int + getNumberOfThreads () const { return (threads_); } + + protected: + bool + initCompute () override; + + void + detectKeypoints (PointCloudOut &output) override; + + private: + /// comparator for responses intensity + inline bool + greaterCornernessAtIndices (int a, int b) const + { + return (response_->points [a] > response_->points [b]); + } + + /// computation method + ComputationMethod method_; + /// Window size + int window_size_; + /// half window size + int half_window_size_; + /// intensity field accessor + IntensityT intensity_; + /// first threshold for quick rejection + float first_threshold_; + /// second threshold for corner evaluation + float second_threshold_; + /// number of threads to be used + unsigned int threads_; + /// point cloud response + pcl::PointCloud::Ptr response_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/trajkovic_3d.h b/deps_install/include/pcl-1.10/pcl/keypoints/trajkovic_3d.h new file mode 100755 index 0000000..fe9f3b2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/trajkovic_3d.h @@ -0,0 +1,215 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief TrajkovicKeypoint3D implements Trajkovic and Hedley corner detector on + * point cloud using geometric information. + * It uses first order statistics to find variation of normals. + * This work is part of Nizar Sallem PhD thesis. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template + class TrajkovicKeypoint3D : public Keypoint + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using PointCloudIn = typename Keypoint::PointCloudIn; + using PointCloudOut = typename Keypoint::PointCloudOut; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + using Normals = pcl::PointCloud; + using NormalsPtr = typename Normals::Ptr; + using NormalsConstPtr = typename Normals::ConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::keypoints_indices_; + using Keypoint::initCompute; + + enum ComputationMethod { FOUR_CORNERS, EIGHT_CORNERS }; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param[in] window_size + * \param[in] first_threshold the threshold used in the simple cornerness test. + * \param[in] second_threshold the threshold used to reject weak corners. + */ + TrajkovicKeypoint3D (ComputationMethod method = FOUR_CORNERS, + int window_size = 3, + float first_threshold = 0.00046, + float second_threshold = 0.03589) + : method_ (method) + , window_size_ (window_size) + , first_threshold_ (first_threshold) + , second_threshold_ (second_threshold) + , threads_ (1) + { + name_ = "TrajkovicKeypoint3D"; + } + + /** \brief set the method of the response to be calculated. + * \param[in] method either 4 corners or 8 corners + */ + inline void + setMethod (ComputationMethod method) { method_ = method; } + + /// \brief \return the computation method + inline ComputationMethod + getMethod () const { return (method_); } + + /// \brief Set window size + inline void + setWindowSize (int window_size) { window_size_= window_size; } + + /// \brief \return window size i.e. window width or height + inline int + getWindowSize () const { return (window_size_); } + + /** \brief set the first_threshold to reject corners in the simple cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setFirstThreshold (float threshold) { first_threshold_= threshold; } + + /// \brief \return first threshold + inline float + getFirstThreshold () const { return (first_threshold_); } + + /** \brief set the second threshold to reject corners in the final cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setSecondThreshold (float threshold) { second_threshold_= threshold; } + + /// \brief \return second threshold + inline float + getSecondThreshold () const { return (second_threshold_); } + + /** \brief Set normals if precalculated normals are available. + * \param normals + */ + inline void + setNormals (const NormalsConstPtr &normals) { normals_ = normals; } + + /// \brief \return points normals as calculated or given + inline void + getNormals () const { return (normals_); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use, 0 for automatic. + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /// \brief \return the number of threads + inline unsigned int + getNumberOfThreads () const { return (threads_); } + + protected: + bool + initCompute () override; + + void + detectKeypoints (PointCloudOut &output) override; + + private: + /** Return a const reference to the normal at (i,j) if it is finite else return + * a reference to a null normal. + * If the returned normal is valid \a counter is incremented. + */ + inline const NormalT& + getNormalOrNull (int i, int j, int& counter) const + { + static const NormalT null; + if (!isFinite ((*normals_) (i,j))) return (null); + ++counter; + return ((*normals_) (i,j)); + } + /// \return difference of two normals vectors + inline float + normalsDiff (const NormalT& a, const NormalT& b) const + { + double nx = a.normal_x; double ny = a.normal_y; double nz = a.normal_z; + double mx = b.normal_x; double my = b.normal_y; double mz = b.normal_z; + return (static_cast (1.0 - (nx*mx + ny*my + nz*mz))); + } + /// \return squared difference of two normals vectors + inline float + squaredNormalsDiff (const NormalT& a, const NormalT& b) const + { + float diff = normalsDiff (a,b); + return (diff * diff); + } + /** Comparator for responses intensity + * \return true if \a response_ at index \aa is greater than response at index \ab + */ + inline bool + greaterCornernessAtIndices (int a, int b) const + { + return (response_->points [a] > response_->points [b]); + } + /// computation method + ComputationMethod method_; + /// window size + int window_size_; + /// half window size + int half_window_size_; + /// first threshold for quick rejection + float first_threshold_; + /// second threshold for corner evaluation + float second_threshold_; + /// number of threads to be used + unsigned int threads_; + /// point cloud normals + NormalsConstPtr normals_; + /// point cloud response + pcl::PointCloud::Ptr response_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/keypoints/uniform_sampling.h b/deps_install/include/pcl-1.10/pcl/keypoints/uniform_sampling.h new file mode 100755 index 0000000..7266dcf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/keypoints/uniform_sampling.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifdef __DEPRECATED +#warning UniformSampling is not a Keypoint anymore, use instead. +#endif + +#include diff --git a/deps_install/include/pcl-1.10/pcl/make_shared.h b/deps_install/include/pcl-1.10/pcl/make_shared.h new file mode 100755 index 0000000..30f6451 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/make_shared.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2019-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + + +#include +#include + +#include +#include + +#include + + +namespace pcl +{ + +#ifdef DOXYGEN_ONLY + +/** + * \brief Returns a pcl::shared_ptr compliant with type T's allocation policy. + * + * boost::allocate_shared or boost::make_shared will be invoked in case T has or + * doesn't have a custom allocator, respectively. + * + * \see pcl::has_custom_allocator, PCL_MAKE_ALIGNED_OPERATOR_NEW + * \tparam T Type of the object to create a pcl::shared_ptr of + * \tparam Args Types for the arguments to pcl::make_shared + * \param args List of arguments with which an instance of T will be constructed + * \return pcl::shared_ptr of an instance of type T + */ +template +shared_ptr make_shared(Args&&... args); + +#else + +template +std::enable_if_t::value, shared_ptr> make_shared(Args&&... args) +{ + return boost::allocate_shared(Eigen::aligned_allocator(), std::forward (args)...); +} + +template +std::enable_if_t::value, shared_ptr> make_shared(Args&&... args) +{ + return boost::make_shared(std::forward (args)...); +} + +#endif + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/branch_estimator.h b/deps_install/include/pcl-1.10/pcl/ml/branch_estimator.h new file mode 100755 index 0000000..ba9d43f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/branch_estimator.h @@ -0,0 +1,142 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#include +#include + +namespace pcl { + +/** Interface for branch estimators. */ +class PCL_EXPORTS BranchEstimator { +public: + /** Destructor. */ + virtual ~BranchEstimator() {} + + /** Returns the number of branches the corresponding tree has. */ + virtual std::size_t + getNumOfBranches() const = 0; + + /** Computes the branch index for the specified result. + * + * \param[in] result the result the branch index will be computed for + * \param[in] flag the flag corresponding to the specified result + * \param[in] threshold the threshold used to compute the branch index + * \param[out] branch_index the destination for the computed branch index + */ + virtual void + computeBranchIndex(const float result, + const unsigned char flag, + const float threshold, + unsigned char& branch_index) const = 0; +}; + +/** Branch estimator for binary trees where the branch is computed only from the + * threshold. */ +class PCL_EXPORTS BinaryTreeThresholdBasedBranchEstimator : public BranchEstimator { +public: + /** Constructor. */ + inline BinaryTreeThresholdBasedBranchEstimator() {} + /** Destructor. */ + inline ~BinaryTreeThresholdBasedBranchEstimator() {} + + /** Returns the number of branches the corresponding tree has. */ + inline std::size_t + getNumOfBranches() const override + { + return 2; + } + + /** Computes the branch index for the specified result. + * + * \param[in] result the result the branch index will be computed for + * \param[in] flag the flag corresponding to the specified result + * \param[in] threshold the threshold used to compute the branch index + * \param[out] branch_index the destination for the computed branch index + */ + inline void + computeBranchIndex(const float result, + const unsigned char flag, + const float threshold, + unsigned char& branch_index) const override + { + (void)flag; + branch_index = (result > threshold) ? 1 : 0; + } +}; + +/** Branch estimator for ternary trees where one branch is used for missing data + * (indicated by flag != 0). */ +class PCL_EXPORTS TernaryTreeMissingDataBranchEstimator : public BranchEstimator { +public: + /** Constructor. */ + inline TernaryTreeMissingDataBranchEstimator() {} + /** Destructor. */ + inline ~TernaryTreeMissingDataBranchEstimator() {} + + /** \brief Returns the number of branches the corresponding tree has. */ + inline std::size_t + getNumOfBranches() const override + { + return 3; + } + + /** Computes the branch index for the specified result. + * + * \param[in] result the result the branch index will be computed for + * \param[in] flag the flag corresponding to the specified result + * \param[in] threshold the threshold used to compute the branch index + * \param[out] branch_index the destination for the computed branch index + */ + inline void + computeBranchIndex(const float result, + const unsigned char flag, + const float threshold, + unsigned char& branch_index) const override + { + if (flag == 0) + branch_index = (result > threshold) ? 1 : 0; + else + branch_index = 2; + } +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/densecrf.h b/deps_install/include/pcl-1.10/pcl/ml/densecrf.h new file mode 100755 index 0000000..5f16522 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/densecrf.h @@ -0,0 +1,166 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#pragma once + +#include +#include +#include + +#include + +namespace pcl { + +class PCL_EXPORTS DenseCrf { +public: + /** Constructor for DenseCrf class. */ + DenseCrf(int N, int m); + + /** Deconstructor for DenseCrf class. */ + ~DenseCrf(); + + /** Set the input data vector. + * + * The input data vector holds the measurements coordinates as ijk of the voxel grid. + */ + void + setDataVector(const std::vector> data); + + /** The associated color of the data. */ + void + setColorVector(const std::vector> color); + + void + setUnaryEnergy(const std::vector unary); + + void + addPairwiseEnergy(const std::vector& feature, + const int feature_dimension, + const float w); + + /** Add a pairwise gaussian kernel. */ + void + addPairwiseGaussian(float sx, float sy, float sz, float w); + + /** Add a bilateral gaussian kernel. */ + void + addPairwiseBilateral( + float sx, float sy, float sz, float sr, float sg, float sb, float w); + + void + addPairwiseNormals( + std::vector>& coord, + std::vector>& normals, + float sx, + float sy, + float sz, + float snx, + float sny, + float snz, + float w); + + void + inference(int n_iterations, std::vector& result, float relax = 1.0f); + + void + mapInference(int n_iterations, std::vector& result, float relax = 1.0f); + + void + expAndNormalize(std::vector& out, + const std::vector& in, + float scale, + float relax = 1.0f); + + void + expAndNormalizeORI(float* out, + const float* in, + float scale = 1.0f, + float relax = 1.0f); + void + map(int n_iterations, std::vector result, float relax = 1.0f); + + std::vector + runInference(int n_iterations, float relax); + + void + startInference(); + + void + stepInference(float relax); + + void + runInference(float relax); + + void + getBarycentric(int idx, std::vector& bary); + + void + getFeatures(int idx, std::vector& features); + +protected: + /** Number of variables and labels */ + int N_, M_; + + /** Data vector */ + std::vector> data_; + + /** Color vector */ + std::vector> color_; + + /** CRF unary potentials */ + /** TODO: double might use to much memory */ + std::vector unary_; + + std::vector current_; + std::vector next_; + std::vector tmp_; + + /** Pairwise potentials */ + std::vector pairwise_potential_; + + /** Input types */ + bool xyz_, rgb_, normal_; + +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest.h b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest.h new file mode 100755 index 0000000..83d8e79 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest.h @@ -0,0 +1,114 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include + +#include +#include + +namespace pcl { + +/** Class representing a decision forest. */ +template +class PCL_EXPORTS DecisionForest : public std::vector> { + +public: + /** Constructor. */ + DecisionForest() {} + + /** Destructor. */ + virtual ~DecisionForest() {} + + /** Serializes the decision tree. + * + * \param[out] stream The destination for the serialization + */ + void + serialize(::std::ostream& stream) const + { + const int num_of_trees = static_cast(this->size()); + stream.write(reinterpret_cast(&num_of_trees), sizeof(num_of_trees)); + + for (std::size_t tree_index = 0; tree_index < this->size(); ++tree_index) { + (*this)[tree_index].serialize(stream); + } + + // const int num_of_trees = static_cast (trees_.size ()); + // stream.write (reinterpret_cast (&num_of_trees), sizeof + // (num_of_trees)); + + // for (std::size_t tree_index = 0; tree_index < trees_.size (); ++tree_index) + //{ + // tree_[tree_index].serialize (stream); + //} + } + + /** Deserializes the decision tree. + * + * \param[in] stream The source for the deserialization + */ + void + deserialize(::std::istream& stream) + { + int num_of_trees; + stream.read(reinterpret_cast(&num_of_trees), sizeof(num_of_trees)); + this->resize(num_of_trees); + + for (std::size_t tree_index = 0; tree_index < this->size(); ++tree_index) { + (*this)[tree_index].deserialize(stream); + } + + // int num_of_trees; + // stream.read (reinterpret_cast (&num_of_trees), sizeof (num_of_trees)); + // trees_.resize (num_of_trees); + + // for (std::size_t tree_index = 0; tree_index < trees_.size (); ++tree_index) + //{ + // tree_[tree_index].deserialize (stream); + //} + } + +private: + /** The decision trees contained in the forest. */ + // std::vector > trees_; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest_evaluator.h b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest_evaluator.h new file mode 100755 index 0000000..e4595ad --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest_evaluator.h @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include + +#include + +namespace pcl { + +/** Utility class for evaluating a decision forests. */ +template +class DecisionForestEvaluator { +public: + /** Constructor. */ + DecisionForestEvaluator(); + + /** Destructor. */ + virtual ~DecisionForestEvaluator(); + + /** Evaluates the specified examples using the supplied forest. + * + * \param[in] DecisionForestEvaluator the decision forest + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] examples the examples that have to be evaluated + * \param[out] label_data the destination for the resulting label data + */ + void + evaluate( + pcl::DecisionForest& DecisionForestEvaluator, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data); + + /** Evaluates a specific patch using the supplied forest. + * + * \param[in] DecisionForestEvaluator the decision forest + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] example the examples that have to be evaluated + * \param[out] leaves the leaves where the patch arrives + */ + void + evaluate( + pcl::DecisionForest& DecisionForestEvaluator, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + ExampleIndex example, + std::vector& leaves); + +private: + /** Evaluator for decision trees. */ + DecisionTreeEvaluator + tree_evaluator_; +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest_trainer.h b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest_trainer.h new file mode 100755 index 0000000..499a256 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_forest_trainer.h @@ -0,0 +1,224 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include + +namespace pcl { + +/** Trainer for decision trees. */ +template +class PCL_EXPORTS DecisionForestTrainer { + +public: + /** Constructor. */ + DecisionForestTrainer(); + + /** Destructor. */ + virtual ~DecisionForestTrainer(); + + /** Sets the number of trees to train. + * + * \param[in] num_of_trees the number of trees + */ + inline void + setNumberOfTreesToTrain(const std::size_t num_of_trees) + { + num_of_trees_to_train_ = num_of_trees; + } + + /** Sets the feature handler used to create and evaluate features. + * + * \param[in] feature_handler the feature handler + */ + inline void + setFeatureHandler( + pcl::FeatureHandler& feature_handler) + { + decision_tree_trainer_.setFeatureHandler(feature_handler); + } + + /** Sets the object for estimating the statistics for tree nodes. + * + * \param[in] stats_estimator the statistics estimator + */ + inline void + setStatsEstimator( + pcl::StatsEstimator& stats_estimator) + { + decision_tree_trainer_.setStatsEstimator(stats_estimator); + } + + /** Sets the maximum depth of the learned tree. + * + * \param[in] max_tree_depth maximum depth of the learned tree + */ + inline void + setMaxTreeDepth(const std::size_t max_tree_depth) + { + decision_tree_trainer_.setMaxTreeDepth(max_tree_depth); + } + + /** Sets the number of features used to find optimal decision features. + * + * \param[in] num_of_features the number of features + */ + inline void + setNumOfFeatures(const std::size_t num_of_features) + { + decision_tree_trainer_.setNumOfFeatures(num_of_features); + } + + /** Sets the number of thresholds tested for finding the optimal decision threshold on + * the feature responses. + * + * \param[in] num_of_threshold the number of thresholds + */ + inline void + setNumOfThresholds(const std::size_t num_of_threshold) + { + decision_tree_trainer_.setNumOfThresholds(num_of_threshold); + } + + /** Sets the input data set used for training. + * + * \param[in] data_set the data set used for training + */ + inline void + setTrainingDataSet(DataSet& data_set) + { + decision_tree_trainer_.setTrainingDataSet(data_set); + } + + /** Example indices that specify the data used for training. + * + * \param[in] examples the examples + */ + inline void + setExamples(std::vector& examples) + { + decision_tree_trainer_.setExamples(examples); + } + + /** Sets the label data corresponding to the example data. + * + * \param[in] label_data the label data + */ + inline void + setLabelData(std::vector& label_data) + { + decision_tree_trainer_.setLabelData(label_data); + } + + /** Sets the minimum number of examples to continue growing a tree. + * + * \param[in] n number of examples + */ + inline void + setMinExamplesForSplit(std::size_t n) + { + decision_tree_trainer_.setMinExamplesForSplit(n); + } + + /** Specify the thresholds to be used when evaluating features. + * + * \param[in] thres the threshold values + */ + void + setThresholds(std::vector& thres) + { + decision_tree_trainer_.setThresholds(thres); + } + + /** Specify the data provider. + * + * \param[in] dtdp the data provider that should implement getDatasetAndLabels() + * function + */ + void + setDecisionTreeDataProvider( + typename pcl::DecisionTreeTrainerDataProvider::Ptr& dtdp) + { + decision_tree_trainer_.setDecisionTreeDataProvider(dtdp); + } + + /** Specify if the features are randomly generated at each split node. + * + * \param[in] b do it or not + */ + void + setRandomFeaturesAtSplitNode(bool b) + { + decision_tree_trainer_.setRandomFeaturesAtSplitNode(b); + } + + /** Trains a decision forest using the set training data and settings. + * + * \param[out] forest destination for the trained forest + */ + void + train(DecisionForest& forest); + +private: + /** The number of trees to train. */ + std::size_t num_of_trees_to_train_; + + /** The trainer for the decision trees of the forest. */ + pcl::DecisionTreeTrainer + decision_tree_trainer_; +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree.h b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree.h new file mode 100755 index 0000000..e5915f0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree.h @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** Class representing a decision tree. */ +template +class PCL_EXPORTS DecisionTree { +public: + /** Constructor. */ + DecisionTree() : root_() {} + + /** Destructor. */ + virtual ~DecisionTree() {} + + /** Sets the root node of the tree. + * + * \param[in] root the root node + */ + void + setRoot(const NodeType& root) + { + root_ = root; + } + + /** Returns the root node of the tree. */ + NodeType& + getRoot() + { + return root_; + } + + /** Serializes the decision tree. + * + * \param[out] stream the destination for the serialization + */ + void + serialize(::std::ostream& stream) const + { + root_.serialize(stream); + } + + /** Deserializes the decision tree. + * + * \param[in] stream the source for the deserialization + */ + void + deserialize(::std::istream& stream) + { + root_.deserialize(stream); + } + +private: + /** The root node of the decision tree. */ + NodeType root_; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_data_provider.h b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_data_provider.h new file mode 100755 index 0000000..fc13b28 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_data_provider.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include + +namespace pcl { +template +class PCL_EXPORTS DecisionTreeTrainerDataProvider { + + /** The training data set. */ + DataSet data_set_; + /** The label data. */ + std::vector label_data_; + +public: + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + /** Constructor. */ + DecisionTreeTrainerDataProvider() {} + + /** Destructor. */ + ~DecisionTreeTrainerDataProvider() {} + + /** Virtual function called to obtain training examples and labels before + * training a specific tree */ + virtual void + getDatasetAndLabels(DataSet& data_set, + std::vector& label_data, + std::vector& examples) = 0; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_evaluator.h b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_evaluator.h new file mode 100755 index 0000000..0729a76 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_evaluator.h @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace pcl { + +/** Utility class for evaluating a decision tree. */ +template +class DecisionTreeEvaluator { + +public: + /** Constructor. */ + DecisionTreeEvaluator(); + + /** Destructor. */ + virtual ~DecisionTreeEvaluator(); + + /** Evaluates the specified examples using the supplied tree. + * + * \param[in] tree the decision tree + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] examples the examples that have to be evaluated + * \param[out] label_data the destination for the resulting label data + */ + void + evaluate( + pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data); + + /** Evaluates the specified examples using the supplied tree and adds the + * results to the supplied results array. + * + * \param[in] tree the decision tree + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] examples the examples that have to be evaluated + * \param[out] label_data the destination where the resulting label data is added to + */ + void + evaluateAndAdd( + pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data); + + /** Evaluates the specified examples using the supplied tree. + * + * \param[in] tree the decision tree + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] example the example that has to be evaluated + * \param[out] leave The leave reached by the examples. + */ + void + evaluate( + pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + ExampleIndex example, + NodeType& leave); + + /** Evaluates the specified examples using the supplied tree. + * + * \param[in] tree the decision tree + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] examples the examples that have to be evaluated + * \param[out] nodes the leaf-nodes reached while evaluation + */ + void + getNodes( + pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& nodes); +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_trainer.h b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_trainer.h new file mode 100755 index 0000000..7513b66 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/dt/decision_tree_trainer.h @@ -0,0 +1,270 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include + +#include + +namespace pcl { + +/** Trainer for decision trees. */ +template +class PCL_EXPORTS DecisionTreeTrainer { + +public: + /** Constructor. */ + DecisionTreeTrainer(); + + /** Destructor. */ + virtual ~DecisionTreeTrainer(); + + /** Sets the feature handler used to create and evaluate features. + * + * \param[in] feature_handler the feature handler + */ + inline void + setFeatureHandler( + pcl::FeatureHandler& feature_handler) + { + feature_handler_ = &feature_handler; + } + + /** Sets the object for estimating the statistics for tree nodes. + * + * \param[in] stats_estimator the statistics estimator + */ + inline void + setStatsEstimator( + pcl::StatsEstimator& stats_estimator) + { + stats_estimator_ = &stats_estimator; + } + + /** Sets the maximum depth of the learned tree. + * + * \param[in] max_tree_depth maximum depth of the learned tree + */ + inline void + setMaxTreeDepth(const std::size_t max_tree_depth) + { + max_tree_depth_ = max_tree_depth; + } + + /** Sets the number of features used to find optimal decision features. + * + * \param[in] num_of_features the number of features + */ + inline void + setNumOfFeatures(const std::size_t num_of_features) + { + num_of_features_ = num_of_features; + } + + /** Sets the number of thresholds tested for finding the optimal decision + * threshold on the feature responses. + * + * \param[in] num_of_threshold the number of thresholds + */ + inline void + setNumOfThresholds(const std::size_t num_of_threshold) + { + num_of_thresholds_ = num_of_threshold; + } + + /** Sets the input data set used for training. + * + * \param[in] data_set the data set used for training + */ + inline void + setTrainingDataSet(DataSet& data_set) + { + data_set_ = data_set; + } + + /** Example indices that specify the data used for training. + * + * \param[in] examples the examples + */ + inline void + setExamples(std::vector& examples) + { + examples_ = examples; + } + + /** Sets the label data corresponding to the example data. + * + * \param[in] label_data the label data + */ + inline void + setLabelData(std::vector& label_data) + { + label_data_ = label_data; + } + + /** Sets the minimum number of examples to continue growing a tree. + * + * \param[in] n number of examples + */ + inline void + setMinExamplesForSplit(std::size_t n) + { + min_examples_for_split_ = n; + } + + /** Specify the thresholds to be used when evaluating features. + * + * \param[in] thres the threshold values + */ + void + setThresholds(std::vector& thres) + { + thresholds_ = thres; + } + + /** Specify the data provider. + * + * \param[in] dtdp the data provider that should implement getDatasetAndLabels() + * function + */ + void + setDecisionTreeDataProvider( + typename pcl::DecisionTreeTrainerDataProvider::Ptr& dtdp) + { + decision_tree_trainer_data_provider_ = dtdp; + } + + /** Specify if the features are randomly generated at each split node. + * + * \param[in] b do it or not + */ + void + setRandomFeaturesAtSplitNode(bool b) + { + random_features_at_split_node_ = b; + } + + /** Trains a decision tree using the set training data and settings. + * + * \param[out] tree destination for the trained tree + */ + void + train(DecisionTree& tree); + +protected: + /** Trains a decision tree node from the specified features, label data, and + * examples. + * + * \param[in] features the feature pool used for training + * \param[in] examples the examples used for training + * \param[in] label_data the label data corresponding to the examples + * \param[in] max_depth the maximum depth of the remaining tree + * \param[out] node the resulting node + */ + void + trainDecisionTreeNode(std::vector& features, + std::vector& examples, + std::vector& label_data, + std::size_t max_depth, + NodeType& node); + + /** Creates uniformely distrebuted thresholds over the range of the supplied + * values. + * + * \param[in] num_of_thresholds the number of thresholds to create + * \param[in] values the values for estimating the expected value range + * \param[out] thresholds the resulting thresholds + */ + static void + createThresholdsUniform(const std::size_t num_of_thresholds, + std::vector& values, + std::vector& thresholds); + +private: + /** Maximum depth of the learned tree. */ + std::size_t max_tree_depth_; + /** Number of features used to find optimal decision features. */ + std::size_t num_of_features_; + /** Number of thresholds. */ + std::size_t num_of_thresholds_; + + /** FeatureHandler instance, responsible for creating and evaluating features. */ + pcl::FeatureHandler* feature_handler_; + /** StatsEstimator instance, responsible for gathering stats about a node. */ + pcl::StatsEstimator* stats_estimator_; + + /** The training data set. */ + DataSet data_set_; + /** The label data. */ + std::vector label_data_; + /** The example data. */ + std::vector examples_; + + /** Minimum number of examples to split a node. */ + std::size_t min_examples_for_split_; + /** Thresholds to be used instead of generating uniform distributed thresholds. */ + std::vector thresholds_; + /** The data provider which is called before training a specific tree, if pointer is + * NULL, then data_set_ is used. */ + typename pcl::DecisionTreeTrainerDataProvider::Ptr + decision_tree_trainer_data_provider_; + /** If true, random features are generated at each node, otherwise, at start of + * training the tree */ + bool random_features_at_split_node_; +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ml/feature_handler.h b/deps_install/include/pcl-1.10/pcl/ml/feature_handler.h new file mode 100755 index 0000000..86be9fe --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/feature_handler.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** Utility class interface which is used for creating and evaluating features. */ +template +class PCL_EXPORTS FeatureHandler { +public: + /** Destructor. */ + virtual ~FeatureHandler(){}; + + /** Creates random features. + * + * \param[in] num_of_features the number of random features to create + * \param[out] features the destination for the created features + */ + virtual void + createRandomFeatures(const std::size_t num_of_features, + std::vector& features) = 0; + + /** Evaluates a feature on the specified data. + * + * \param[in] feature the features to evaluate + * \param[in] data_set the data set on which the feature is evaluated + * \param[in] examples the examples which specify on which parts of the data set the + * feature is evaluated + * \param[out] results the destination for the results of the feature evaluation + * \param[out] flags flags that are supplied together with the + * results + */ + virtual void + evaluateFeature(const FeatureType& feature, + DataSet& data_set, + std::vector& examples, + std::vector& results, + std::vector& flags) const = 0; + + /** Evaluates a feature on the specified data. + * + * \param[in] feature the features to evaluate + * \param[in] data_set the data set on which the feature is evaluated + * \param[in] example the examples which specify on which parts of the data set the + * feature is evaluated + * \param[out] result the destination for the results of the feature evaluation + * \param[out] flag flags that are supplied together with the results + */ + virtual void + evaluateFeature(const FeatureType& feature, + DataSet& data_set, + const ExampleIndex& example, + float& result, + unsigned char& flag) const = 0; + + /** Generates evaluation code for the specified feature and writes it to the specified + * stream + * + * \param[in] feature the feature for which code is generated + * \param[out] stream the destination for the code + */ + virtual void + generateCodeForEvaluation(const FeatureType& feature, + ::std::ostream& stream) const = 0; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/ferns/fern.h b/deps_install/include/pcl-1.10/pcl/ml/ferns/fern.h new file mode 100755 index 0000000..6477dde --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/ferns/fern.h @@ -0,0 +1,210 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** Class representing a Fern. */ +template +class PCL_EXPORTS Fern { +public: + /** Constructor. */ + Fern() : num_of_decisions_(0), features_(0), thresholds_(0), nodes_(1) {} + + /** Destructor. */ + virtual ~Fern() {} + + /** Initializes the fern. + * + * \param num_of_decisions the number of decisions taken to access the nodes + */ + void + initialize(const std::size_t num_of_decisions) + { + num_of_decisions_ = num_of_decisions; + features_.resize(num_of_decisions_); + thresholds_.resize(num_of_decisions_, std::numeric_limits::quiet_NaN()); + nodes_.resize(0x1 << num_of_decisions_); + } + + /** Returns the number of nodes the Fern has. */ + inline std::size_t + getNumOfNodes() + { + return 0x1U << num_of_decisions_; + } + + /** Returns the number of features the Fern has. */ + inline std::size_t + getNumOfFeatures() + { + return num_of_decisions_; + } + + /** Serializes the fern. + * + * \param[out] stream the destination for the serialization + */ + void + serialize(::std::ostream& stream) const + { + // const int tmp_value = static_cast (num_of_decisions_); + // stream.write (reinterpret_cast (&tmp_value), sizeof (tmp_value)); + stream.write(reinterpret_cast(&num_of_decisions_), + sizeof(num_of_decisions_)); + + for (std::size_t feature_index = 0; feature_index < features_.size(); + ++feature_index) { + features_[feature_index].serialize(stream); + } + + for (std::size_t threshold_index = 0; threshold_index < thresholds_.size(); + ++threshold_index) { + stream.write(reinterpret_cast(&(thresholds_[threshold_index])), + sizeof(thresholds_[threshold_index])); + } + + for (std::size_t node_index = 0; node_index < nodes_.size(); ++node_index) { + nodes_[node_index].serialize(stream); + } + } + + /** Deserializes the fern. + * + * \param[in] stream the source for the deserialization + */ + void + deserialize(::std::istream& stream) + { + stream.read(reinterpret_cast(&num_of_decisions_), sizeof(num_of_decisions_)); + + features_.resize(num_of_decisions_); + thresholds_.resize(num_of_decisions_); + nodes_.resize(0x1 << num_of_decisions_); + + for (std::size_t feature_index = 0; feature_index < features_.size(); + ++feature_index) { + features_[feature_index].deserialize(stream); + } + + for (std::size_t threshold_index = 0; threshold_index < thresholds_.size(); + ++threshold_index) { + stream.read(reinterpret_cast(&(thresholds_[threshold_index])), + sizeof(thresholds_[threshold_index])); + } + + for (std::size_t node_index = 0; node_index < nodes_.size(); ++node_index) { + nodes_[node_index].deserialize(stream); + } + } + + /** Access operator for nodes. + * + * \param node_index the index of the node to access + */ + inline NodeType& operator[](const std::size_t node_index) + { + return nodes_[node_index]; + } + + /** Access operator for nodes. + * + * \param node_index the index of the node to access + */ + inline const NodeType& operator[](const std::size_t node_index) const + { + return nodes_[node_index]; + } + + /** Access operator for features. + * + * \param feature_index the index of the feature to access + */ + inline FeatureType& + accessFeature(const std::size_t feature_index) + { + return features_[feature_index]; + } + + /** Access operator for features. + * + * \param feature_index the index of the feature to access + */ + inline const FeatureType& + accessFeature(const std::size_t feature_index) const + { + return features_[feature_index]; + } + + /** Access operator for thresholds. + * + * \param threshold_index the index of the threshold to access + */ + inline float& + accessThreshold(const std::size_t threshold_index) + { + return thresholds_[threshold_index]; + } + + /** Access operator for thresholds. + * + * \param threshold_index the index of the threshold to access + */ + inline const float& + accessThreshold(const std::size_t threshold_index) const + { + return thresholds_[threshold_index]; + } + +private: + /** The number of decisions. */ + std::size_t num_of_decisions_; + /** The list of Features used to make the decisions. */ + std::vector features_; + /** The list of thresholds used to make the decisions. */ + std::vector thresholds_; + /** The list of Nodes accessed by the Fern. */ + std::vector nodes_; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/ferns/fern_evaluator.h b/deps_install/include/pcl-1.10/pcl/ml/ferns/fern_evaluator.h new file mode 100755 index 0000000..f869b44 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/ferns/fern_evaluator.h @@ -0,0 +1,126 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace pcl { + +/** Utility class for evaluating a fern. */ +template +class FernEvaluator { + +public: + /** Constructor. */ + FernEvaluator(); + + /** Destructor. */ + virtual ~FernEvaluator(); + + /** Evaluates the specified examples using the supplied tree. + * + * \param[in] fern the decision tree + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] examples the examples that have to be evaluated + * \param[out] label_data the destination for the resulting label data + */ + void + evaluate( + pcl::Fern& fern, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data); + + /** Evaluates the specified examples using the supplied tree and adds the + * results to the supplied results array. + * + * \param[in] fern the decision tree + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] examples the examples that have to be evaluated + * \param[out] label_data the destination where the resulting label data is added to + */ + void + evaluateAndAdd( + pcl::Fern& fern, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data); + + /** Evaluates the specified examples using the supplied tree. + * + * \param[in] fern the decision tree + * \param[in] feature_handler the feature handler used to train the tree + * \param[in] stats_estimator the statistics estimation instance used while training + * the tree + * \param[in] data_set the data set used for evaluation + * \param[in] examples the examples that have to be evaluated + * \param[out] nodes the leaf-nodes reached while evaluation + */ + void + getNodes( + pcl::Fern& fern, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& nodes); +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ml/ferns/fern_trainer.h b/deps_install/include/pcl-1.10/pcl/ml/ferns/fern_trainer.h new file mode 100755 index 0000000..f07f60e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/ferns/fern_trainer.h @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace pcl { + +/** Trainer for a Fern. */ +template +class PCL_EXPORTS FernTrainer { + +public: + /** Constructor. */ + FernTrainer(); + + /** Destructor. */ + virtual ~FernTrainer(); + + /** Sets the feature handler used to create and evaluate features. + * + * \param[in] feature_handler the feature handler + */ + inline void + setFeatureHandler( + pcl::FeatureHandler& feature_handler) + { + feature_handler_ = &feature_handler; + } + + /** Sets the object for estimating the statistics for tree nodes. + * + * \param[in] stats_estimator the statistics estimator + */ + inline void + setStatsEstimator( + pcl::StatsEstimator& stats_estimator) + { + stats_estimator_ = &stats_estimator; + } + + /** Sets the maximum depth of the learned tree. + * + * \param[in] fern_depth maximum depth of the learned tree + */ + inline void + setFernDepth(const std::size_t fern_depth) + { + fern_depth_ = fern_depth; + } + + /** Sets the number of features used to find optimal decision features. + * + * \param[in] num_of_features the number of features + */ + inline void + setNumOfFeatures(const std::size_t num_of_features) + { + num_of_features_ = num_of_features; + } + + /** Sets the number of thresholds tested for finding the optimal decision + * threshold on the feature responses. + * + * \param[in] num_of_threshold the number of thresholds + */ + inline void + setNumOfThresholds(const std::size_t num_of_threshold) + { + num_of_thresholds_ = num_of_threshold; + } + + /** Sets the input data set used for training. + * + * \param[in] data_set the data set used for training + */ + inline void + setTrainingDataSet(DataSet& data_set) + { + data_set_ = data_set; + } + + /** Example indices that specify the data used for training. + * + * \param[in] examples the examples + */ + inline void + setExamples(std::vector& examples) + { + examples_ = examples; + } + + /** Sets the label data corresponding to the example data. + * + * \param[in] label_data the label data + */ + inline void + setLabelData(std::vector& label_data) + { + label_data_ = label_data; + } + + /** Trains a decision tree using the set training data and settings. + * + * \param[out] fern destination for the trained tree + */ + void + train(Fern& fern); + +protected: + /** Creates uniformely distrebuted thresholds over the range of the supplied + * values. + * + * \param[in] num_of_thresholds the number of thresholds to create + * \param[in] values the values for estimating the expected value range + * \param[out] thresholds the resulting thresholds + */ + static void + createThresholdsUniform(const std::size_t num_of_thresholds, + std::vector& values, + std::vector& thresholds); + +private: + /** Desired depth of the learned fern. */ + std::size_t fern_depth_; + /** Number of features used to find optimal decision features. */ + std::size_t num_of_features_; + /** Number of thresholds. */ + std::size_t num_of_thresholds_; + + /** FeatureHandler instance, responsible for creating and evaluating features. */ + pcl::FeatureHandler* feature_handler_; + /** StatsEstimator instance, responsible for gathering stats about a node. */ + pcl::StatsEstimator* stats_estimator_; + + /** The training data set. */ + DataSet data_set_; + /** The label data. */ + std::vector label_data_; + /** The example data. */ + std::vector examples_; +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_forest_evaluator.hpp b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_forest_evaluator.hpp new file mode 100755 index 0000000..b9cfee0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_forest_evaluator.hpp @@ -0,0 +1,127 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include + +#include + +template +pcl::DecisionForestEvaluator:: + DecisionForestEvaluator() +: tree_evaluator_() +{} + +template +pcl::DecisionForestEvaluator:: + ~DecisionForestEvaluator() +{} + +template +void +pcl::DecisionForestEvaluator:: + evaluate(pcl::DecisionForest& forest, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& + stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data) +{ + const std::size_t num_of_examples = examples.size(); + label_data.resize(num_of_examples, 0); + + for (std::size_t forest_index = 0; forest_index < forest.size(); ++forest_index) { + tree_evaluator_.evaluateAndAdd(forest[forest_index], + feature_handler, + stats_estimator, + data_set, + examples, + label_data); + } + + const float inv_num_of_trees = 1.0f / static_cast(forest.size()); + for (std::size_t label_index = 0; label_index < label_data.size(); ++label_index) { + label_data[label_index] *= inv_num_of_trees; + } +} + +template +void +pcl::DecisionForestEvaluator:: + evaluate(pcl::DecisionForest& forest, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& + stats_estimator, + DataSet& data_set, + ExampleIndex example, + std::vector& leaves) +{ + leaves.resize(forest.size()); + for (std::size_t forest_index = 0; forest_index < forest.size(); ++forest_index) { + NodeType leave; + tree_evaluator_.evaluate(forest[forest_index], + feature_handler, + stats_estimator, + data_set, + example, + leave); + leaves[forest_index] = leave; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_forest_trainer.hpp b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_forest_trainer.hpp new file mode 100755 index 0000000..019454e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_forest_trainer.hpp @@ -0,0 +1,74 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +template +pcl::DecisionForestTrainer:: + DecisionForestTrainer() +: num_of_trees_to_train_(1), decision_tree_trainer_() +{} + +template +pcl::DecisionForestTrainer:: + ~DecisionForestTrainer() +{} + +template +void +pcl::DecisionForestTrainer:: + train(pcl::DecisionForest& forest) +{ + for (std::size_t tree_index = 0; tree_index < num_of_trees_to_train_; ++tree_index) { + pcl::DecisionTree tree; + decision_tree_trainer_.train(tree); + + forest.push_back(tree); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_tree_evaluator.hpp b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_tree_evaluator.hpp new file mode 100755 index 0000000..3d55a85 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_tree_evaluator.hpp @@ -0,0 +1,208 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +template +pcl::DecisionTreeEvaluator:: + DecisionTreeEvaluator() +{} + +template +pcl::DecisionTreeEvaluator:: + ~DecisionTreeEvaluator() +{} + +template +void +pcl::DecisionTreeEvaluator:: + evaluate(pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& + stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data) +{ + const std::size_t num_of_examples = examples.size(); + label_data.resize(num_of_examples); + for (int example_index = 0; example_index < num_of_examples; ++example_index) { + NodeType* node = &(tree.getRoot()); + + while (node->sub_nodes.size() != 0) { + float feature_result = 0.0f; + unsigned char flag = 0; + unsigned char branch_index = 0; + + feature_handler.evaluateFeature( + node->feature, data_set, examples[example_index], feature_result, flag); + stats_estimator.computeBranchIndex( + feature_result, flag, node->threshold, branch_index); + + node = &(node->sub_nodes[branch_index]); + } + + label_data[example_index] = stats_estimator.getLabelOfNode(*node); + } +} + +template +void +pcl::DecisionTreeEvaluator:: + evaluateAndAdd( + pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& + stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data) +{ + const std::size_t num_of_examples = examples.size(); + for (int example_index = 0; example_index < num_of_examples; ++example_index) { + NodeType* node = &(tree.getRoot()); + + while (node->sub_nodes.size() != 0) { + float feature_result = 0.0f; + unsigned char flag = 0; + unsigned char branch_index = 0; + + feature_handler.evaluateFeature( + node->feature, data_set, examples[example_index], feature_result, flag); + stats_estimator.computeBranchIndex( + feature_result, flag, node->threshold, branch_index); + + node = &(node->sub_nodes[branch_index]); + } + + label_data[example_index] += stats_estimator.getLabelOfNode(*node); + } +} + +template +void +pcl::DecisionTreeEvaluator:: + evaluate(pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& + stats_estimator, + DataSet& data_set, + ExampleIndex example, + NodeType& leave) +{ + + NodeType* node = &(tree.getRoot()); + + while (!node->sub_nodes.empty()) { + float feature_result = 0.0f; + unsigned char flag = 0; + unsigned char branch_index = 0; + + feature_handler.evaluateFeature( + node->feature, data_set, example, feature_result, flag); + stats_estimator.computeBranchIndex( + feature_result, flag, node->threshold, branch_index); + + node = &(node->sub_nodes[branch_index]); + } + + leave = *node; +} + +template +void +pcl::DecisionTreeEvaluator:: + getNodes(pcl::DecisionTree& tree, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& + stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& nodes) +{ + const std::size_t num_of_examples = examples.size(); + for (int example_index = 0; example_index < num_of_examples; ++example_index) { + NodeType* node = &(tree.getRoot()); + + while (node->sub_nodes.size() != 0) { + float feature_result = 0.0f; + unsigned char flag = 0; + unsigned char branch_index = 0; + + feature_handler.evaluateFeature( + node->feature, data_set, examples[example_index], feature_result, flag); + stats_estimator.computeBranchIndex( + feature_result, node->threshold, flag, branch_index); + + node = &(node->subNodes[branch_index]); + } + + nodes.push_back(node); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_tree_trainer.hpp b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_tree_trainer.hpp new file mode 100755 index 0000000..74db81c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/impl/dt/decision_tree_trainer.hpp @@ -0,0 +1,305 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +template +pcl::DecisionTreeTrainer:: + DecisionTreeTrainer() +: max_tree_depth_(15) +, num_of_features_(1000) +, num_of_thresholds_(10) +, feature_handler_(nullptr) +, stats_estimator_(nullptr) +, data_set_() +, label_data_() +, examples_() +, decision_tree_trainer_data_provider_() +, random_features_at_split_node_(false) +{} + +template +pcl::DecisionTreeTrainer:: + ~DecisionTreeTrainer() +{} + +template +void +pcl::DecisionTreeTrainer:: + train(pcl::DecisionTree& tree) +{ + // create random features + std::vector features; + + if (!random_features_at_split_node_) + feature_handler_->createRandomFeatures(num_of_features_, features); + + // recursively build decision tree + NodeType root_node; + tree.setRoot(root_node); + + if (decision_tree_trainer_data_provider_) { + std::cerr << "use decision_tree_trainer_data_provider_" << std::endl; + + decision_tree_trainer_data_provider_->getDatasetAndLabels( + data_set_, label_data_, examples_); + trainDecisionTreeNode( + features, examples_, label_data_, max_tree_depth_, tree.getRoot()); + label_data_.clear(); + data_set_.clear(); + examples_.clear(); + } + else { + trainDecisionTreeNode( + features, examples_, label_data_, max_tree_depth_, tree.getRoot()); + } +} + +template +void +pcl::DecisionTreeTrainer:: + trainDecisionTreeNode(std::vector& features, + std::vector& examples, + std::vector& label_data, + const std::size_t max_depth, + NodeType& node) +{ + const std::size_t num_of_examples = examples.size(); + if (num_of_examples == 0) { + PCL_ERROR( + "Reached invalid point in decision tree training: Number of examples is 0!"); + return; + }; + + if (max_depth == 0) { + stats_estimator_->computeAndSetNodeStats(data_set_, examples, label_data, node); + return; + }; + + if (examples.size() < min_examples_for_split_) { + stats_estimator_->computeAndSetNodeStats(data_set_, examples, label_data, node); + return; + } + + if (random_features_at_split_node_) { + features.clear(); + feature_handler_->createRandomFeatures(num_of_features_, features); + } + + std::vector feature_results; + std::vector flags; + + feature_results.reserve(num_of_examples); + flags.reserve(num_of_examples); + + // find best feature for split + int best_feature_index = -1; + float best_feature_threshold = 0.0f; + float best_feature_information_gain = 0.0f; + + const std::size_t num_of_features = features.size(); + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + // evaluate features + feature_handler_->evaluateFeature( + features[feature_index], data_set_, examples, feature_results, flags); + + // get list of thresholds + if (!thresholds_.empty()) { + // compute information gain for each threshold and store threshold with highest + // information gain + for (std::size_t threshold_index = 0; threshold_index < thresholds_.size(); + ++threshold_index) { + + const float information_gain = + stats_estimator_->computeInformationGain(data_set_, + examples, + label_data, + feature_results, + flags, + thresholds_[threshold_index]); + + if (information_gain > best_feature_information_gain) { + best_feature_information_gain = information_gain; + best_feature_index = static_cast(feature_index); + best_feature_threshold = thresholds_[threshold_index]; + } + } + } + else { + std::vector thresholds; + thresholds.reserve(num_of_thresholds_); + createThresholdsUniform(num_of_thresholds_, feature_results, thresholds); + + // compute information gain for each threshold and store threshold with highest + // information gain + for (std::size_t threshold_index = 0; threshold_index < num_of_thresholds_; + ++threshold_index) { + const float threshold = thresholds[threshold_index]; + + // compute information gain + const float information_gain = stats_estimator_->computeInformationGain( + data_set_, examples, label_data, feature_results, flags, threshold); + + if (information_gain > best_feature_information_gain) { + best_feature_information_gain = information_gain; + best_feature_index = static_cast(feature_index); + best_feature_threshold = threshold; + } + } + } + } + + if (best_feature_index == -1) { + stats_estimator_->computeAndSetNodeStats(data_set_, examples, label_data, node); + return; + } + + // get branch indices for best feature and best threshold + std::vector branch_indices; + branch_indices.reserve(num_of_examples); + { + feature_handler_->evaluateFeature( + features[best_feature_index], data_set_, examples, feature_results, flags); + + stats_estimator_->computeBranchIndices( + feature_results, flags, best_feature_threshold, branch_indices); + } + + stats_estimator_->computeAndSetNodeStats(data_set_, examples, label_data, node); + + // separate data + { + const std::size_t num_of_branches = stats_estimator_->getNumOfBranches(); + + std::vector branch_counts(num_of_branches, 0); + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + ++branch_counts[branch_indices[example_index]]; + } + + node.feature = features[best_feature_index]; + node.threshold = best_feature_threshold; + node.sub_nodes.resize(num_of_branches); + + for (std::size_t branch_index = 0; branch_index < num_of_branches; ++branch_index) { + if (branch_counts[branch_index] == 0) { + NodeType branch_node; + stats_estimator_->computeAndSetNodeStats( + data_set_, examples, label_data, branch_node); + // branch_node->num_of_sub_nodes = 0; + + node.sub_nodes[branch_index] = branch_node; + + continue; + } + + std::vector branch_labels; + std::vector branch_examples; + branch_labels.reserve(branch_counts[branch_index]); + branch_examples.reserve(branch_counts[branch_index]); + + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + if (branch_indices[example_index] == branch_index) { + branch_examples.push_back(examples[example_index]); + branch_labels.push_back(label_data[example_index]); + } + } + + trainDecisionTreeNode(features, + branch_examples, + branch_labels, + max_depth - 1, + node.sub_nodes[branch_index]); + } + } +} + +template +void +pcl::DecisionTreeTrainer:: + createThresholdsUniform(const std::size_t num_of_thresholds, + std::vector& values, + std::vector& thresholds) +{ + // estimate range of values + float min_value = ::std::numeric_limits::max(); + float max_value = -::std::numeric_limits::max(); + + const std::size_t num_of_values = values.size(); + for (std::size_t value_index = 0; value_index < num_of_values; ++value_index) { + const float value = values[value_index]; + + if (value < min_value) + min_value = value; + if (value > max_value) + max_value = value; + } + + const float range = max_value - min_value; + const float step = range / static_cast(num_of_thresholds + 2); + + // compute thresholds + thresholds.resize(num_of_thresholds); + + for (std::size_t threshold_index = 0; threshold_index < num_of_thresholds; + ++threshold_index) { + thresholds[threshold_index] = + min_value + step * (static_cast(threshold_index + 1)); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/ml/impl/ferns/fern_evaluator.hpp b/deps_install/include/pcl-1.10/pcl/ml/impl/ferns/fern_evaluator.hpp new file mode 100755 index 0000000..ade2cc8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/impl/ferns/fern_evaluator.hpp @@ -0,0 +1,226 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include + +template +pcl::FernEvaluator:: + FernEvaluator() +{} + +template +pcl::FernEvaluator:: + ~FernEvaluator() +{} + +template +void +pcl::FernEvaluator::evaluate( + pcl::Fern& fern, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data) +{ + const std::size_t num_of_examples = examples.size(); + const std::size_t num_of_branches = stats_estimator.getNumOfBranches(); + const std::size_t num_of_features = fern.getNumOfFeatures(); + + label_data.resize(num_of_examples); + + std::vector> results(num_of_features); + std::vector> flags(num_of_features); + std::vector> branch_indices(num_of_features); + + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + results[feature_index].reserve(num_of_examples); + flags[feature_index].reserve(num_of_examples); + branch_indices[feature_index].reserve(num_of_examples); + + feature_handler.evaluateFeature(fern.accessFeature(feature_index), + data_set, + examples, + results[feature_index], + flags[feature_index]); + stats_estimator.computeBranchIndices(results[feature_index], + flags[feature_index], + fern.accessThreshold(feature_index), + branch_indices[feature_index]); + } + + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + std::size_t node_index = 0; + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + node_index *= num_of_branches; + node_index += branch_indices[feature_index][example_index]; + } + + label_data[example_index] = stats_estimator.getLabelOfNode(fern[node_index]); + } +} + +template +void +pcl::FernEvaluator:: + evaluateAndAdd( + pcl::Fern& fern, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& + stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data) +{ + const std::size_t num_of_examples = examples.size(); + const std::size_t num_of_branches = stats_estimator.getNumOfBranches(); + const std::size_t num_of_features = fern.getNumOfFeatures(); + + std::vector> results(num_of_features); + std::vector> flags(num_of_features); + std::vector> branch_indices(num_of_features); + + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + results[feature_index].reserve(num_of_examples); + flags[feature_index].reserve(num_of_examples); + branch_indices[feature_index].reserve(num_of_examples); + + feature_handler.evaluateFeature(fern.accessFeature(feature_index), + data_set, + examples, + results[feature_index], + flags[feature_index]); + stats_estimator.computeBranchIndices(results[feature_index], + flags[feature_index], + fern.accessThreshold(feature_index), + branch_indices[feature_index]); + } + + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + std::size_t node_index = 0; + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + node_index *= num_of_branches; + node_index += branch_indices[feature_index][example_index]; + } + + label_data[example_index] = stats_estimator.getLabelOfNode(fern[node_index]); + } +} + +template +void +pcl::FernEvaluator::getNodes( + pcl::Fern& fern, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& nodes) +{ + const std::size_t num_of_examples = examples.size(); + const std::size_t num_of_branches = stats_estimator.getNumOfBranches(); + const std::size_t num_of_features = fern.getNumOfFeatures(); + + nodes.reserve(num_of_examples); + + std::vector> results(num_of_features); + std::vector> flags(num_of_features); + std::vector> branch_indices(num_of_features); + + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + results[feature_index].reserve(num_of_examples); + flags[feature_index].reserve(num_of_examples); + branch_indices[feature_index].reserve(num_of_examples); + + feature_handler.evaluateFeature(fern.accessFeature(feature_index), + data_set, + examples, + results[feature_index], + flags[feature_index]); + stats_estimator.computeBranchIndices(results[feature_index], + flags[feature_index], + fern.accessThreshold(feature_index), + branch_indices[feature_index]); + } + + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + std::size_t node_index = 0; + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + node_index *= num_of_branches; + node_index += branch_indices[feature_index][example_index]; + } + + nodes.push_back(&(fern[node_index])); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/ml/impl/ferns/fern_trainer.hpp b/deps_install/include/pcl-1.10/pcl/ml/impl/ferns/fern_trainer.hpp new file mode 100755 index 0000000..608c56f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/impl/ferns/fern_trainer.hpp @@ -0,0 +1,327 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +template +pcl::FernTrainer::FernTrainer() +: fern_depth_(10) +, num_of_features_(1000) +, num_of_thresholds_(10) +, feature_handler_(nullptr) +, stats_estimator_(nullptr) +, data_set_() +, label_data_() +, examples_() +{} + +template +pcl::FernTrainer:: + ~FernTrainer() +{} + +template +void +pcl::FernTrainer::train( + pcl::Fern& fern) +{ + const std::size_t num_of_branches = stats_estimator_->getNumOfBranches(); + const std::size_t num_of_examples = examples_.size(); + + // create random features + std::vector features; + feature_handler_->createRandomFeatures(num_of_features_, features); + + // setup fern + fern.initialize(fern_depth_); + + // evaluate all features + std::vector> feature_results(num_of_features_); + std::vector> flags(num_of_features_); + + for (std::size_t feature_index = 0; feature_index < num_of_features_; + ++feature_index) { + feature_results[feature_index].reserve(num_of_examples); + flags[feature_index].reserve(num_of_examples); + + feature_handler_->evaluateFeature(features[feature_index], + data_set_, + examples_, + feature_results[feature_index], + flags[feature_index]); + } + + // iteratively select features and thresholds + std::vector>> branch_feature_results( + num_of_features_); // [feature_index][branch_index][result_index] + std::vector>> branch_flags( + num_of_features_); // [feature_index][branch_index][flag_index] + std::vector>> branch_examples( + num_of_features_); // [feature_index][branch_index][result_index] + std::vector>> branch_label_data( + num_of_features_); // [feature_index][branch_index][flag_index] + + // - initialize branch feature results and flags + for (std::size_t feature_index = 0; feature_index < num_of_features_; + ++feature_index) { + branch_feature_results[feature_index].resize(1); + branch_flags[feature_index].resize(1); + branch_examples[feature_index].resize(1); + branch_label_data[feature_index].resize(1); + + branch_feature_results[feature_index][0] = feature_results[feature_index]; + branch_flags[feature_index][0] = flags[feature_index]; + branch_examples[feature_index][0] = examples_; + branch_label_data[feature_index][0] = label_data_; + } + + for (std::size_t depth_index = 0; depth_index < fern_depth_; ++depth_index) { + // get thresholds + std::vector> thresholds(num_of_features_); + + for (std::size_t feature_index = 0; feature_index < num_of_features_; + ++feature_index) { + thresholds.reserve(num_of_thresholds_); + createThresholdsUniform(num_of_thresholds_, + feature_results[feature_index], + thresholds[feature_index]); + } + + // compute information gain + int best_feature_index = -1; + float best_feature_threshold = 0.0f; + float best_feature_information_gain = 0.0f; + + for (std::size_t feature_index = 0; feature_index < num_of_features_; + ++feature_index) { + for (std::size_t threshold_index = 0; threshold_index < num_of_thresholds_; + ++threshold_index) { + float information_gain = 0.0f; + for (std::size_t branch_index = 0; + branch_index < branch_feature_results[feature_index].size(); + ++branch_index) { + const float branch_information_gain = + stats_estimator_->computeInformationGain( + data_set_, + branch_examples[feature_index][branch_index], + branch_label_data[feature_index][branch_index], + branch_feature_results[feature_index][branch_index], + branch_flags[feature_index][branch_index], + thresholds[feature_index][threshold_index]); + + information_gain += + branch_information_gain * + branch_feature_results[feature_index][branch_index].size(); + } + + if (information_gain > best_feature_information_gain) { + best_feature_information_gain = information_gain; + best_feature_index = static_cast(feature_index); + best_feature_threshold = thresholds[feature_index][threshold_index]; + } + } + } + + // add feature to the feature list of the fern + fern.accessFeature(depth_index) = features[best_feature_index]; + fern.accessThreshold(depth_index) = best_feature_threshold; + + // update branch feature results and flags + for (std::size_t feature_index = 0; feature_index < num_of_features_; + ++feature_index) { + std::vector>& cur_branch_feature_results = + branch_feature_results[feature_index]; + std::vector>& cur_branch_flags = + branch_flags[feature_index]; + std::vector>& cur_branch_examples = + branch_examples[feature_index]; + std::vector>& cur_branch_label_data = + branch_label_data[feature_index]; + + const std::size_t total_num_of_new_branches = + num_of_branches * cur_branch_feature_results.size(); + + std::vector> new_branch_feature_results( + total_num_of_new_branches); // [branch_index][example_index] + std::vector> new_branch_flags( + total_num_of_new_branches); // [branch_index][example_index] + std::vector> new_branch_examples( + total_num_of_new_branches); // [branch_index][example_index] + std::vector> new_branch_label_data( + total_num_of_new_branches); // [branch_index][example_index] + + for (std::size_t branch_index = 0; + branch_index < cur_branch_feature_results.size(); + ++branch_index) { + const std::size_t num_of_examples_in_this_branch = + cur_branch_feature_results[branch_index].size(); + + std::vector branch_indices; + branch_indices.reserve(num_of_examples_in_this_branch); + + stats_estimator_->computeBranchIndices(cur_branch_feature_results[branch_index], + cur_branch_flags[branch_index], + best_feature_threshold, + branch_indices); + + // split results into different branches + const std::size_t base_branch_index = branch_index * num_of_branches; + for (std::size_t example_index = 0; + example_index < num_of_examples_in_this_branch; + ++example_index) { + const std::size_t combined_branch_index = + base_branch_index + branch_indices[example_index]; + + new_branch_feature_results[combined_branch_index].push_back( + cur_branch_feature_results[branch_index][example_index]); + new_branch_flags[combined_branch_index].push_back( + cur_branch_flags[branch_index][example_index]); + new_branch_examples[combined_branch_index].push_back( + cur_branch_examples[branch_index][example_index]); + new_branch_label_data[combined_branch_index].push_back( + cur_branch_label_data[branch_index][example_index]); + } + } + + branch_feature_results[feature_index] = new_branch_feature_results; + branch_flags[feature_index] = new_branch_flags; + branch_examples[feature_index] = new_branch_examples; + branch_label_data[feature_index] = new_branch_label_data; + } + } + + // set node statistics + // - re-evaluate selected features + std::vector> final_feature_results( + fern_depth_); // [feature_index][example_index] + std::vector> final_flags( + fern_depth_); // [feature_index][example_index] + std::vector> final_branch_indices( + fern_depth_); // [feature_index][example_index] + for (std::size_t depth_index = 0; depth_index < fern_depth_; ++depth_index) { + final_feature_results[depth_index].reserve(num_of_examples); + final_flags[depth_index].reserve(num_of_examples); + final_branch_indices[depth_index].reserve(num_of_examples); + + feature_handler_->evaluateFeature(fern.accessFeature(depth_index), + data_set_, + examples_, + final_feature_results[depth_index], + final_flags[depth_index]); + + stats_estimator_->computeBranchIndices(final_feature_results[depth_index], + final_flags[depth_index], + fern.accessThreshold(depth_index), + final_branch_indices[depth_index]); + } + + // - distribute examples to nodes + std::vector> node_labels( + 0x1 << fern_depth_); // [node_index][example_index] + std::vector> node_examples( + 0x1 << fern_depth_); // [node_index][example_index] + + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + std::size_t node_index = 0; + for (std::size_t depth_index = 0; depth_index < fern_depth_; ++depth_index) { + node_index *= num_of_branches; + node_index += final_branch_indices[depth_index][example_index]; + } + + node_labels[node_index].push_back(label_data_[example_index]); + node_examples[node_index].push_back(examples_[example_index]); + } + + // - compute and set statistics for every node + const std::size_t num_of_nodes = 0x1 << fern_depth_; + for (std::size_t node_index = 0; node_index < num_of_nodes; ++node_index) { + stats_estimator_->computeAndSetNodeStats(data_set_, + node_examples[node_index], + node_labels[node_index], + fern[node_index]); + } +} + +template +void +pcl::FernTrainer:: + createThresholdsUniform(const std::size_t num_of_thresholds, + std::vector& values, + std::vector& thresholds) +{ + // estimate range of values + float min_value = ::std::numeric_limits::max(); + float max_value = -::std::numeric_limits::max(); + + const std::size_t num_of_values = values.size(); + for (int value_index = 0; value_index < num_of_values; ++value_index) { + const float value = values[value_index]; + + if (value < min_value) + min_value = value; + if (value > max_value) + max_value = value; + } + + const float range = max_value - min_value; + const float step = range / (num_of_thresholds + 2); + + // compute thresholds + thresholds.resize(num_of_thresholds); + + for (int threshold_index = 0; threshold_index < num_of_thresholds; + ++threshold_index) { + thresholds[threshold_index] = min_value + step * (threshold_index + 1); + } +} diff --git a/deps_install/include/pcl-1.10/pcl/ml/impl/svm/svm_wrapper.hpp b/deps_install/include/pcl-1.10/pcl/ml/impl/svm/svm_wrapper.hpp new file mode 100755 index 0000000..f7976f3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/impl/svm/svm_wrapper.hpp @@ -0,0 +1,41 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of copyright holders nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include diff --git a/deps_install/include/pcl-1.10/pcl/ml/kmeans.h b/deps_install/include/pcl-1.10/pcl/ml/kmeans.h new file mode 100755 index 0000000..1cac4e6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/kmeans.h @@ -0,0 +1,190 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace pcl { + +/** K-means clustering. + * + * \author Christian Potthast + * \ingroup ML + */ +class PCL_EXPORTS Kmeans { +public: + using PointId = unsigned int; // the id of this point + using ClusterId = unsigned int; // the id of this cluster + + // using Point = std::vector; // a point (a centroid) + + using SetPoints = std::set; // set of points + + using Point = std::vector; + + // ClusterId -> (PointId, PointId, PointId, .... ) + using ClustersToPoints = std::vector; + // PointId -> ClusterId + using PointsToClusters = std::vector; + // coll of centroids + using Centroids = std::vector; + + /** Empty constructor. */ + Kmeans(unsigned int num_points, unsigned int num_dimensions); + + /** This destructor destroys. */ + ~Kmeans(); + + /** This method sets the k-means cluster size. + * + * \param[in] k number of clusters + */ + void + setClusterSize(unsigned int k) + { + num_clusters_ = k; + }; + + /* + void + setClusterField (std::string field_name) + { + cluster_field_name_ = field_name; + }; + */ + + // void + // getClusterCentroids (PointT &out); + + // void + // cluster (std::vector &clusters); + + void + kMeans(); + + void + setInputData(std::vector& data) + { + if (num_points_ != data.size()) + std::cout << "Data vector not the same" << std::endl; + + data_ = data; + } + + void + addDataPoint(Point& data_point) + { + if (num_dimensions_ != data_point.size()) + std::cout << "Dimensions not the same" << std::endl; + + data_.push_back(data_point); + } + + // Initial partition points among available clusters + void + initialClusterPoints(); + + void + computeCentroids(); + + // distance between two points + float + distance(const Point& x, const Point& y) + { + float total = 0.0; + float diff; + + auto cpy = y.cbegin(); + for (auto cpx = x.cbegin(), cpx_end = x.cend(); cpx != cpx_end; ++cpx, ++cpy) { + diff = *cpx - *cpy; + total += (diff * diff); + } + return total; // no need to take sqrt, which is monotonic + } + + Centroids + get_centroids() + { + return centroids_; + } + +protected: + // Members derived from the base class + /* + using BasePCLBase::input_; + using BasePCLBase::indices_; + using BasePCLBase::initCompute; + using BasePCLBase::deinitCompute; + */ + + unsigned int num_points_; + unsigned int num_dimensions_; + + /** The number of clusters. */ + unsigned int num_clusters_; + + /** The cluster centroids. */ + // std::vector + + // std::string cluster_field_name_; + + // one data point + + // all data points + std::vector data_; + + ClustersToPoints clusters_to_points_; + PointsToClusters points_to_clusters_; + Centroids centroids_; + +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_comparison_feature.h b/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_comparison_feature.h new file mode 100755 index 0000000..7b4bf73 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_comparison_feature.h @@ -0,0 +1,90 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** Feature for comparing two sample points in 2D multi-channel data. */ +template +class PCL_EXPORTS MultiChannel2DComparisonFeature { +public: + /** Constructor. */ + MultiChannel2DComparisonFeature() : p1(), p2(), channel(0) {} + + /** Destructor. */ + virtual ~MultiChannel2DComparisonFeature() {} + + /** Serializes the feature to a stream. + * + * \param[out] stream the destination for the serialization + */ + inline void + serialize(std::ostream& stream) const + { + p1.serialize(stream); + p2.serialize(stream); + stream.write(reinterpret_cast(&channel), sizeof(channel)); + } + + /** Deserializes the feature from a stream. + * + * \param[in] stream the source for the deserialization + */ + inline void + deserialize(std::istream& stream) + { + p1.deserialize(stream); + p2.deserialize(stream); + stream.read(reinterpret_cast(&channel), sizeof(channel)); + } + +public: + /** First sample point. */ + PointT p1; + /** Second sample point. */ + PointT p2; + /** Specifies which channel is used for comparison. */ + unsigned char channel; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_comparison_feature_handler.h b/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_comparison_feature_handler.h new file mode 100755 index 0000000..93d9372 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_comparison_feature_handler.h @@ -0,0 +1,462 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace pcl { + +/** Feature utility class that handles the creation and evaluation of RGBD + * comparison features. */ +template +class PCL_EXPORTS MultiChannel2DComparisonFeatureHandler +: public pcl::FeatureHandler, + pcl::MultiChannel2DDataSet, + pcl::MultipleData2DExampleIndex> { + +public: + /** Constructor. */ + MultiChannel2DComparisonFeatureHandler(const int feature_window_width, + const int feature_window_height) + : feature_window_width_(feature_window_width) + , feature_window_height_(feature_window_height) + {} + + /** Destructor. */ + virtual ~MultiChannel2DComparisonFeatureHandler() {} + + /** Sets the feature window size. + * + * \param[in] width the width of the feature window + * \param[in] height the height of the feature window + */ + inline void + setFeatureWindowSize(int width, int height) + { + feature_window_width_ = width; + feature_window_height_ = height; + } + + /** Creates random features. + * + * \param[in] num_of_features the number of random features to create + * \param[out] features the destination for the created random features + */ + inline void + createRandomFeatures( + const std::size_t num_of_features, + std::vector>& features) + { + features.resize(num_of_features); + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + features[feature_index].p1 = PointXY32i::randomPoint(-feature_window_width_ / 2, + feature_window_width_ / 2, + -feature_window_height_ / 2, + feature_window_height_ / 2); + features[feature_index].p2 = PointXY32i::randomPoint(-feature_window_width_ / 2, + feature_window_width_ / 2, + -feature_window_height_ / 2, + feature_window_height_ / 2); + features[feature_index].channel = static_cast( + NUM_OF_CHANNELS * (static_cast(rand()) / (RAND_MAX + 1))); + } + } + + /** Evaluates a feature for a set of examples on the specified data set. + * + * \param[in] feature the feature to evaluate + * \param[in] data_set the data set the feature is evaluated on + * \param[in] examples the examples the feature is evaluated for + * \param[out] results the destination for the evaluation results + * \param[out] flags the destination for the flags corresponding to the evaluation + * results + */ + inline void + evaluateFeature(const MultiChannel2DComparisonFeature& feature, + MultiChannel2DDataSet& data_set, + std::vector& examples, + std::vector& results, + std::vector& flags) const + { + results.resize(examples.size()); + flags.resize(examples.size()); + for (int example_index = 0; example_index < examples.size(); ++example_index) { + const MultipleData2DExampleIndex& example = examples[example_index]; + + evaluateFeature( + feature, data_set, example, results[example_index], flags[example_index]); + } + } + + /** Evaluates a feature for one examples on the specified data set. + * + * \param[in] feature the feature to evaluate + * \param[in] data_set the data set the feature is evaluated on + * \param[in] example the example the feature is evaluated for + * \param[out] result the destination for the evaluation result + * \param[out] flag the destination for the flag corresponding to the evaluation + * result + */ + inline void + evaluateFeature(const MultiChannel2DComparisonFeature& feature, + MultiChannel2DDataSet& data_set, + const MultipleData2DExampleIndex& example, + float& result, + unsigned char& flag) const + { + const int center_col_index = example.x; + const int center_row_index = example.y; + + const std::size_t p1_col = + static_cast(feature.p1.x + center_col_index); + const std::size_t p1_row = + static_cast(feature.p1.y + center_row_index); + + const std::size_t p2_col = + static_cast(feature.p2.x + center_col_index); + const std::size_t p2_row = + static_cast(feature.p2.y + center_row_index); + + const unsigned char channel = feature.channel; + + const float value1 = + static_cast(data_set(example.data_set_id, p1_col, p1_row)[channel]); + const float value2 = + static_cast(data_set(example.data_set_id, p2_col, p2_row)[channel]); + + result = value1 - value2; + flag = (std::isfinite(value1) && std::isfinite(value2)) ? 0 : 1; + } + + /** Generates code for feature evaluation. + * + * \param[in] feature the feature for which code is generated + * \param[out] stream the destination for the generated code + */ + void + generateCodeForEvaluation(const MultiChannel2DComparisonFeature& feature, + std::ostream& stream) const + { + stream << "ERROR: RegressionVarianceStatsEstimator does not implement " + "generateCodeForBranchIndex(...)"; + // stream << "const float value = ( (*dataSet)(dataSetId, centerY+" << feature.p1.y + // << ", centerX+" << feature.p1.x << ")[" << static_cast(feature.colorChannel) + // << "]" + // << " - " << "(*dataSet)(dataSetId, centerY+" << feature.p2.y << ", centerX+" << + // feature.p2.x << ")[" << static_cast(feature.colorChannel) << "] );" << + // ::std::endl; + } + +private: + /** The width of the feature window. */ + int feature_window_width_; + /** The height of the feature window. */ + int feature_window_height_; +}; + +/** Feature utility class that handles the creation and evaluation of RGBD + * comparison features. */ +template +class PCL_EXPORTS ScaledMultiChannel2DComparisonFeatureHandler +: public pcl::FeatureHandler, + pcl::MultiChannel2DDataSet, + pcl::MultipleData2DExampleIndex> { + +public: + /** Constructor. */ + ScaledMultiChannel2DComparisonFeatureHandler(const int feature_window_width, + const int feature_window_height) + : feature_window_width_(feature_window_width) + , feature_window_height_(feature_window_height) + {} + + /** Destructor. */ + virtual ~ScaledMultiChannel2DComparisonFeatureHandler() {} + + /** Sets the feature window size. + * + * \param[in] width the width of the feature window + * \param[in] height the height of the feature window + */ + inline void + setFeatureWindowSize(int width, int height) + { + feature_window_width_ = width; + feature_window_height_ = height; + } + + /** Creates random features. + * + * \param[in] num_of_features the number of random features to create + * \param[out] features the destination for the created random features + */ + inline void + createRandomFeatures( + const std::size_t num_of_features, + std::vector>& features) + { + features.resize(num_of_features); + for (std::size_t feature_index = 0; feature_index < num_of_features; + ++feature_index) { + features[feature_index].p1 = PointXY32f::randomPoint(-feature_window_width_ / 2, + feature_window_width_ / 2, + -feature_window_height_ / 2, + feature_window_height_ / 2); + features[feature_index].p2 = PointXY32f::randomPoint(-feature_window_width_ / 2, + feature_window_width_ / 2, + -feature_window_height_ / 2, + feature_window_height_ / 2); + features[feature_index].channel = static_cast( + NUM_OF_CHANNELS * (static_cast(rand()) / (RAND_MAX + 1))); + } + } + + /** Evaluates a feature for a set of examples on the specified data set. + * + * \param[in] feature the feature to evaluate + * \param[in] data_set the data set the feature is evaluated on + * \param[in] examples the examples the feature is evaluated for + * \param[out] results the destination for the evaluation results + * \param[out] flags the destination for the flags corresponding to the evaluation + * results + */ + inline void + evaluateFeature(const MultiChannel2DComparisonFeature& feature, + MultiChannel2DDataSet& data_set, + std::vector& examples, + std::vector& results, + std::vector& flags) const + { + results.resize(examples.size()); + flags.resize(examples.size()); + for (int example_index = 0; example_index < examples.size(); ++example_index) { + const MultipleData2DExampleIndex& example = examples[example_index]; + + evaluateFeature( + feature, data_set, example, results[example_index], flags[example_index]); + } + } + + /** Evaluates a feature for one examples on the specified data set. + * + * \param[in] feature the feature to evaluate + * \param[in] data_set the data set the feature is evaluated on + * \param[in] example the example the feature is evaluated for + * \param[out] result the destination for the evaluation result + * \param[out] flag the destination for the flag corresponding to the evaluation + * result + */ + inline void + evaluateFeature(const MultiChannel2DComparisonFeature& feature, + MultiChannel2DDataSet& data_set, + const MultipleData2DExampleIndex& example, + float& result, + unsigned char& flag) const + { + const int center_col_index = example.x; + const int center_row_index = example.y; + + float scale; + if (INVERT_SCALE) + scale = 1.0f / static_cast(data_set(example.data_set_id, + center_col_index, + center_row_index)[SCALE_CHANNEL]); + else + scale = static_cast(data_set( + example.data_set_id, center_col_index, center_row_index)[SCALE_CHANNEL]); + + const std::size_t p1_col = + static_cast(scale * feature.p1.x + center_col_index); + const std::size_t p1_row = + static_cast(scale * feature.p1.y + center_row_index); + + const std::size_t p2_col = + static_cast(scale * feature.p2.x + center_col_index); + const std::size_t p2_row = + static_cast(scale * feature.p2.y + center_row_index); + + const unsigned char channel = feature.channel; + + const float value1 = + static_cast(data_set(example.data_set_id, p1_col, p1_row)[channel]); + const float value2 = + static_cast(data_set(example.data_set_id, p2_col, p2_row)[channel]); + + result = value1 - value2; + flag = (std::isfinite(value1) && std::isfinite(value2)) ? 0 : 1; + } + + /** Generates code for feature evaluation. + * + * \param[in] feature the feature for which code is generated + * \param[out] stream the destination for the generated code + */ + void + generateCodeForEvaluation(const MultiChannel2DComparisonFeature& feature, + std::ostream& stream) const + { + stream << "ERROR: ScaledMultiChannel2DComparisonFeatureHandler does not implement " + "generateCodeForBranchIndex(...)" + << std::endl; + + // pcl::PointXY32f p1 = feature.p1; + // pcl::PointXY32f p2 = feature.p2; + + // stream << "const float eval_value = data_ptr + " << p1.x << " + " << p1.y << " * + // width; + + // stream << "const float value = ( (*dataSet)(dataSetId, centerY+" << feature.p1.y + // << ", centerX+" << feature.p1.x << ")[" << static_cast(feature.colorChannel) + // << "]" + // << " - " << "(*dataSet)(dataSetId, centerY+" << feature.p2.y << ", centerX+" << + // feature.p2.x << ")[" << static_cast(feature.colorChannel) << "] );" << + // ::std::endl; + } + +private: + /** The width of the feature window. */ + int feature_window_width_; + /** The height of the feature window. */ + int feature_window_height_; +}; + +template +class PCL_EXPORTS ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator +: public pcl::FeatureHandlerCodeGenerator< + pcl::MultiChannel2DComparisonFeature, + pcl::MultiChannel2DDataSet, + pcl::MultipleData2DExampleIndex> { +public: + ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator() {} + virtual ~ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator() {} + + void + generateEvalFunctionCode(std::ostream& stream) const; + + void + generateEvalCode(const MultiChannel2DComparisonFeature& feature, + std::ostream& stream) const; +}; + +template +void +ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator< + DATA_TYPE, + NUM_OF_CHANNELS, + SCALE_CHANNEL, + INVERT_SCALE>::generateEvalFunctionCode(std::ostream& stream) const +{ + if (NUM_OF_CHANNELS == 1 && SCALE_CHANNEL == 0 && INVERT_SCALE) { + stream << "const float scale = 1.0f / static_cast (*data_ptr);" + << std::endl; + stream << "" << std::endl; + stream << "struct LocalFeatureHandler" << std::endl; + stream << "{" << std::endl; + stream << " static inline void eval (" << typeid(DATA_TYPE).name() + << " * a_ptr, const float a_x1, const float a_y1, const float a_x2, const " + "float a_y2, const float a_scale, const int a_width, float & a_result, " + "unsigned char & a_flags)" + << std::endl; + stream << " {" << std::endl; + stream << " a_result = *(a_ptr + static_cast (a_scale*a_x1) + " + "(static_cast (a_scale*a_y1)*a_width)) - *(a_ptr + static_cast " + "(a_scale*a_x2) + (static_cast (a_scale*a_y2)*a_width));" + << std::endl; + stream << " }" << std::endl; + stream << "};" << std::endl; + } + else { + stream << "ERROR: generateEvalFunctionCode not implemented" << std::endl; + } +} + +template +void +ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator:: + generateEvalCode(const MultiChannel2DComparisonFeature& feature, + std::ostream& stream) const +{ + stream << "LocalFeatureHandler::eval (data_ptr, " << feature.p1.x << ", " + << feature.p1.y << ", " << feature.p2.x << ", " << feature.p2.y << ", " + << "scale, width, result, flags);" << std::endl; +} + +using Depth2DComparisonFeatureHandler = + MultiChannel2DComparisonFeatureHandler; +using IntensityDepth2DComparisonFeatureHandler = + MultiChannel2DComparisonFeatureHandler; +using RGB2DComparisonFeatureHandler = MultiChannel2DComparisonFeatureHandler; +using RGBD2DComparisonFeatureHandler = MultiChannel2DComparisonFeatureHandler; + +using ScaledDepth2DComparisonFeatureHandler = + ScaledMultiChannel2DComparisonFeatureHandler; +using ScaledIntensityDepth2DComparisonFeatureHandler = + ScaledMultiChannel2DComparisonFeatureHandler; +using ScaledRGBD2DComparisonFeatureHandler = + ScaledMultiChannel2DComparisonFeatureHandler; + +using ScaledDepth2DComparisonFeatureHandlerCCodeGenerator = + ScaledMultiChannel2DComparisonFeatureHandlerCCodeGenerator; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_data_set.h b/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_data_set.h new file mode 100755 index 0000000..9eba52c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/multi_channel_2d_data_set.h @@ -0,0 +1,250 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** Holds two-dimensional multi-channel data. */ +template +class PCL_EXPORTS MultiChannel2DData { +public: + /** Constructor. */ + inline MultiChannel2DData() : data_(NULL), width_(0), height_(0) {} + + /** Destructor. */ + virtual ~MultiChannel2DData() {} + + /** Resizes the internal data storage. + * + * \param[in] width the width of the resized 2D data array + * \param[in] height the height of the resized 2D data array + */ + inline void + resize(std::size_t width, std::size_t height) + { + data_.resize(NUM_OF_CHANNELS * width * height); + width_ = width; + height_ = height; + } + + /** Clears the internal data storage and sets width and height to 0. */ + void + clear() + { + width_ = 0; + height_ = 0; + data_.clear(); + } + + /** Returns a pointer to the internal data at the specified location. + * + * \param[in] col_index the column index + * \param[in] row_index the row index + */ + inline DATA_TYPE* + operator()(const std::size_t col_index, const std::size_t row_index) + { + return &(data_[NUM_OF_CHANNELS * (row_index * width_ + col_index)]); + }; + + /** Returns a pointer to the internal data at the specified location. + * + * \param[in] col_index the column index + * \param[in] row_index the row index + */ + inline const DATA_TYPE* + operator()(const std::size_t col_index, const std::size_t row_index) const + { + return &(data_[NUM_OF_CHANNELS * (row_index * width_ + col_index)]); + }; + + /** Returns a reference to the internal data at the specified location. + * + * \param[in] col_index the column index + * \param[in] row_index the row index + * \param[in] channel the channel index + */ + inline DATA_TYPE& + operator()(const std::size_t col_index, + const std::size_t row_index, + const std::size_t channel) + { + return data_[NUM_OF_CHANNELS * (row_index * width_ + col_index) + channel]; + }; + + /** Returns a reference to the internal data at the specified location. + * + * \param[in] col_index the column index + * \param[in] row_index the row index + * \param[in] channel the channel index + */ + inline const DATA_TYPE& + operator()(const std::size_t col_index, + const std::size_t row_index, + const std::size_t channel) const + { + return data_[NUM_OF_CHANNELS * (row_index * width_ + col_index) + channel]; + }; + +private: + /** The internal data storage. */ + std::vector data_; + /** The width of the data storage. */ + std::size_t width_; + /** The height of the data storage. */ + std::size_t height_; +}; + +/** Holds a set of two-dimensional multi-channel data. */ +template +class PCL_EXPORTS MultiChannel2DDataSet { +public: + /** Constructor. */ + inline MultiChannel2DDataSet() : data_set_() {} + + /** Destructor. */ + virtual ~MultiChannel2DDataSet() {} + + /** Adds a new two-dimensional data block to the data set. + * + * \param[in] width the width of the new data block + * \param[in] height the height of the new data block + */ + void + addData(const std::size_t width, const std::size_t height) + { + MultiChannel2DData* data = + new MultiChannel2DData(); + data->resize(width, height); + + data_set_.push_back(data); + }; + + /** Releases the data stored in the data set. */ + void + releaseDataSet() + { + for (std::size_t data_set_index = 0; data_set_index < data_set_.size(); + ++data_set_index) { + delete data_set_[data_set_index]; + } + } + + /** Releases the data stored in the data set. */ + void + clear() + { + releaseDataSet(); + } + + /** Returns a pointer to the specified data block at the specified location. + * + * \param[in] data_set_id the index of the data block + * \param[in] col the column of the desired location + * \param[in] row the row of the desired location + */ + inline DATA_TYPE* + operator()(const std::size_t data_set_id, + const std::size_t col, + const std::size_t row) + { + return (*data_set_[data_set_id])(col, row); + }; + + /** Returns a pointer to the specified data block at the specified location. + * + * \param[in] data_set_id the index of the data block + * \param[in] col the column of the desired location + * \param[in] row the row of the desired location + */ + inline const DATA_TYPE* + operator()(const std::size_t data_set_id, + const std::size_t col, + const std::size_t row) const + { + return (*data_set_[data_set_id])(col, row); + }; + + /** Returns a reference to the specified data block at the specified location. + * + * \param[in] data_set_id the index of the data block + * \param[in] col the column of the desired location + * \param[in] row the row of the desired location + * \param[in] channel the channel index + */ + inline DATA_TYPE& + operator()(const std::size_t data_set_id, + const std::size_t col, + const std::size_t row, + const std::size_t channel) + { + return (*data_set_[data_set_id])(col, row, channel); + }; + + /** Returns a reference to the specified data block at the specified location. + * + * \param[in] data_set_id the index of the data block + * \param[in] col the column of the desired location + * \param[in] row the row of the desired location + * \param[in] channel the channel index + */ + inline const DATA_TYPE& + operator()(const std::size_t data_set_id, + const std::size_t col, + const std::size_t row, + const std::size_t channel) const + { + return (*data_set_[data_set_id])(col, row, channel); + }; + +private: + /** The data set. */ + std::vector*> data_set_; +}; + +using Depth2DDataSet = MultiChannel2DDataSet; +using IntensityDepth2DDataSet = MultiChannel2DDataSet; +using RGB2DDataSet = MultiChannel2DDataSet; +using RGBD2DDataSet = MultiChannel2DDataSet; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/multiple_data_2d_example_index.h b/deps_install/include/pcl-1.10/pcl/ml/multiple_data_2d_example_index.h new file mode 100755 index 0000000..438ceac --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/multiple_data_2d_example_index.h @@ -0,0 +1,57 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** Example index for a set of 2D data blocks. */ +struct MultipleData2DExampleIndex { + /** The data set index. */ + int data_set_id; + /** The x-coordinate. */ + int x; + /** The y-coordinate. */ + int y; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/pairwise_potential.h b/deps_install/include/pcl-1.10/pcl/ml/pairwise_potential.h new file mode 100755 index 0000000..9b16bdc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/pairwise_potential.h @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#pragma once + +#include + +#include + +namespace pcl { + +class PairwisePotential { +public: + /** Constructor for PairwisePotential class. */ + PairwisePotential(const std::vector& feature, + const int D, + const int N, + const float w); + + /** Deconstructor for PairwisePotential class. */ + ~PairwisePotential(){}; + + void + compute(std::vector& out, + const std::vector& in, + std::vector& tmp, + int value_size) const; + +protected: + /// Permutohedral lattice + Permutohedral lattice_; + + /// Number of variables + int N_; + + /// Weight + float w_; + + /// Norm + std::vector norm_; + +public: + std::vector bary_; + std::vector features_; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/permutohedral.h b/deps_install/include/pcl-1.10/pcl/ml/permutohedral.h new file mode 100755 index 0000000..bfee826 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/permutohedral.h @@ -0,0 +1,304 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#include +#include +#include +#include + +// TODO: SWAP with Boost intrusive hash table +#include +#include +#include +#include +#include + +#include + +namespace pcl { +/** Implementation of a high-dimensional gaussian filtering using the permutohedral + * lattice. + * + * \author Christian Potthast (potthast@usc.edu) + * + * Adams_fasthigh-dimensional + * author = {Andrew Adams and Jongmin Baek and Myers Abraham Davis}, + * title = {Fast high-dimensional filtering using the permutohedral lattice}, + * booktitle = {Computer Graphics Forum (EG 2010 Proceedings}, + * year = {}, + * pages = {2010} + * } + */ + +class Permutohedral { +protected: + struct Neighbors { + int n1, n2; + Neighbors(int n1 = 0, int n2 = 0) : n1(n1), n2(n2) {} + }; + +public: + /** Constructor for Permutohedral class. */ + Permutohedral(); + + /** Deconstructor for Permutohedral class. */ + ~Permutohedral(){}; + + /** Initialization. */ + void + init(const std::vector& feature, const int feature_dimension, const int N); + + void + compute(std::vector& out, + const std::vector& in, + int value_size, + int in_offset = 0, + int out_offset = 0, + int in_size = -1, + int out_size = -1) const; + + void + initOLD(const std::vector& feature, const int feature_dimension, const int N); + + void + computeOLD(std::vector& out, + const std::vector& in, + int value_size, + int in_offset = 0, + int out_offset = 0, + int in_size = -1, + int out_size = -1) const; + + void + debug(); + + /** Pseudo radnom generator. */ + inline std::size_t + generateHashKey(const std::vector& k) + { + std::size_t r = 0; + for (int i = 0; i < d_; i++) { + r += k[i]; + r *= 1664525; + // r *= 5; + } + return r; // % (2* N_ * (d_+1)); + } + +public: + /// Number of variables + int N_; + + std::vector blur_neighbors_; + + /// Size of sparse discretized space + int M_; + + /// Dimension of feature + int d_; + + std::vector offset_; + std::vector offsetTMP_; + std::vector barycentric_; + + Neighbors* blur_neighborsOLD_; + int* offsetOLD_; + float* barycentricOLD_; + std::vector baryOLD_; + +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +class HashTableOLD { + // Don't copy! + HashTableOLD(const HashTableOLD& o) + : key_size_(o.key_size_), filled_(0), capacity_(o.capacity_) + { + table_ = new int[capacity_]; + keys_ = new short[(capacity_ / 2 + 10) * key_size_]; + memset(table_, -1, capacity_ * sizeof(int)); + } + +protected: + std::size_t key_size_, filled_, capacity_; + short* keys_; + int* table_; + + void + grow() + { + std::cout << "GROW" << std::endl; + + // Swap out the old memory + short* old_keys = keys_; + int* old_table = table_; + int old_capacity = static_cast(capacity_); + capacity_ *= 2; + // Allocate the new memory + keys_ = new short[(old_capacity + 10) * key_size_]; + table_ = new int[capacity_]; + memset(table_, -1, capacity_ * sizeof(int)); + memcpy(keys_, old_keys, filled_ * key_size_ * sizeof(short)); + + // Reinsert each element + for (int i = 0; i < old_capacity; i++) + if (old_table[i] >= 0) { + int e = old_table[i]; + std::size_t h = hash(old_keys + (getKey(e) - keys_)) % capacity_; + for (; table_[h] >= 0; h = h < capacity_ - 1 ? h + 1 : 0) { + }; + table_[h] = e; + } + + delete[] old_keys; + delete[] old_table; + } + + std::size_t + hash(const short* k) + { + std::size_t r = 0; + for (std::size_t i = 0; i < key_size_; i++) { + r += k[i]; + r *= 1664525; + } + return r; + } + +public: + explicit HashTableOLD(int key_size, int n_elements) + : key_size_(key_size), filled_(0), capacity_(2 * n_elements) + { + table_ = new int[capacity_]; + keys_ = new short[(capacity_ / 2 + 10) * key_size_]; + memset(table_, -1, capacity_ * sizeof(int)); + } + + ~HashTableOLD() + { + delete[] keys_; + delete[] table_; + } + + int + size() const + { + return static_cast(filled_); + } + + void + reset() + { + filled_ = 0; + memset(table_, -1, capacity_ * sizeof(int)); + } + + int + find(const short* k, bool create = false) + { + if (2 * filled_ >= capacity_) + grow(); + // Get the hash value + std::size_t h = hash(k) % capacity_; + // Find the element with he right key, using linear probing + while (1) { + int e = table_[h]; + if (e == -1) { + if (create) { + // Insert a new key and return the new id + for (std::size_t i = 0; i < key_size_; i++) + keys_[filled_ * key_size_ + i] = k[i]; + return table_[h] = static_cast(filled_++); + } + else + return -1; + } + // Check if the current key is The One + bool good = true; + for (std::size_t i = 0; i < key_size_ && good; i++) + if (keys_[e * key_size_ + i] != k[i]) + good = false; + if (good) + return e; + // Continue searching + h++; + if (h == capacity_) + h = 0; + } + } + + const short* + getKey(int i) const + { + return keys_ + i * key_size_; + } +}; + +/* +class HashTable +{ + public: + HashTable ( int N ) : N_ (2 * N) {}; + + find (const std::vector &k, bool create = false;) + { + + + + + + } + + + + protected: + std::multimap table_; + + std::vector > keys; + //keys.reserve ( (d_+1) * N_ ); + // number of elements + int N_; +};*/ + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/point_xy_32f.h b/deps_install/include/pcl-1.10/pcl/ml/point_xy_32f.h new file mode 100755 index 0000000..feb9ea6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/point_xy_32f.h @@ -0,0 +1,94 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** 2D point with float x- and y-coordinates. */ +class PCL_EXPORTS PointXY32f { +public: + /** Constructor. */ + inline PointXY32f() : x(0.0f), y(0.0f) {} + /** Destructor. */ + inline virtual ~PointXY32f() {} + + /** Serializes the point to the specified stream. + * + * \param[out] stream the destination for the serialization + */ + inline void + serialize(std::ostream& stream) const + { + stream.write(reinterpret_cast(&x), sizeof(x)); + stream.write(reinterpret_cast(&y), sizeof(y)); + } + + /** Deserializes the point from the specified stream. + * + * \param[in] stream the source for the deserialization + */ + inline void + deserialize(std::istream& stream) + { + stream.read(reinterpret_cast(&x), sizeof(x)); + stream.read(reinterpret_cast(&y), sizeof(y)); + } + + /** Creates a random point within the specified window. + * + * \param[in] min_x the minimum value for the x-coordinate of the point + * \param[in] max_x the maximum value for the x-coordinate of the point + * \param[in] min_y the minimum value for the y-coordinate of the point + * \param[in] max_y the maximum value for the y-coordinate of the point + */ + static PointXY32f + randomPoint(const int min_x, const int max_x, const int min_y, const int max_y); + +public: + /** The x-coordinate of the point. */ + float x; + /** The y-coordinate of the point. */ + float y; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/point_xy_32i.h b/deps_install/include/pcl-1.10/pcl/ml/point_xy_32i.h new file mode 100755 index 0000000..a62deca --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/point_xy_32i.h @@ -0,0 +1,95 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** 2D point with integer x- and y-coordinates. */ +class PCL_EXPORTS PointXY32i { +public: + /** Constructor. */ + inline PointXY32i() : x(0), y(0) {} + + /** Destructor. */ + inline virtual ~PointXY32i() {} + + /** Serializes the point to the specified stream. + * + * \param[out] stream the destination for the serialization + */ + inline void + serialize(std::ostream& stream) const + { + stream.write(reinterpret_cast(&x), sizeof(x)); + stream.write(reinterpret_cast(&y), sizeof(y)); + } + + /** Deserializes the point from the specified stream. + * + * \param[in] stream the source for the deserialization + */ + inline void + deserialize(std::istream& stream) + { + stream.read(reinterpret_cast(&x), sizeof(x)); + stream.read(reinterpret_cast(&y), sizeof(y)); + } + + /** Creates a random point within the specified window. + * + * \param[in] min_x the minimum value for the x-coordinate of the point + * \param[in] max_x the maximum value for the x-coordinate of the point + * \param[in] min_y the minimum value for the y-coordinate of the point + * \param[in] max_y the maximum value for the y-coordinate of the point + */ + static PointXY32i + randomPoint(const int min_x, const int max_x, const int min_y, const int max_y); + +public: + /** The x-coordinate of the point. */ + int x; + /** The y-coordinate of the point. */ + int y; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/regression_variance_stats_estimator.h b/deps_install/include/pcl-1.10/pcl/ml/regression_variance_stats_estimator.h new file mode 100755 index 0000000..2b93aad --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/regression_variance_stats_estimator.h @@ -0,0 +1,331 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace pcl { + +/** Node for a regression trees which optimizes variance. */ +template +class PCL_EXPORTS RegressionVarianceNode { +public: + /** Constructor. */ + RegressionVarianceNode() : value(0), variance(0), threshold(0), sub_nodes() {} + + /** Destructor. */ + virtual ~RegressionVarianceNode() {} + + /** Serializes the node to the specified stream. + * + * \param[out] stream the destination for the serialization + */ + inline void + serialize(std::ostream& stream) const + { + feature.serialize(stream); + + stream.write(reinterpret_cast(&threshold), sizeof(threshold)); + + stream.write(reinterpret_cast(&value), sizeof(value)); + stream.write(reinterpret_cast(&variance), sizeof(variance)); + + const int num_of_sub_nodes = static_cast(sub_nodes.size()); + stream.write(reinterpret_cast(&num_of_sub_nodes), + sizeof(num_of_sub_nodes)); + for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; ++sub_node_index) { + sub_nodes[sub_node_index].serialize(stream); + } + } + + /** Deserializes a node from the specified stream. + * + * \param[in] stream the source for the deserialization + */ + inline void + deserialize(std::istream& stream) + { + feature.deserialize(stream); + + stream.read(reinterpret_cast(&threshold), sizeof(threshold)); + + stream.read(reinterpret_cast(&value), sizeof(value)); + stream.read(reinterpret_cast(&variance), sizeof(variance)); + + int num_of_sub_nodes; + stream.read(reinterpret_cast(&num_of_sub_nodes), sizeof(num_of_sub_nodes)); + sub_nodes.resize(num_of_sub_nodes); + + if (num_of_sub_nodes > 0) { + for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; + ++sub_node_index) { + sub_nodes[sub_node_index].deserialize(stream); + } + } + } + +public: + /** The feature associated with the node. */ + FeatureType feature; + + /** The threshold applied on the feature response. */ + float threshold; + + /** The label value of this node. */ + LabelType value; + + /** The variance of the labels that ended up at this node during training. */ + LabelType variance; + + /** The child nodes. */ + std::vector sub_nodes; +}; + +/** Statistics estimator for regression trees which optimizes variance. */ +template +class PCL_EXPORTS RegressionVarianceStatsEstimator +: public pcl::StatsEstimator { +public: + /** Constructor. */ + RegressionVarianceStatsEstimator(BranchEstimator* branch_estimator) + : branch_estimator_(branch_estimator) + {} + + /** Destructor. */ + virtual ~RegressionVarianceStatsEstimator() {} + + /** Returns the number of branches the corresponding tree has. */ + inline std::size_t + getNumOfBranches() const + { + // return 2; + return branch_estimator_->getNumOfBranches(); + } + + /** Returns the label of the specified node. + * + * \param[in] node the node which label is returned + */ + inline LabelDataType + getLabelOfNode(NodeType& node) const + { + return node.value; + } + + /** Computes the information gain obtained by the specified threshold. + * + * \param[in] data_set the data set corresponding to the supplied result data + * \param[in] examples the examples used for extracting the supplied result data + * \param[in] label_data the label data corresponding to the specified examples + * \param[in] results the results computed using the specified examples + * \param[in] flags the flags corresponding to the results + * \param[in] threshold the threshold for which the information gain is computed + */ + float + computeInformationGain(DataSet& data_set, + std::vector& examples, + std::vector& label_data, + std::vector& results, + std::vector& flags, + const float threshold) const + { + const std::size_t num_of_examples = examples.size(); + const std::size_t num_of_branches = getNumOfBranches(); + + // compute variance + std::vector sums(num_of_branches + 1, 0); + std::vector sqr_sums(num_of_branches + 1, 0); + std::vector branch_element_count(num_of_branches + 1, 0); + + for (std::size_t branch_index = 0; branch_index < num_of_branches; ++branch_index) { + branch_element_count[branch_index] = 1; + ++branch_element_count[num_of_branches]; + } + + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + unsigned char branch_index; + computeBranchIndex( + results[example_index], flags[example_index], threshold, branch_index); + + LabelDataType label = label_data[example_index]; + + sums[branch_index] += label; + sums[num_of_branches] += label; + + sqr_sums[branch_index] += label * label; + sqr_sums[num_of_branches] += label * label; + + ++branch_element_count[branch_index]; + ++branch_element_count[num_of_branches]; + } + + std::vector variances(num_of_branches + 1, 0); + for (std::size_t branch_index = 0; branch_index < num_of_branches + 1; + ++branch_index) { + const float mean_sum = + static_cast(sums[branch_index]) / branch_element_count[branch_index]; + const float mean_sqr_sum = static_cast(sqr_sums[branch_index]) / + branch_element_count[branch_index]; + variances[branch_index] = mean_sqr_sum - mean_sum * mean_sum; + } + + float information_gain = variances[num_of_branches]; + for (std::size_t branch_index = 0; branch_index < num_of_branches; ++branch_index) { + // const float weight = static_cast(sums[branchIndex]) / + // sums[numOfBranches]; + const float weight = static_cast(branch_element_count[branch_index]) / + static_cast(branch_element_count[num_of_branches]); + information_gain -= weight * variances[branch_index]; + } + + return information_gain; + } + + /** Computes the branch indices for all supplied results. + * + * \param[in] results the results the branch indices will be computed for + * \param[in] flags the flags corresponding to the specified results + * \param[in] threshold the threshold used to compute the branch indices + * \param[out] branch_indices the destination for the computed branch indices + */ + void + computeBranchIndices(std::vector& results, + std::vector& flags, + const float threshold, + std::vector& branch_indices) const + { + const std::size_t num_of_results = results.size(); + const std::size_t num_of_branches = getNumOfBranches(); + + branch_indices.resize(num_of_results); + for (std::size_t result_index = 0; result_index < num_of_results; ++result_index) { + unsigned char branch_index; + computeBranchIndex( + results[result_index], flags[result_index], threshold, branch_index); + branch_indices[result_index] = branch_index; + } + } + + /** Computes the branch index for the specified result. + * + * \param[in] result the result the branch index will be computed for + * \param[in] flag the flag corresponding to the specified result + * \param[in] threshold the threshold used to compute the branch index + * \param[out] branch_index the destination for the computed branch index + */ + inline void + computeBranchIndex(const float result, + const unsigned char flag, + const float threshold, + unsigned char& branch_index) const + { + branch_estimator_->computeBranchIndex(result, flag, threshold, branch_index); + // branch_index = (result > threshold) ? 1 : 0; + } + + /** Computes and sets the statistics for a node. + * + * \param[in] data_set the data set which is evaluated + * \param[in] examples the examples which define which parts of the data set are use + * for evaluation + * \param[in] label_data the label_data corresponding to the examples + * \param[out] node the destination node for the statistics + */ + void + computeAndSetNodeStats(DataSet& data_set, + std::vector& examples, + std::vector& label_data, + NodeType& node) const + { + const std::size_t num_of_examples = examples.size(); + + LabelDataType sum = 0.0f; + LabelDataType sqr_sum = 0.0f; + for (std::size_t example_index = 0; example_index < num_of_examples; + ++example_index) { + const LabelDataType label = label_data[example_index]; + + sum += label; + sqr_sum += label * label; + } + + sum /= num_of_examples; + sqr_sum /= num_of_examples; + + const float variance = sqr_sum - sum * sum; + + node.value = sum; + node.variance = variance; + } + + /** Generates code for branch index computation. + * + * \param[in] node the node for which code is generated + * \param[out] stream the destination for the generated code + */ + void + generateCodeForBranchIndexComputation(NodeType& node, std::ostream& stream) const + { + stream << "ERROR: RegressionVarianceStatsEstimator does not implement " + "generateCodeForBranchIndex(...)"; + } + + /** Generates code for label output. + * + * \param[in] node the node for which code is generated + * \param[out] stream the destination for the generated code + */ + void + generateCodeForOutput(NodeType& node, std::ostream& stream) const + { + stream << "ERROR: RegressionVarianceStatsEstimator does not implement " + "generateCodeForBranchIndex(...)"; + } + +private: + /// The branch estimator + pcl::BranchEstimator* branch_estimator_; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/stats_estimator.h b/deps_install/include/pcl-1.10/pcl/ml/stats_estimator.h new file mode 100755 index 0000000..a607816 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/stats_estimator.h @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { + +/** Class interface for gathering statistics for decision tree learning. */ +template +class PCL_EXPORTS StatsEstimator { + +public: + /** Destructor. */ + virtual ~StatsEstimator(){}; + + /** Returns the number of brances a node can have (e.g. a binary tree has 2). */ + virtual std::size_t + getNumOfBranches() const = 0; + + /** Computes and sets the statistics for a node. + * + * \param[in] data_set the data set used for training + * \param[in] examples the examples used for computing the statistics for the + * specified node + * \param[in] label_data the labels corresponding to the examples + * \param[out] node The destination node for the statistics + */ + virtual void + computeAndSetNodeStats(DataSet& data_set, + std::vector& examples, + std::vector& label_data, + NodeType& node) const = 0; + + /** Returns the label of the specified node. + * + * \param[in] node The node from which the label is extracted + */ + virtual LabelDataType + getLabelOfNode(NodeType& node) const = 0; + + /** Computes the information gain obtained by the specified threshold on the supplied + * feature evaluation results. + * + * \param[in] data_set the data set used for extracting the supplied result values. + * \param[in] examples the examples used to extract the supplied result values + * \param[in] label_data the labels corresponding to the examples + * \param[in] results the results obtained from the feature evaluation + * \param[in] flags the flags obtained together with the results + * \param[in] threshold the threshold which is used to compute the information gain + */ + virtual float + computeInformationGain(DataSet& data_set, + std::vector& examples, + std::vector& label_data, + std::vector& results, + std::vector& flags, + const float threshold) const = 0; + + /** Computes the branch indices obtained by the specified threshold on the supplied + * feature evaluation results. + * + * \param[in] results the results obtained from the feature evaluation + * \param[in] flags the flags obtained together with the results. + * \param[in] threshold the threshold which is used to compute the branch indices + * \param[out] branch_indices the destination for the computed branch indices. + */ + virtual void + computeBranchIndices(std::vector& results, + std::vector& flags, + const float threshold, + std::vector& branch_indices) const = 0; + + /** Computes the branch indices obtained by the specified threshold on the supplied + * feature evaluation results. + * + * \param[in] result the result obtained from the feature evaluation + * \param[in] flag the flag obtained together with the result + * \param[in] threshold the threshold which is used to compute the branch index + * \param[out] branch_index the destination for the computed branch index + */ + virtual void + computeBranchIndex(const float result, + const unsigned char flag, + const float threshold, + unsigned char& branch_index) const = 0; + + /** Generates code for computing the branch indices for the specified node and writes + * it to the specified stream. + * + * \param[in] node the node for which the branch index estimation code is generated + * \param[out] stream the destination for the code + */ + virtual void + generateCodeForBranchIndexComputation(NodeType& node, std::ostream& stream) const = 0; + + /** Generates code for computing the output for the specified node and writes it to + * the specified stream. + * + * \param[in] node the node for which the output estimation code is generated + * \param[out] stream the destination for the code + */ + virtual void + generateCodeForOutput(NodeType& node, std::ostream& stream) const = 0; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/ml/svm.h b/deps_install/include/pcl-1.10/pcl/ml/svm.h new file mode 100755 index 0000000..d1d8619 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/svm.h @@ -0,0 +1,184 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of copyright holders nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#define LIBSVM_VERSION 311 + +#ifdef __cplusplus +extern "C" { +#endif + +extern int libsvm_version; + +struct svm_node { + int index; + double value; +}; + +struct svm_problem { + int l; + double* y; + + struct svm_node** x; +}; + +struct svm_scaling { + // index = 1 if usable, index = 0 if not + + struct svm_node* obj; + + // max features scaled + int max; + + svm_scaling() : max(0) {} +}; + +enum { C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR }; /* svm_type */ +enum { LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED }; /* kernel_type */ + +struct svm_parameter { + int svm_type; + int kernel_type; + int degree; /* for poly */ + double gamma; /* for poly/rbf/sigmoid */ + double coef0; /* for poly/sigmoid */ + + /* these are for training only */ + double cache_size; /* in MB */ + double eps; /* stopping criteria */ + double C; /* for C_SVC, EPSILON_SVR and NU_SVR */ + int nr_weight; /* for C_SVC */ + int* weight_label; /* for C_SVC */ + double* weight; /* for C_SVC */ + double nu; /* for NU_SVC, ONE_CLASS, and NU_SVR */ + double p; /* for EPSILON_SVR */ + int shrinking; /* use the shrinking heuristics */ + int probability; /* do probability estimates */ +}; + +// +// svm_model +// + +struct svm_model { + + struct svm_parameter param; /* parameter */ + int nr_class; /* number of classes, = 2 in regression/one class svm */ + int l; /* total #SV */ + + struct svm_node** SV; /* SVs (SV[l]) */ + double** sv_coef; /* coefficients for SVs in decision functions (sv_coef[k-1][l]) */ + double* rho; /* constants in decision functions (rho[k*(k-1)/2]) */ + double* probA; /* pariwise probability information */ + double* probB; + + /* for classification only */ + + int* label; /* label of each class (label[k]) */ + int* nSV; /* number of SVs for each class (nSV[k]) */ + /* nSV[0] + nSV[1] + ... + nSV[k-1] = l */ + /* XXX */ + int free_sv; /* 1 if svm_model is created by svm_load_model*/ + /* 0 if svm_model is created by svm_train */ + + /* for scaling */ + + struct svm_node* scaling; +}; + +struct svm_model* +svm_train(const struct svm_problem* prob, const struct svm_parameter* param); +void +svm_cross_validation(const struct svm_problem* prob, + const struct svm_parameter* param, + int nr_fold, + double* target); + +int +svm_save_model(const char* model_file_name, const struct svm_model* model); + +struct svm_model* +svm_load_model(const char* model_file_name); + +int +svm_get_svm_type(const struct svm_model* model); + +int +svm_get_nr_class(const struct svm_model* model); + +void +svm_get_labels(const struct svm_model* model, int* label); + +double +svm_get_svr_probability(const struct svm_model* model); + +double +svm_predict_values(const struct svm_model* model, + const struct svm_node* x, + double* dec_values); +double +svm_predict(const struct svm_model* model, const struct svm_node* x); + +double +svm_predict_probability(const struct svm_model* model, + const struct svm_node* x, + double* prob_estimates); + +void +svm_free_model_content(struct svm_model* model_ptr); + +void +svm_free_and_destroy_model(struct svm_model** model_ptr_ptr); + +void +svm_destroy_param(struct svm_parameter* param); + +const char* +svm_check_parameter(const struct svm_problem* prob, const struct svm_parameter* param); + +int +svm_check_probability_model(const struct svm_model* model); + +void +svm_set_print_string_function(void (*print_func)(const char*)); + +#ifdef __cplusplus +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/ml/svm_wrapper.h b/deps_install/include/pcl-1.10/pcl/ml/svm_wrapper.h new file mode 100755 index 0000000..0e7a63b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/ml/svm_wrapper.h @@ -0,0 +1,568 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2000-2012 Chih-Chung Chang and Chih-Jen Lin + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of copyright holders nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#define Malloc(type, n) static_cast(malloc((n) * sizeof(type))) + +namespace pcl { + +/** The structure stores the parameters for the classificationa nd must be initialized + * and passed to the training method pcl::SVMTrain. + * + * \param svm_type {C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR} + * \param kernel_type {LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED} + * \param probability sets the probability estimates + */ +struct SVMParam : svm_parameter { + SVMParam() + { + svm_type = C_SVC; // C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR + kernel_type = RBF; // LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED + degree = 3; // for poly + gamma = 0; // 1/num_features {for poly/rbf/sigmoid} + coef0 = 0; // for poly/sigmoid + + nu = 0.5; // for NU_SVC, ONE_CLASS, and NU_SVR + cache_size = 100; // in MB + C = 1; // for C_SVC, EPSILON_SVR and NU_SVR + eps = 1e-3; // stopping criteria + p = 0.1; // for EPSILON_SVR + shrinking = 0; // use the shrinking heuristics + probability = 0; // do probability estimates + + nr_weight = 0; // for C_SVC + weight_label = nullptr; // for C_SVC + weight = nullptr; // for C_SVC + } +}; + +/** The structure initialize a model created by the SVM (Support Vector Machines) + * classifier (pcl::SVMTrain). + */ +struct SVMModel : svm_model { + SVMModel() + { + l = 0; + probA = nullptr; + probB = nullptr; + } +}; + +/** The structure initialize a single feature value for the classification using + * SVM (Support Vector Machines). + */ +struct SVMDataPoint { + /// It's the feature index. It has to be an integer number greater or equal to zero + int idx; + /// The value assigned to the correspondent feature. + float value; + + SVMDataPoint() : idx(-1), value(0) {} +}; + +/** The structure stores the features and the label of a single sample which has to be + * used for the training or the classification of the SVM (Support Vector Machines). + */ +struct SVMData { + /// Pointer to the label value. It is a mandatory to train the classifier + double label; + /// Vector of features for the specific sample. + std::vector SV; + + SVMData() : label(std::numeric_limits::signaling_NaN()) {} +}; + +/** Base class for SVM SVM (Support Vector Machines). */ +class SVM { +protected: + std::vector training_set_; // Basic training set + svm_problem prob_; // contains the problem (vector of samples with their features) + SVMModel model_; // model of the classifier + svm_scaling scaling_; // for the best model training, the input dataset is scaled and + // the scaling factors are stored here + SVMParam param_; // it stores the training parameters + std::string class_name_; // The SVM class name. + + char* line_; // buffer for line reading + int max_line_len_; // max line length in the input file + bool labelled_training_set_; // it stores whether the input set of samples is labelled + + /** Set for output printings during classification. */ + static void + printNull(const char*){}; + + /** To read a line from the input file. Stored in "line_". */ + char* + readline(FILE* input); + + /** Outputs an error in file reading. */ + void + exitInputError(int line_num) + { + fprintf(stderr, "Wrong input format at line %d\n", line_num); + exit(1); + } + + /** Get a string representation of the name of this class. */ + inline const std::string& + getClassName() const + { + return (class_name_); + } + + /** Convert the input format (vector of SVMData) into a readable format for libSVM. */ + void + adaptInputToLibSVM(std::vector training_set, svm_problem& prob); + + /** Convert the libSVM format (svm_problem) into a easier output format. */ + void + adaptLibSVMToInput(std::vector& training_set, svm_problem prob); + + /** Load a problem from an extern file. */ + bool + loadProblem(const char* filename, svm_problem& prob); + + /** Save the raw problem in an extern file.*/ + bool + saveProblem(const char* filename, bool labelled); + + /** Save the problem (with normalized values) in an extern file.*/ + bool + saveProblemNorm(const char* filename, svm_problem prob_, bool labelled); + +public: + /** Constructor. */ + SVM() : prob_(), line_(nullptr), max_line_len_(10000), labelled_training_set_(true) {} + + /** Destructor. */ + ~SVM() + { + svm_destroy_param(¶m_); // delete parameters + + if (scaling_.max > 0) + free(scaling_.obj); // delete scaling factors + + // delete the problem + if (prob_.l > 0) { + free(prob_.x); + free(prob_.y); + } + } + + /** Return the labels order from the classifier model. */ + void + getLabel(std::vector& labels) + { + int nr_class = svm_get_nr_class(&model_); + int* labels_ = static_cast(malloc(nr_class * sizeof(int))); + svm_get_labels(&model_, labels_); + + for (int j = 0; j < nr_class; j++) + labels.push_back(labels_[j]); + + free(labels_); + }; + + /** Save the classifier model in an extern file (in svmlight format). */ + void + saveClassifierModel(const char* filename) + { + // exit if model has no data + if (model_.l == 0) + return; + + if (svm_save_model(filename, &model_)) { + fprintf(stderr, "can't save model to file %s\n", filename); + exit(1); + } + }; +}; + +/** SVM (Support Vector Machines) training class for the SVM machine learning. + * + * It creates a model for the classifier from a labelled input dataset. + * + * OPTIONAL: pcl::SVMParam has to be given as input to vary the default training method + * and parameters. + */ +class SVMTrain : public SVM { +protected: + using SVM::class_name_; + using SVM::labelled_training_set_; + using SVM::line_; + using SVM::max_line_len_; + using SVM::model_; + using SVM::param_; + using SVM::prob_; + using SVM::scaling_; + using SVM::training_set_; + + /// Set to 1 to see the training output + bool debug_; + /// Set too 1 for cross validating the classifier + int cross_validation_; + /// Number of folds to be used during cross validation. It indicates in how many parts + /// is split the input training set. + int nr_fold_; + + /** To cross validate the classifier. It is automatic for probability estimate. */ + void + doCrossValidation(); + + /** It extracts scaling factors from the input training_set. + * + * The scaling of the training_set is a mandatory for a good training of the + * classifier. */ + void + scaleFactors(std::vector training_set, svm_scaling& scaling); + +public: + /** Constructor. */ + SVMTrain() : debug_(false), cross_validation_(0), nr_fold_(0) + { + class_name_ = "SVMTrain"; + svm_set_print_string_function( + &printNull); // Default to NULL to not print debugging info + } + + /** Destructor. */ + ~SVMTrain() + { + if (model_.l > 0) + svm_free_model_content(&model_); + } + + /** Change default training parameters (pcl::SVMParam). */ + void + setParameters(SVMParam param) + { + param_ = param; + } + + /** Return the current training parameters. */ + SVMParam + getParameters() + { + return param_; + } + + /** Return the result of the training. */ + SVMModel + getClassifierModel() + { + return model_; + } + + /** It adds/store the training set with labelled data. */ + void + setInputTrainingSet(std::vector training_set) + { + training_set_.insert(training_set_.end(), training_set.begin(), training_set.end()); + } + + /** Return the current training set. */ + std::vector + getInputTrainingSet() + { + return training_set_; + } + + /** Reset the training set. */ + void + resetTrainingSet() + { + training_set_.clear(); + } + + /** Start the training of the SVM classifier. + * + * \return false if fails + */ + bool + trainClassifier(); + + /** Read in a problem (in svmlight format). + * + * \return false if fails + */ + bool + loadProblem(const char* filename) + { + return SVM::loadProblem(filename, prob_); + }; + + /** Set to 1 for debugging info. */ + void + setDebugMode(bool in) + { + debug_ = in; + + if (in) + svm_set_print_string_function(nullptr); + else + svm_set_print_string_function(&printNull); + }; + + /** Save the raw training set in a file (in svmlight format). + * + * \return false if fails + */ + bool + saveTrainingSet(const char* filename) + { + return SVM::saveProblem(filename, true); + }; + + /** Save the normalized training set in a file (in svmlight format). + * + * \return false if fails + */ + bool + saveNormTrainingSet(const char* filename) + { + return SVM::saveProblemNorm(filename, prob_, true); + }; +}; + +/** SVM (Support Vector Machines) classification of a dataset. + * + * It can be used both for testing a classifier model and for classify of new data. + */ +class SVMClassify : public SVM { +protected: + using SVM::class_name_; + using SVM::labelled_training_set_; + using SVM::line_; + using SVM::max_line_len_; + using SVM::model_; + using SVM::param_; + using SVM::prob_; + using SVM::scaling_; + using SVM::training_set_; + + bool model_extern_copied_; // Set to 0 if the model is loaded from an extern file. + bool predict_probability_; // Set to 1 to predict probabilities. + std::vector> prediction_; // It stores the resulting prediction. + + /** It scales the input dataset using the model information. */ + void + scaleProblem(svm_problem& input, svm_scaling scaling); + +public: + /** Constructor. */ + SVMClassify() : model_extern_copied_(false), predict_probability_(false) + { + class_name_ = "SvmClassify"; + } + + /** Destructor. */ + ~SVMClassify() + { + if (!model_extern_copied_ && model_.l > 0) + svm_free_model_content(&model_); + } + + /** It adds/store the training set with labelled data. */ + void + setInputTrainingSet(std::vector training_set) + { + assert(training_set.size() > 0); + + if (scaling_.max == 0) { + // to be sure to have loaded the scaling + PCL_ERROR("[pcl::%s::setInputTrainingSet] Classifier model not loaded!\n", + getClassName().c_str()); + return; + } + + training_set_.insert(training_set_.end(), training_set.begin(), training_set.end()); + SVM::adaptInputToLibSVM(training_set_, prob_); + } + + /** Return the current training set. */ + std::vector + getInputTrainingSet() + { + return training_set_; + } + + /** Reset the training set. */ + void + resetTrainingSet() + { + training_set_.clear(); + } + + /** Read in a classifier model (in svmlight format). + * + * \return false if fails + */ + bool + loadClassifierModel(const char* filename); + + /** Get the result of the classification. */ + void + getClassificationResult(std::vector>& out) + { + out.clear(); + out.insert(out.begin(), prediction_.begin(), prediction_.end()); + } + + /** Save the classification result in an extern file. */ + void + saveClassificationResult(const char* filename); + + /** Set the classifier model. */ + void + setClassifierModel(SVMModel model) + { + // model (inner pointers are references) + model_ = model; + int i = 0; + + while (model_.scaling[i].index != -1) + i++; + + scaling_.max = i; + scaling_.obj = Malloc(struct svm_node, i + 1); + scaling_.obj[i].index = -1; + + // Performing full scaling copy + for (int j = 0; j < i; j++) { + scaling_.obj[j] = model_.scaling[j]; + } + + model_extern_copied_ = true; + }; + + /** Read in a raw classification problem (in svmlight format). + * + * The values are normalized using the classifier model information. + * + * \return false if fails + */ + bool + loadClassProblem(const char* filename) + { + assert(model_.l != 0); + + bool out = SVM::loadProblem(filename, prob_); + SVM::adaptLibSVMToInput(training_set_, prob_); + scaleProblem(prob_, scaling_); + return out; + }; + + /** Read in a normalized classification problem (in svmlight format). + * + * The data are kept whitout normalizing. + * + * \return false if fails + */ + bool + loadNormClassProblem(const char* filename) + { + bool out = SVM::loadProblem(filename, prob_); + SVM::adaptLibSVMToInput(training_set_, prob_); + return out; + }; + + /** Set whether the classification has to be done with the probability estimate. (The + * classifier model has to support it). */ + void + setProbabilityEstimates(bool set) + { + predict_probability_ = set; + }; + + /** Start the classification on labelled input dataset. + * + * It returns the accuracy percentage. To get the classification result, use + * getClassificationResult(). + * + * \return false if fails + */ + bool + classificationTest(); + + /** Start the classification on un-labelled input dataset. + * + * To get the classification result, use getClassificationResult(). + * + * \return false if fails + */ + bool + classification(); + + /** Start the classification on a single set. */ + std::vector + classification(SVMData in); + + /** Save the raw classification problem in a file (in svmlight format). + * + * \return false if fails + */ + bool + saveClassProblem(const char* filename) + { + return SVM::saveProblem(filename, false); + }; + + /** Save the normalized classification problem in a file (in svmlight format). + * + * \return false if fails + */ + bool + saveNormClassProblem(const char* filename) + { + return SVM::saveProblemNorm(filename, prob_, false); + }; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/octree/boost.h b/deps_install/include/pcl-1.10/pcl/octree/boost.h new file mode 100755 index 0000000..3510b23 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/boost.h @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +// Marking all Boost headers as system headers to remove warnings +#include diff --git a/deps_install/include/pcl-1.10/pcl/octree/impl/octree2buf_base.hpp b/deps_install/include/pcl-1.10/pcl/octree/impl/octree2buf_base.hpp new file mode 100755 index 0000000..edaea81 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/impl/octree2buf_base.hpp @@ -0,0 +1,837 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_OCTREE_2BUF_BASE_HPP +#define PCL_OCTREE_2BUF_BASE_HPP + +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////// +template +Octree2BufBase::Octree2BufBase() +: leaf_count_(0) +, branch_count_(1) +, root_node_(new BranchNode()) +, depth_mask_(0) +, buffer_selector_(0) +, tree_dirty_flag_(false) +, octree_depth_(0) +, dynamic_depth_enabled_(false) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +Octree2BufBase::~Octree2BufBase() +{ + // deallocate tree structure + deleteTree(); + delete (root_node_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::setMaxVoxelIndex( + unsigned int max_voxel_index_arg) +{ + unsigned int treeDepth; + + assert(max_voxel_index_arg > 0); + + // tree depth == amount of bits of maxVoxels + treeDepth = std::max( + (std::min(static_cast(OctreeKey::maxDepth), + static_cast(std::ceil(std::log2(max_voxel_index_arg))))), + static_cast(0)); + + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (treeDepth - 1)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::setTreeDepth(unsigned int depth_arg) +{ + assert(depth_arg > 0); + + // set octree depth + octree_depth_ = depth_arg; + + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (depth_arg - 1)); + + // define max. keys + max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +LeafContainerT* +Octree2BufBase::findLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // check if key exist in octree + return (findLeaf(key)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +LeafContainerT* +Octree2BufBase::createLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // check if key exist in octree + return (createLeaf(key)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +Octree2BufBase::existLeaf( + unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // check if key exist in octree + return existLeaf(key); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::removeLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // free voxel at key + return (this->removeLeaf(key)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deleteTree() +{ + if (root_node_) { + // reset octree + deleteBranch(*root_node_); + leaf_count_ = 0; + branch_count_ = 1; + + tree_dirty_flag_ = false; + depth_mask_ = 0; + octree_depth_ = 0; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::switchBuffers() +{ + if (tree_dirty_flag_) { + // make sure that all unused branch nodes from previous buffer are deleted + treeCleanUpRecursive(root_node_); + } + + // switch butter selector + buffer_selector_ = !buffer_selector_; + + // reset flags + tree_dirty_flag_ = true; + leaf_count_ = 0; + branch_count_ = 1; + + // we can safely remove children references of root node + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + root_node_->setChildPtr(buffer_selector_, child_idx, nullptr); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeTree( + std::vector& binary_tree_out_arg, bool do_XOR_encoding_arg) +{ + OctreeKey new_key; + + // clear binary vector + binary_tree_out_arg.clear(); + binary_tree_out_arg.reserve(this->branch_count_); + + serializeTreeRecursive( + root_node_, new_key, &binary_tree_out_arg, nullptr, do_XOR_encoding_arg, false); + + // serializeTreeRecursive cleans-up unused octree nodes in previous octree + tree_dirty_flag_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeTree( + std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_encoding_arg) +{ + OctreeKey new_key; + + // clear output vectors + binary_tree_out_arg.clear(); + leaf_container_vector_arg.clear(); + + leaf_container_vector_arg.reserve(leaf_count_); + binary_tree_out_arg.reserve(this->branch_count_); + + serializeTreeRecursive(root_node_, + new_key, + &binary_tree_out_arg, + &leaf_container_vector_arg, + do_XOR_encoding_arg, + false); + + // serializeTreeRecursive cleans-up unused octree nodes in previous octree + tree_dirty_flag_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeLeafs( + std::vector& leaf_container_vector_arg) +{ + OctreeKey new_key; + + // clear output vector + leaf_container_vector_arg.clear(); + + leaf_container_vector_arg.reserve(leaf_count_); + + serializeTreeRecursive( + root_node_, new_key, nullptr, &leaf_container_vector_arg, false, false); + + // serializeLeafsRecursive cleans-up unused octree nodes in previous octree + tree_dirty_flag_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deserializeTree( + std::vector& binary_tree_in_arg, bool do_XOR_decoding_arg) +{ + OctreeKey new_key; + + // we will rebuild an octree -> reset leafCount + leaf_count_ = 0; + + // iterator for binary tree structure vector + std::vector::const_iterator binary_tree_in_it = binary_tree_in_arg.begin(); + std::vector::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end(); + + deserializeTreeRecursive(root_node_, + depth_mask_, + new_key, + binary_tree_in_it, + binary_tree_in_it_end, + nullptr, + nullptr, + false, + do_XOR_decoding_arg); + + // we modified the octree structure -> clean-up/tree-reset might be required + tree_dirty_flag_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deserializeTree( + std::vector& binary_tree_in_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_decoding_arg) +{ + OctreeKey new_key; + + // set data iterator to first element + typename std::vector::const_iterator leaf_container_vector_it = + leaf_container_vector_arg.begin(); + + // set data iterator to last element + typename std::vector::const_iterator leaf_container_vector_it_end = + leaf_container_vector_arg.end(); + + // we will rebuild an octree -> reset leafCount + leaf_count_ = 0; + + // iterator for binary tree structure vector + std::vector::const_iterator binary_tree_in_it = binary_tree_in_arg.begin(); + std::vector::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end(); + + deserializeTreeRecursive(root_node_, + depth_mask_, + new_key, + binary_tree_in_it, + binary_tree_in_it_end, + &leaf_container_vector_it, + &leaf_container_vector_it_end, + false, + do_XOR_decoding_arg); + + // we modified the octree structure -> clean-up/tree-reset might be required + tree_dirty_flag_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeNewLeafs( + std::vector& leaf_container_vector_arg) +{ + OctreeKey new_key; + + // clear output vector + leaf_container_vector_arg.clear(); + leaf_container_vector_arg.reserve(leaf_count_); + + serializeTreeRecursive( + root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true); + + // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer + tree_dirty_flag_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +unsigned int +Octree2BufBase::createLeafRecursive( + const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg, + bool branch_reset_arg) +{ + // branch reset -> this branch has been taken from previous buffer + if (branch_reset_arg) { + // we can safely remove children references + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); + } + } + + // find branch child from key + unsigned char child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* child_branch; + bool doNodeReset; + + doNodeReset = false; + + // if required branch does not exist + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { + // check if we find a branch node reference in previous buffer + + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx); + + if (child_node->getNodeType() == BRANCH_NODE) { + child_branch = static_cast(child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); + } + else { + // depth has changed.. child in preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_branch = createBranchChild(*branch_arg, child_idx); + } + + // take child branch from previous buffer + doNodeReset = true; // reset the branch pointer array of stolen child node + } + else { + // if required branch does not exist -> create it + child_branch = createBranchChild(*branch_arg, child_idx); + } + + branch_count_++; + } + // required branch node already exists - use it + else + child_branch = static_cast( + branch_arg->getChildPtr(buffer_selector_, child_idx)); + + // recursively proceed with indexed child branch + return createLeafRecursive(key_arg, + depth_mask_arg / 2, + child_branch, + return_leaf_arg, + parent_of_leaf_arg, + doNodeReset); + } + + // branch childs are leaf nodes + LeafNode* child_leaf; + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { + // leaf node at child_idx does not exist + + // check if we can take copy a reference from previous buffer + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + + OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx); + if (child_node->getNodeType() == LEAF_NODE) { + child_leaf = static_cast(child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); + } + else { + // depth has changed.. child in preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_leaf = createLeafChild(*branch_arg, child_idx); + } + leaf_count_++; + } + else { + // if required leaf does not exist -> create it + child_leaf = createLeafChild(*branch_arg, child_idx); + leaf_count_++; + } + + // return leaf node + return_leaf_arg = child_leaf; + parent_of_leaf_arg = branch_arg; + } + else { + // leaf node already exist + return_leaf_arg = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); + parent_of_leaf_arg = branch_arg; + } + + return depth_mask_arg; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::findLeafRecursive( + const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const +{ + // return leaf node + unsigned char child_idx; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* child_branch; + child_branch = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); + + if (child_branch) + // recursively proceed with indexed child branch + findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg); + } + else { + // we reached leaf node level + if (branch_arg->hasChild(buffer_selector_, child_idx)) { + // return existing leaf node + LeafNode* leaf_node = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); + result_arg = leaf_node->getContainerPtr(); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +Octree2BufBase::deleteLeafRecursive( + const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg) +{ + // index to branch child + unsigned char child_idx; + // indicates if branch is empty and can be safely removed + bool bNoChilds; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* child_branch; + + // next branch child on our path through the tree + child_branch = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); + + if (child_branch) { + // recursively explore the indexed child branch + bool bBranchOccupied = + deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch); + + if (!bBranchOccupied) { + // child branch does not own any sub-child nodes anymore -> delete child branch + deleteBranchChild(*branch_arg, buffer_selector_, child_idx); + branch_count_--; + } + } + } + else { + // our child is a leaf node -> delete it + deleteBranchChild(*branch_arg, buffer_selector_, child_idx); + leaf_count_--; + } + + // check if current branch still owns childs + bNoChilds = false; + for (child_idx = 0; child_idx < 8; child_idx++) { + bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx); + if (bNoChilds) + break; + } + + // return true if current branch can be deleted + return (bNoChilds); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeTreeRecursive( + BranchNode* branch_arg, + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg, + bool do_XOR_encoding_arg, + bool new_leafs_filter_arg) +{ + // bit pattern + char branch_bit_pattern_curr_buffer; + char branch_bit_pattern_prev_buffer; + char node_XOR_bit_pattern; + + // occupancy bit patterns of branch node (current and previous octree buffer) + branch_bit_pattern_curr_buffer = getBranchBitPattern(*branch_arg, buffer_selector_); + branch_bit_pattern_prev_buffer = getBranchBitPattern(*branch_arg, !buffer_selector_); + + // XOR of current and previous occupancy bit patterns + node_XOR_bit_pattern = + branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer; + + if (binary_tree_out_arg) { + if (do_XOR_encoding_arg) { + // write XOR bit pattern to output vector + binary_tree_out_arg->push_back(node_XOR_bit_pattern); + } + else { + // write bit pattern of current buffer to output vector + binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer); + } + } + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (branch_arg->hasChild(buffer_selector_, child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); + + OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx); + + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + serializeTreeRecursive(static_cast(child_node), + key_arg, + binary_tree_out_arg, + leaf_container_vector_arg, + do_XOR_encoding_arg, + new_leafs_filter_arg); + break; + } + case LEAF_NODE: { + LeafNode* child_leaf = static_cast(child_node); + + if (new_leafs_filter_arg) { + if (!branch_arg->hasChild(!buffer_selector_, child_idx)) { + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); + + serializeTreeCallback(**child_leaf, key_arg); + } + } + else { + + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); + + serializeTreeCallback(**child_leaf, key_arg); + } + + break; + } + default: + break; + } + + // pop current branch voxel from key + key_arg.popBranch(); + } + else if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + // delete branch, free memory + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deserializeTreeRecursive( + BranchNode* branch_arg, + unsigned int depth_mask_arg, + OctreeKey& key_arg, + typename std::vector::const_iterator& binaryTreeIT_arg, + typename std::vector::const_iterator& binaryTreeIT_End_arg, + typename std::vector::const_iterator* dataVectorIterator_arg, + typename std::vector::const_iterator* dataVectorEndIterator_arg, + bool branch_reset_arg, + bool do_XOR_decoding_arg) +{ + + // branch reset -> this branch has been taken from previous buffer + if (branch_reset_arg) { + // we can safely remove children references + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); + } + } + + if (binaryTreeIT_arg != binaryTreeIT_End_arg) { + // read branch occupancy bit pattern from vector + char nodeBits = *binaryTreeIT_arg++; + + // recover branch occupancy bit pattern + char recoveredNodeBits; + if (do_XOR_decoding_arg) { + recoveredNodeBits = + getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits; + } + else { + recoveredNodeBits = nodeBits; + } + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + // if occupancy bit for child_idx is set.. + if (recoveredNodeBits & (1 << child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); + + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + + bool doNodeReset = false; + + BranchNode* child_branch; + + // check if we find a branch node reference in previous buffer + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { + + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + OctreeNode* child_node = + branch_arg->getChildPtr(!buffer_selector_, child_idx); + + if (child_node->getNodeType() == BRANCH_NODE) { + child_branch = static_cast(child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); + } + else { + // depth has changed.. child in preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_branch = createBranchChild(*branch_arg, child_idx); + } + + // take child branch from previous buffer + doNodeReset = true; // reset the branch pointer array of stolen child node + } + else { + // if required branch does not exist -> create it + child_branch = createBranchChild(*branch_arg, child_idx); + } + + branch_count_++; + } + else { + // required branch node already exists - use it + child_branch = static_cast( + branch_arg->getChildPtr(buffer_selector_, child_idx)); + } + + // recursively proceed with indexed child branch + deserializeTreeRecursive(child_branch, + depth_mask_arg / 2, + key_arg, + binaryTreeIT_arg, + binaryTreeIT_End_arg, + dataVectorIterator_arg, + dataVectorEndIterator_arg, + doNodeReset, + do_XOR_decoding_arg); + } + else { + // branch childs are leaf nodes + LeafNode* child_leaf; + + // check if we can take copy a reference pointer from previous buffer + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + // take child leaf node from previous buffer + OctreeNode* child_node = + branch_arg->getChildPtr(!buffer_selector_, child_idx); + if (child_node->getNodeType() == LEAF_NODE) { + child_leaf = static_cast(child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); + } + else { + // depth has changed.. child in preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_leaf = createLeafChild(*branch_arg, child_idx); + } + } + else { + // if required leaf does not exist -> create it + child_leaf = createLeafChild(*branch_arg, child_idx); + } + + // we reached leaf node level + + if (dataVectorIterator_arg && + (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) { + LeafContainerT& container = **child_leaf; + container = ***dataVectorIterator_arg; + ++*dataVectorIterator_arg; + } + + leaf_count_++; + + // execute deserialization callback + deserializeTreeCallback(**child_leaf, key_arg); + } + + // pop current branch voxel from key + key_arg.popBranch(); + } + else if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + // remove old branch pointer information in current branch + branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); + + // remove unused branches in previous buffer + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::treeCleanUpRecursive( + BranchNode* branch_arg) +{ + // occupancy bit pattern of branch node (previous octree buffer) + char occupied_children_bit_pattern_prev_buffer = + getBranchBitPattern(*branch_arg, !buffer_selector_); + + // XOR of current and previous occupancy bit patterns + char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg); + + // bit pattern indicating unused octree nodes in previous branch + char unused_branches_bit_pattern = + node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer; + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (branch_arg->hasChild(buffer_selector_, child_idx)) { + OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx); + + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + treeCleanUpRecursive(static_cast(child_node)); + break; + } + case LEAF_NODE: + // leaf level - nothing to do.. + break; + default: + break; + } + } + + // check for unused branches in previous buffer + if (unused_branches_bit_pattern & (1 << child_idx)) { + // delete branch, free memory + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + } + } +} +} // namespace octree +} // namespace pcl + +#define PCL_INSTANTIATE_Octree2BufBase(T) \ + template class PCL_EXPORTS pcl::octree::Octree2BufBase; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/impl/octree_base.hpp b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_base.hpp new file mode 100755 index 0000000..d7521bc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_base.hpp @@ -0,0 +1,563 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_OCTREE_BASE_HPP +#define PCL_OCTREE_BASE_HPP + +#include + +#include + +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBase::OctreeBase() +: leaf_count_(0) +, branch_count_(1) +, root_node_(new BranchNode()) +, depth_mask_(0) +, octree_depth_(0) +, dynamic_depth_enabled_(false) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBase::~OctreeBase() +{ + // deallocate tree structure + deleteTree(); + delete (root_node_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::setMaxVoxelIndex( + unsigned int max_voxel_index_arg) +{ + unsigned int tree_depth; + + assert(max_voxel_index_arg > 0); + + // tree depth == bitlength of maxVoxels + tree_depth = + std::min(static_cast(OctreeKey::maxDepth), + static_cast(std::ceil(std::log2(max_voxel_index_arg)))); + + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (tree_depth - 1)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::setTreeDepth(unsigned int depth_arg) +{ + assert(depth_arg > 0); + + // set octree depth + octree_depth_ = depth_arg; + + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (depth_arg - 1)); + + // define max. keys + max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +LeafContainerT* +OctreeBase::findLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // check if key exist in octree + return (findLeaf(key)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +LeafContainerT* +OctreeBase::createLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // check if key exist in octree + return (createLeaf(key)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +OctreeBase::existLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) const +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // check if key exist in octree + return (existLeaf(key)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::removeLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) +{ + // generate key + OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg); + + // check if key exist in octree + deleteLeafRecursive(key, depth_mask_, root_node_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::deleteTree() +{ + + if (root_node_) { + // reset octree + deleteBranch(*root_node_); + leaf_count_ = 0; + branch_count_ = 1; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::serializeTree( + std::vector& binary_tree_out_arg) +{ + + OctreeKey new_key; + + // clear binary vector + binary_tree_out_arg.clear(); + binary_tree_out_arg.reserve(this->branch_count_); + + serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg, nullptr); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::serializeTree( + std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg) +{ + + OctreeKey new_key; + + // clear output vectors + binary_tree_out_arg.clear(); + leaf_container_vector_arg.clear(); + + binary_tree_out_arg.reserve(this->branch_count_); + leaf_container_vector_arg.reserve(this->leaf_count_); + + serializeTreeRecursive( + root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::serializeLeafs( + std::vector& leaf_container_vector_arg) +{ + OctreeKey new_key; + + // clear output vector + leaf_container_vector_arg.clear(); + + leaf_container_vector_arg.reserve(this->leaf_count_); + + serializeTreeRecursive(root_node_, new_key, nullptr, &leaf_container_vector_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::deserializeTree( + std::vector& binary_tree_out_arg) +{ + OctreeKey new_key; + + // free existing tree before tree rebuild + deleteTree(); + + // iterator for binary tree structure vector + std::vector::const_iterator binary_tree_out_it = binary_tree_out_arg.begin(); + std::vector::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end(); + + deserializeTreeRecursive(root_node_, + depth_mask_, + new_key, + binary_tree_out_it, + binary_tree_out_it_end, + nullptr, + nullptr); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::deserializeTree( + std::vector& binary_tree_in_arg, + std::vector& leaf_container_vector_arg) +{ + OctreeKey new_key; + + // set data iterator to first element + typename std::vector::const_iterator leaf_vector_it = + leaf_container_vector_arg.begin(); + + // set data iterator to last element + typename std::vector::const_iterator leaf_vector_it_end = + leaf_container_vector_arg.end(); + + // free existing tree before tree rebuild + deleteTree(); + + // iterator for binary tree structure vector + std::vector::const_iterator binary_tree_input_it = binary_tree_in_arg.begin(); + std::vector::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end(); + + deserializeTreeRecursive(root_node_, + depth_mask_, + new_key, + binary_tree_input_it, + binary_tree_input_it_end, + &leaf_vector_it, + &leaf_vector_it_end); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +unsigned int +OctreeBase::createLeafRecursive( + const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg) +{ + // index to branch child + unsigned char child_idx; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + OctreeNode* child_node = (*branch_arg)[child_idx]; + + if (!child_node) { + if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) { + // if required branch does not exist -> create it + BranchNode* childBranch = createBranchChild(*branch_arg, child_idx); + + branch_count_++; + + // recursively proceed with indexed child branch + return createLeafRecursive(key_arg, + depth_mask_arg / 2, + childBranch, + return_leaf_arg, + parent_of_leaf_arg); + } + // if leaf node at child_idx does not exist + LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx); + return_leaf_arg = leaf_node; + parent_of_leaf_arg = branch_arg; + this->leaf_count_++; + } + else { + // Node exists already + switch (child_node->getNodeType()) { + case BRANCH_NODE: + // recursively proceed with indexed child branch + return createLeafRecursive(key_arg, + depth_mask_arg / 2, + static_cast(child_node), + return_leaf_arg, + parent_of_leaf_arg); + break; + + case LEAF_NODE: + return_leaf_arg = static_cast(child_node); + parent_of_leaf_arg = branch_arg; + break; + } + } + + return (depth_mask_arg >> 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::findLeafRecursive( + const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const +{ + // index to branch child + unsigned char child_idx; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + OctreeNode* child_node = (*branch_arg)[child_idx]; + + if (child_node) { + switch (child_node->getNodeType()) { + case BRANCH_NODE: + // we have not reached maximum tree depth + BranchNode* child_branch; + child_branch = static_cast(child_node); + + findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg); + break; + + case LEAF_NODE: + // return existing leaf node + LeafNode* child_leaf; + child_leaf = static_cast(child_node); + + result_arg = child_leaf->getContainerPtr(); + break; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +OctreeBase::deleteLeafRecursive( + const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg) +{ + // index to branch child + unsigned char child_idx; + // indicates if branch is empty and can be safely removed + bool b_no_children; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + OctreeNode* child_node = (*branch_arg)[child_idx]; + + if (child_node) { + switch (child_node->getNodeType()) { + + case BRANCH_NODE: + BranchNode* child_branch; + child_branch = static_cast(child_node); + + // recursively explore the indexed child branch + b_no_children = deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch); + + if (!b_no_children) { + // child branch does not own any sub-child nodes anymore -> delete child branch + deleteBranchChild(*branch_arg, child_idx); + branch_count_--; + } + break; + + case LEAF_NODE: + // return existing leaf node + + // our child is a leaf node -> delete it + deleteBranchChild(*branch_arg, child_idx); + this->leaf_count_--; + break; + } + } + + // check if current branch still owns children + b_no_children = false; + for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++) { + b_no_children = branch_arg->hasChild(child_idx); + } + // return true if current branch can be deleted + return (b_no_children); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::serializeTreeRecursive( + const BranchNode* branch_arg, + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg) const +{ + char node_bit_pattern; + + // branch occupancy bit pattern + node_bit_pattern = getBranchBitPattern(*branch_arg); + + // write bit pattern to output vector + if (binary_tree_out_arg) + binary_tree_out_arg->push_back(node_bit_pattern); + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + + // if child exist + if (branch_arg->hasChild(child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); + + OctreeNode* childNode = branch_arg->getChildPtr(child_idx); + + switch (childNode->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + serializeTreeRecursive(static_cast(childNode), + key_arg, + binary_tree_out_arg, + leaf_container_vector_arg); + break; + } + case LEAF_NODE: { + LeafNode* child_leaf = static_cast(childNode); + + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); + + // we reached a leaf node -> execute serialization callback + serializeTreeCallback(**child_leaf, key_arg); + break; + } + default: + break; + } + + // pop current branch voxel from key + key_arg.popBranch(); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::deserializeTreeRecursive( + BranchNode* branch_arg, + unsigned int depth_mask_arg, + OctreeKey& key_arg, + typename std::vector::const_iterator& binary_tree_input_it_arg, + typename std::vector::const_iterator& binary_tree_input_it_end_arg, + typename std::vector::const_iterator* leaf_container_vector_it_arg, + typename std::vector::const_iterator* + leaf_container_vector_it_end_arg) +{ + + if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) { + // read branch occupancy bit pattern from input vector + char node_bits = (*binary_tree_input_it_arg); + binary_tree_input_it_arg++; + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + // if occupancy bit for child_idx is set.. + if (node_bits & (1 << child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); + + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* newBranch = createBranchChild(*branch_arg, child_idx); + + branch_count_++; + + // recursively proceed with new child branch + deserializeTreeRecursive(newBranch, + depth_mask_arg / 2, + key_arg, + binary_tree_input_it_arg, + binary_tree_input_it_end_arg, + leaf_container_vector_it_arg, + leaf_container_vector_it_end_arg); + } + else { + // we reached leaf node level + + LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx); + + if (leaf_container_vector_it_arg && + (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) { + LeafContainerT& container = **child_leaf; + container = ***leaf_container_vector_it_arg; + ++*leaf_container_vector_it_arg; + } + + leaf_count_++; + + // execute deserialization callback + deserializeTreeCallback(**child_leaf, key_arg); + } + + // pop current branch voxel from key + key_arg.popBranch(); + } + } + } +} + +} // namespace octree +} // namespace pcl + +#define PCL_INSTANTIATE_OctreeBase(T) \ + template class PCL_EXPORTS pcl::octree::OctreeBase; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/impl/octree_iterator.hpp b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_iterator.hpp new file mode 100755 index 0000000..76e33fa --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_iterator.hpp @@ -0,0 +1,377 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_OCTREE_ITERATOR_HPP_ +#define PCL_OCTREE_ITERATOR_HPP_ + +#include + +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeDepthFirstIterator::OctreeDepthFirstIterator(unsigned int max_depth_arg) +: OctreeIteratorBase(max_depth_arg), stack_() +{ + // initialize iterator + this->reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeDepthFirstIterator::OctreeDepthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg) +: OctreeIteratorBase(octree_arg, max_depth_arg), stack_() +{ + // initialize iterator + this->reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeDepthFirstIterator::reset() +{ + OctreeIteratorBase::reset(); + + if (this->octree_) { + // allocate stack + stack_.reserve(this->max_octree_depth_); + + // empty stack + stack_.clear(); + + // pushing root node to stack + IteratorState stack_entry; + stack_entry.node_ = this->octree_->getRootNode(); + stack_entry.depth_ = 0; + stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0; + + stack_.push_back(stack_entry); + + this->current_state_ = &stack_.back(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeDepthFirstIterator::skipChildVoxels() +{ + + if (stack_.size()) { + // current depth + unsigned char current_depth = stack_.back().depth_; + + // pop from stack as long as we find stack elements with depth >= current depth + while (stack_.size() && (stack_.back().depth_ >= current_depth)) + stack_.pop_back(); + + if (stack_.size()) { + this->current_state_ = &stack_.back(); + } + else { + this->current_state_ = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeDepthFirstIterator& +OctreeDepthFirstIterator::operator++() +{ + + if (stack_.size()) { + // get stack element + IteratorState stack_entry = stack_.back(); + stack_.pop_back(); + + stack_entry.depth_++; + + if ((this->max_octree_depth_ >= stack_entry.depth_) && + (stack_entry.node_->getNodeType() == BRANCH_NODE)) { + // current node is a branch node + BranchNode* current_branch = static_cast(stack_entry.node_); + + OctreeKey& current_key = stack_entry.key_; + + // add all children to stack + for (std::int8_t i = 7; i >= 0; --i) { + const unsigned char child_idx = (unsigned char)i; + + // if child exist + if (this->octree_->branchHasChild(*current_branch, child_idx)) { + // add child to stack + current_key.pushBranch(child_idx); + + stack_entry.node_ = + this->octree_->getBranchChildPtr(*current_branch, child_idx); + + stack_.push_back(stack_entry); + + current_key.popBranch(); + } + } + } + + if (stack_.size()) { + this->current_state_ = &stack_.back(); + } + else { + this->current_state_ = 0; + } + } + + return (*this); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBreadthFirstIterator::OctreeBreadthFirstIterator( + unsigned int max_depth_arg) +: OctreeIteratorBase(max_depth_arg), FIFO_() +{ + OctreeIteratorBase::reset(); + + // initialize iterator + this->reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBreadthFirstIterator::OctreeBreadthFirstIterator( + OctreeT* octree_arg, unsigned int max_depth_arg) +: OctreeIteratorBase(octree_arg, max_depth_arg), FIFO_() +{ + OctreeIteratorBase::reset(); + + // initialize iterator + this->reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBreadthFirstIterator::reset() +{ + OctreeIteratorBase::reset(); + + // init FIFO + FIFO_.clear(); + + if (this->octree_) { + // pushing root node to stack + IteratorState FIFO_entry; + FIFO_entry.node_ = this->octree_->getRootNode(); + FIFO_entry.depth_ = 0; + FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0; + + FIFO_.push_back(FIFO_entry); + + this->current_state_ = &FIFO_.front(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBreadthFirstIterator& +OctreeBreadthFirstIterator::operator++() +{ + + if (FIFO_.size()) { + // get stack element + IteratorState FIFO_entry = FIFO_.front(); + FIFO_.pop_front(); + + FIFO_entry.depth_++; + + if ((this->max_octree_depth_ >= FIFO_entry.depth_) && + (FIFO_entry.node_->getNodeType() == BRANCH_NODE)) { + // current node is a branch node + BranchNode* current_branch = static_cast(FIFO_entry.node_); + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; ++child_idx) { + + // if child exist + if (this->octree_->branchHasChild(*current_branch, child_idx)) { + // add child to stack + OctreeKey& current_key = FIFO_entry.key_; + current_key.pushBranch(static_cast(child_idx)); + + FIFO_entry.node_ = + this->octree_->getBranchChildPtr(*current_branch, child_idx); + + FIFO_.push_back(FIFO_entry); + + current_key.popBranch(); + } + } + } + + if (FIFO_.size()) { + this->current_state_ = &FIFO_.front(); + } + else { + this->current_state_ = 0; + } + } + + return (*this); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeFixedDepthIterator::OctreeFixedDepthIterator() +: OctreeBreadthFirstIterator(0u), fixed_depth_(0u) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeFixedDepthIterator::OctreeFixedDepthIterator( + OctreeT* octree_arg, unsigned int fixed_depth_arg) +: OctreeBreadthFirstIterator(octree_arg, fixed_depth_arg) +, fixed_depth_(fixed_depth_arg) +{ + this->reset(fixed_depth_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeFixedDepthIterator::reset(unsigned int fixed_depth_arg) +{ + // Set the desired depth to walk through + fixed_depth_ = fixed_depth_arg; + + if (!this->octree_) { + return; + } + + // If I'm nowhere, reset + // If I'm somewhere and I can't guarantee I'm before the first node, reset + if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth())) + OctreeBreadthFirstIterator::reset(); + + if (this->octree_->getTreeDepth() < fixed_depth_) { + PCL_WARN("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger " + "than the octree's depth.\n"); + PCL_WARN("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n", + this->octree_->getTreeDepth(), + fixed_depth_); + } + + // By default for the parent class OctreeBreadthFirstIterator, if the + // depth argument is equal to 0, the iterator would run over every node. + // For the OctreeFixedDepthIterator, whatever the depth ask, set the + // max_octree_depth_ accordingly + this->max_octree_depth_ = std::min(fixed_depth_, this->octree_->getTreeDepth()); + + // Restore previous state in case breath first iterator had child nodes already set up + if (FIFO_.size()) + this->current_state_ = &FIFO_.front(); + + // Iterate all the way to the desired level + while (this->current_state_ && (this->getCurrentOctreeDepth() != fixed_depth_)) + OctreeBreadthFirstIterator::operator++(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator( + unsigned int max_depth_arg) +: OctreeBreadthFirstIterator(max_depth_arg) +{ + reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator( + OctreeT* octree_arg, unsigned int max_depth_arg) +: OctreeBreadthFirstIterator(octree_arg, max_depth_arg) +{ + reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::deque& fifo) +: OctreeBreadthFirstIterator(octree_arg, max_depth_arg, current_state, fifo) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeLeafNodeBreadthFirstIterator::reset() +{ + OctreeBreadthFirstIterator::reset(); + ++*this; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator& +OctreeLeafNodeBreadthFirstIterator::operator++() +{ + do { + OctreeBreadthFirstIterator::operator++(); + } while ((this->current_state_) && + (this->current_state_->node_->getNodeType() != LEAF_NODE)); + + return (*this); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator +OctreeLeafNodeBreadthFirstIterator::operator++(int) +{ + OctreeLeafNodeBreadthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); +} +} // namespace octree +} // namespace pcl + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud.hpp b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud.hpp new file mode 100755 index 0000000..d2ee373 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud.hpp @@ -0,0 +1,1056 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_OCTREE_POINTCLOUD_HPP_ +#define PCL_OCTREE_POINTCLOUD_HPP_ + +#include + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::octree::OctreePointCloud:: + OctreePointCloud(const double resolution) +: OctreeT() +, input_(PointCloudConstPtr()) +, indices_(IndicesConstPtr()) +, epsilon_(0) +, resolution_(resolution) +, min_x_(0.0f) +, max_x_(resolution) +, min_y_(0.0f) +, max_y_(resolution) +, min_z_(0.0f) +, max_z_(resolution) +, bounding_box_defined_(false) +, max_objs_per_leaf_(0) +{ + assert(resolution > 0.0f); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + addPointsFromInputCloud() +{ + if (indices_) { + for (const int& index : *indices_) { + assert((index >= 0) && (index < static_cast(input_->points.size()))); + + if (isFinite(input_->points[index])) { + // add points to octree + this->addPointIdx(index); + } + } + } + else { + for (std::size_t i = 0; i < input_->points.size(); i++) { + if (isFinite(input_->points[i])) { + // add points to octree + this->addPointIdx(static_cast(i)); + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg) +{ + this->addPointIdx(point_idx_arg); + if (indices_arg) + indices_arg->push_back(point_idx_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg) +{ + assert(cloud_arg == input_); + + cloud_arg->push_back(point_arg); + + this->addPointIdx(static_cast(cloud_arg->points.size()) - 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + addPointToCloud(const PointT& point_arg, + PointCloudPtr cloud_arg, + IndicesPtr indices_arg) +{ + assert(cloud_arg == input_); + assert(indices_arg == indices_); + + cloud_arg->push_back(point_arg); + + this->addPointFromCloud(static_cast(cloud_arg->points.size()) - 1, + indices_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloud:: + isVoxelOccupiedAtPoint(const PointT& point_arg) const +{ + if (!isPointWithinBoundingBox(point_arg)) { + return false; + } + + OctreeKey key; + + // generate key for point + this->genOctreeKeyforPoint(point_arg, key); + + // search for key in octree + return (this->existLeaf(key)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloud:: + isVoxelOccupiedAtPoint(const int& point_idx_arg) const +{ + // retrieve point from input cloud + const PointT& point = this->input_->points[point_idx_arg]; + + // search for voxel at point in octree + return (this->isVoxelOccupiedAtPoint(point)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloud:: + isVoxelOccupiedAtPoint(const double point_x_arg, + const double point_y_arg, + const double point_z_arg) const +{ + // create a new point with the argument coordinates + PointT point; + point.x = point_x_arg; + point.y = point_y_arg; + point.z = point_z_arg; + + // search for voxel at point in octree + return (this->isVoxelOccupiedAtPoint(point)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + deleteVoxelAtPoint(const PointT& point_arg) +{ + if (!isPointWithinBoundingBox(point_arg)) { + return; + } + + OctreeKey key; + + // generate key for point + this->genOctreeKeyforPoint(point_arg, key); + + this->removeLeaf(key); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + deleteVoxelAtPoint(const int& point_idx_arg) +{ + // retrieve point from input cloud + const PointT& point = this->input_->points[point_idx_arg]; + + // delete leaf at point + this->deleteVoxelAtPoint(point); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloud:: + getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const +{ + OctreeKey key; + key.x = key.y = key.z = 0; + + voxel_center_list_arg.clear(); + + return getOccupiedVoxelCentersRecursive(this->root_node_, key, voxel_center_list_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloud:: + getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin, + const Eigen::Vector3f& end, + AlignedPointTVector& voxel_center_list, + float precision) +{ + Eigen::Vector3f direction = end - origin; + float norm = direction.norm(); + direction.normalize(); + + const float step_size = static_cast(resolution_) * precision; + // Ensure we get at least one step for the first voxel. + const int nsteps = std::max(1, static_cast(norm / step_size)); + + OctreeKey prev_key; + + bool bkeyDefined = false; + + // Walk along the line segment with small steps. + for (int i = 0; i < nsteps; ++i) { + Eigen::Vector3f p = origin + (direction * step_size * static_cast(i)); + + PointT octree_p; + octree_p.x = p.x(); + octree_p.y = p.y(); + octree_p.z = p.z(); + + OctreeKey key; + this->genOctreeKeyforPoint(octree_p, key); + + // Not a new key, still the same voxel. + if ((key == prev_key) && (bkeyDefined)) + continue; + + prev_key = key; + bkeyDefined = true; + + PointT center; + genLeafNodeCenterFromOctreeKey(key, center); + voxel_center_list.push_back(center); + } + + OctreeKey end_key; + PointT end_p; + end_p.x = end.x(); + end_p.y = end.y(); + end_p.z = end.z(); + this->genOctreeKeyforPoint(end_p, end_key); + if (!(end_key == prev_key)) { + PointT center; + genLeafNodeCenterFromOctreeKey(end_key, center); + voxel_center_list.push_back(center); + } + + return (static_cast(voxel_center_list.size())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + defineBoundingBox() +{ + + double minX, minY, minZ, maxX, maxY, maxZ; + + PointT min_pt; + PointT max_pt; + + // bounding box cannot be changed once the octree contains elements + assert(this->leaf_count_ == 0); + + pcl::getMinMax3D(*input_, min_pt, max_pt); + + float minValue = std::numeric_limits::epsilon() * 512.0f; + + minX = min_pt.x; + minY = min_pt.y; + minZ = min_pt.z; + + maxX = max_pt.x + minValue; + maxY = max_pt.y + minValue; + maxZ = max_pt.z + minValue; + + // generate bit masks for octree + defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + defineBoundingBox(const double min_x_arg, + const double min_y_arg, + const double min_z_arg, + const double max_x_arg, + const double max_y_arg, + const double max_z_arg) +{ + // bounding box cannot be changed once the octree contains elements + assert(this->leaf_count_ == 0); + + assert(max_x_arg >= min_x_arg); + assert(max_y_arg >= min_y_arg); + assert(max_z_arg >= min_z_arg); + + min_x_ = min_x_arg; + max_x_ = max_x_arg; + + min_y_ = min_y_arg; + max_y_ = max_y_arg; + + min_z_ = min_z_arg; + max_z_ = max_z_arg; + + min_x_ = std::min(min_x_, max_x_); + min_y_ = std::min(min_y_, max_y_); + min_z_ = std::min(min_z_, max_z_); + + max_x_ = std::max(min_x_, max_x_); + max_y_ = std::max(min_y_, max_y_); + max_z_ = std::max(min_z_, max_z_); + + // generate bit masks for octree + getKeyBitSize(); + + bounding_box_defined_ = true; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + defineBoundingBox(const double max_x_arg, + const double max_y_arg, + const double max_z_arg) +{ + // bounding box cannot be changed once the octree contains elements + assert(this->leaf_count_ == 0); + + assert(max_x_arg >= 0.0f); + assert(max_y_arg >= 0.0f); + assert(max_z_arg >= 0.0f); + + min_x_ = 0.0f; + max_x_ = max_x_arg; + + min_y_ = 0.0f; + max_y_ = max_y_arg; + + min_z_ = 0.0f; + max_z_ = max_z_arg; + + min_x_ = std::min(min_x_, max_x_); + min_y_ = std::min(min_y_, max_y_); + min_z_ = std::min(min_z_, max_z_); + + max_x_ = std::max(min_x_, max_x_); + max_y_ = std::max(min_y_, max_y_); + max_z_ = std::max(min_z_, max_z_); + + // generate bit masks for octree + getKeyBitSize(); + + bounding_box_defined_ = true; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + defineBoundingBox(const double cubeLen_arg) +{ + // bounding box cannot be changed once the octree contains elements + assert(this->leaf_count_ == 0); + + assert(cubeLen_arg >= 0.0f); + + min_x_ = 0.0f; + max_x_ = cubeLen_arg; + + min_y_ = 0.0f; + max_y_ = cubeLen_arg; + + min_z_ = 0.0f; + max_z_ = cubeLen_arg; + + min_x_ = std::min(min_x_, max_x_); + min_y_ = std::min(min_y_, max_y_); + min_z_ = std::min(min_z_, max_z_); + + max_x_ = std::max(min_x_, max_x_); + max_y_ = std::max(min_y_, max_y_); + max_z_ = std::max(min_z_, max_z_); + + // generate bit masks for octree + getKeyBitSize(); + + bounding_box_defined_ = true; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + getBoundingBox(double& min_x_arg, + double& min_y_arg, + double& min_z_arg, + double& max_x_arg, + double& max_y_arg, + double& max_z_arg) const +{ + min_x_arg = min_x_; + min_y_arg = min_y_; + min_z_arg = min_z_; + + max_x_arg = max_x_; + max_y_arg = max_y_; + max_z_arg = max_z_; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + adoptBoundingBoxToPoint(const PointT& point_idx_arg) +{ + + const float minValue = std::numeric_limits::epsilon(); + + // increase octree size until point fits into bounding box + while (true) { + bool bLowerBoundViolationX = (point_idx_arg.x < min_x_); + bool bLowerBoundViolationY = (point_idx_arg.y < min_y_); + bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_); + + bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_); + bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_); + bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_); + + // do we violate any bounds? + if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || + bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ || + (!bounding_box_defined_)) { + + if (bounding_box_defined_) { + + double octreeSideLen; + unsigned char child_idx; + + // octree not empty - we add another tree level and thus increase its size by a + // factor of 2*2*2 + child_idx = static_cast(((!bUpperBoundViolationX) << 2) | + ((!bUpperBoundViolationY) << 1) | + ((!bUpperBoundViolationZ))); + + BranchNode* newRootBranch; + + newRootBranch = new BranchNode(); + this->branch_count_++; + + this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_); + + this->root_node_ = newRootBranch; + + octreeSideLen = static_cast(1 << this->octree_depth_) * resolution_; + + if (!bUpperBoundViolationX) + min_x_ -= octreeSideLen; + + if (!bUpperBoundViolationY) + min_y_ -= octreeSideLen; + + if (!bUpperBoundViolationZ) + min_z_ -= octreeSideLen; + + // configure tree depth of octree + this->octree_depth_++; + this->setTreeDepth(this->octree_depth_); + + // recalculate bounding box width + octreeSideLen = + static_cast(1 << this->octree_depth_) * resolution_ - minValue; + + // increase octree bounding box + max_x_ = min_x_ + octreeSideLen; + max_y_ = min_y_ + octreeSideLen; + max_z_ = min_z_ + octreeSideLen; + } + // bounding box is not defined - set it to point position + else { + // octree is empty - we set the center of the bounding box to our first pixel + this->min_x_ = point_idx_arg.x - this->resolution_ / 2; + this->min_y_ = point_idx_arg.y - this->resolution_ / 2; + this->min_z_ = point_idx_arg.z - this->resolution_ / 2; + + this->max_x_ = point_idx_arg.x + this->resolution_ / 2; + this->max_y_ = point_idx_arg.y + this->resolution_ / 2; + this->max_z_ = point_idx_arg.z + this->resolution_ / 2; + + getKeyBitSize(); + + bounding_box_defined_ = true; + } + } + else + // no bound violations anymore - leave while loop + break; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + expandLeafNode(LeafNode* leaf_node, + BranchNode* parent_branch, + unsigned char child_idx, + unsigned int depth_mask) +{ + + if (depth_mask) { + // get amount of objects in leaf container + std::size_t leaf_obj_count = (*leaf_node)->getSize(); + + // copy leaf data + std::vector leafIndices; + leafIndices.reserve(leaf_obj_count); + + (*leaf_node)->getPointIndices(leafIndices); + + // delete current leaf node + this->deleteBranchChild(*parent_branch, child_idx); + this->leaf_count_--; + + // create new branch node + BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx); + this->branch_count_++; + + // add data to new branch + OctreeKey new_index_key; + + for (const int& leafIndex : leafIndices) { + + const PointT& point_from_index = input_->points[leafIndex]; + // generate key + genOctreeKeyforPoint(point_from_index, new_index_key); + + LeafNode* newLeaf; + BranchNode* newBranchParent; + this->createLeafRecursive( + new_index_key, depth_mask, childBranch, newLeaf, newBranchParent); + + (*newLeaf)->addPointIndex(leafIndex); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + addPointIdx(const int point_idx_arg) +{ + OctreeKey key; + + assert(point_idx_arg < static_cast(input_->points.size())); + + const PointT& point = input_->points[point_idx_arg]; + + // make sure bounding box is big enough + adoptBoundingBoxToPoint(point); + + // generate key + genOctreeKeyforPoint(point, key); + + LeafNode* leaf_node; + BranchNode* parent_branch_of_leaf_node; + unsigned int depth_mask = this->createLeafRecursive( + key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node); + + if (this->dynamic_depth_enabled_ && depth_mask) { + // get amount of objects in leaf container + std::size_t leaf_obj_count = (*leaf_node)->getSize(); + + while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) { + // index to branch child + unsigned char child_idx = key.getChildIdxWithDepthMask(depth_mask * 2); + + expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask); + + depth_mask = this->createLeafRecursive(key, + this->depth_mask_, + this->root_node_, + leaf_node, + parent_branch_of_leaf_node); + leaf_obj_count = (*leaf_node)->getSize(); + } + } + + (*leaf_node)->addPointIndex(point_idx_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +const PointT& +pcl::octree::OctreePointCloud:: + getPointByIndex(const unsigned int index_arg) const +{ + // retrieve point from input cloud + assert(index_arg < static_cast(input_->points.size())); + return (this->input_->points[index_arg]); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + getKeyBitSize() +{ + unsigned int max_voxels; + + unsigned int max_key_x; + unsigned int max_key_y; + unsigned int max_key_z; + + double octree_side_len; + + const float minValue = std::numeric_limits::epsilon(); + + // find maximum key values for x, y, z + max_key_x = + static_cast(std::ceil((max_x_ - min_x_ - minValue) / resolution_)); + max_key_y = + static_cast(std::ceil((max_y_ - min_y_ - minValue) / resolution_)); + max_key_z = + static_cast(std::ceil((max_z_ - min_z_ - minValue) / resolution_)); + + // find maximum amount of keys + max_voxels = std::max(std::max(std::max(max_key_x, max_key_y), max_key_z), + static_cast(2)); + + // tree depth == amount of bits of max_voxels + this->octree_depth_ = + std::max((std::min(static_cast(OctreeKey::maxDepth), + static_cast( + std::ceil(std::log2(max_voxels) - minValue)))), + static_cast(0)); + + octree_side_len = static_cast(1 << this->octree_depth_) * resolution_; + + if (this->leaf_count_ == 0) { + double octree_oversize_x; + double octree_oversize_y; + double octree_oversize_z; + + octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0; + octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0; + octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0; + + assert(octree_oversize_x > -minValue); + assert(octree_oversize_y > -minValue); + assert(octree_oversize_z > -minValue); + + if (octree_oversize_x > minValue) { + min_x_ -= octree_oversize_x; + max_x_ += octree_oversize_x; + } + if (octree_oversize_y > minValue) { + min_y_ -= octree_oversize_y; + max_y_ += octree_oversize_y; + } + if (octree_oversize_z > minValue) { + min_z_ -= octree_oversize_z; + max_z_ += octree_oversize_z; + } + } + else { + max_x_ = min_x_ + octree_side_len; + max_y_ = min_y_ + octree_side_len; + max_z_ = min_z_ + octree_side_len; + } + + // configure tree depth of octree + this->setTreeDepth(this->octree_depth_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const +{ + // calculate integer key for point coordinates + key_arg.x = + static_cast((point_arg.x - this->min_x_) / this->resolution_); + key_arg.y = + static_cast((point_arg.y - this->min_y_) / this->resolution_); + key_arg.z = + static_cast((point_arg.z - this->min_z_) / this->resolution_); + + assert(key_arg.x <= this->max_key_.x); + assert(key_arg.y <= this->max_key_.y); + assert(key_arg.z <= this->max_key_.z); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + genOctreeKeyforPoint(const double point_x_arg, + const double point_y_arg, + const double point_z_arg, + OctreeKey& key_arg) const +{ + PointT temp_point; + + temp_point.x = static_cast(point_x_arg); + temp_point.y = static_cast(point_y_arg); + temp_point.z = static_cast(point_z_arg); + + // generate key for point + genOctreeKeyforPoint(temp_point, key_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloud:: + genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const +{ + const PointT temp_point = getPointByIndex(data_arg); + + // generate key for point + genOctreeKeyforPoint(temp_point, key_arg); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + genLeafNodeCenterFromOctreeKey(const OctreeKey& key, PointT& point) const +{ + // define point to leaf node voxel center + point.x = static_cast((static_cast(key.x) + 0.5f) * this->resolution_ + + this->min_x_); + point.y = static_cast((static_cast(key.y) + 0.5f) * this->resolution_ + + this->min_y_); + point.z = static_cast((static_cast(key.z) + 0.5f) * this->resolution_ + + this->min_z_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + genVoxelCenterFromOctreeKey(const OctreeKey& key_arg, + unsigned int tree_depth_arg, + PointT& point_arg) const +{ + // generate point for voxel center defined by treedepth (bitLen) and key + point_arg.x = static_cast( + (static_cast(key_arg.x) + 0.5f) * + (this->resolution_ * + static_cast(1 << (this->octree_depth_ - tree_depth_arg))) + + this->min_x_); + point_arg.y = static_cast( + (static_cast(key_arg.y) + 0.5f) * + (this->resolution_ * + static_cast(1 << (this->octree_depth_ - tree_depth_arg))) + + this->min_y_); + point_arg.z = static_cast( + (static_cast(key_arg.z) + 0.5f) * + (this->resolution_ * + static_cast(1 << (this->octree_depth_ - tree_depth_arg))) + + this->min_z_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloud:: + genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg, + unsigned int tree_depth_arg, + Eigen::Vector3f& min_pt, + Eigen::Vector3f& max_pt) const +{ + // calculate voxel size of current tree depth + double voxel_side_len = + this->resolution_ * + static_cast(1 << (this->octree_depth_ - tree_depth_arg)); + + // calculate voxel bounds + min_pt(0) = static_cast(static_cast(key_arg.x) * voxel_side_len + + this->min_x_); + min_pt(1) = static_cast(static_cast(key_arg.y) * voxel_side_len + + this->min_y_); + min_pt(2) = static_cast(static_cast(key_arg.z) * voxel_side_len + + this->min_z_); + + max_pt(0) = static_cast(static_cast(key_arg.x + 1) * voxel_side_len + + this->min_x_); + max_pt(1) = static_cast(static_cast(key_arg.y + 1) * voxel_side_len + + this->min_y_); + max_pt(2) = static_cast(static_cast(key_arg.z + 1) * voxel_side_len + + this->min_z_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +double +pcl::octree::OctreePointCloud:: + getVoxelSquaredSideLen(unsigned int tree_depth_arg) const +{ + double side_len; + + // side length of the voxel cube increases exponentially with the octree depth + side_len = this->resolution_ * + static_cast(1 << (this->octree_depth_ - tree_depth_arg)); + + // squared voxel side length + side_len *= side_len; + + return (side_len); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +double +pcl::octree::OctreePointCloud:: + getVoxelSquaredDiameter(unsigned int tree_depth_arg) const +{ + // return the squared side length of the voxel cube as a function of the octree depth + return (getVoxelSquaredSideLen(tree_depth_arg) * 3); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloud:: + getOccupiedVoxelCentersRecursive(const BranchNode* node_arg, + const OctreeKey& key_arg, + AlignedPointTVector& voxel_center_list_arg) const +{ + int voxel_count = 0; + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (!this->branchHasChild(*node_arg, child_idx)) + continue; + + const OctreeNode* child_node; + child_node = this->getBranchChildPtr(*node_arg, child_idx); + + // generate new key for current branch voxel + OctreeKey new_key; + new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2))); + new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1))); + new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0))); + + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + voxel_count += getOccupiedVoxelCentersRecursive( + static_cast(child_node), new_key, voxel_center_list_arg); + break; + } + case LEAF_NODE: { + PointT new_point; + + genLeafNodeCenterFromOctreeKey(new_key, new_point); + voxel_center_list_arg.push_back(new_point); + + voxel_count++; + break; + } + default: + break; + } + } + return (voxel_count); +} + +#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndices, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeBase>; +#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndices, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::Octree2BufBase>; + +#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndex, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeBase>; +#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndex, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::Octree2BufBase>; + +#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeBase>; +#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::Octree2BufBase>; + +#endif /* OCTREE_POINTCLOUD_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud_adjacency.hpp new file mode 100755 index 0000000..adf3ac5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud_adjacency.hpp @@ -0,0 +1,334 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Jeremie Papon + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ +#define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ + +#include +#include +/* + * OctreePointCloudAdjacency is not precompiled, since it's used in other + * parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT + * used, octree_pointcloud_adjacency.h includes this file but octree_pointcloud.h + * would not include the implementation because it's precompiled. So we need to + * include it here "manually". + */ +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::octree::OctreePointCloudAdjacency:: + OctreePointCloudAdjacency(const double resolution_arg) +: OctreePointCloud>(resolution_arg) +{} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudAdjacency:: + addPointsFromInputCloud() +{ + // double t1,t2; + float minX = std::numeric_limits::max(), + minY = std::numeric_limits::max(), + minZ = std::numeric_limits::max(); + float maxX = -std::numeric_limits::max(), + maxY = -std::numeric_limits::max(), + maxZ = -std::numeric_limits::max(); + + for (std::size_t i = 0; i < input_->size(); ++i) { + PointT temp(input_->points[i]); + if (transform_func_) // Search for point with + transform_func_(temp); + if (!pcl::isFinite( + temp)) // Check to make sure transform didn't make point not finite + continue; + if (temp.x < minX) + minX = temp.x; + if (temp.y < minY) + minY = temp.y; + if (temp.z < minZ) + minZ = temp.z; + if (temp.x > maxX) + maxX = temp.x; + if (temp.y > maxY) + maxY = temp.y; + if (temp.z > maxZ) + maxZ = temp.z; + } + this->defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ); + + OctreePointCloud::addPointsFromInputCloud(); + + leaf_vector_.reserve(this->getLeafCount()); + for (auto leaf_itr = this->leaf_depth_begin(); leaf_itr != this->leaf_depth_end(); + ++leaf_itr) { + OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey(); + LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer()); + + // Run the leaf's compute function + leaf_container->computeData(); + + computeNeighbors(leaf_key, leaf_container); + + leaf_vector_.push_back(leaf_container); + } + // Make sure our leaf vector is correctly sized + assert(leaf_vector_.size() == this->getLeafCount()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudAdjacency:: + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const +{ + if (transform_func_) { + PointT temp(point_arg); + transform_func_(temp); + // calculate integer key for transformed point coordinates + if (pcl::isFinite(temp)) // Make sure transformed point is finite - if it is not, it + // gets default key + { + key_arg.x = + static_cast((temp.x - this->min_x_) / this->resolution_); + key_arg.y = + static_cast((temp.y - this->min_y_) / this->resolution_); + key_arg.z = + static_cast((temp.z - this->min_z_) / this->resolution_); + } + else { + key_arg = OctreeKey(); + } + } + else { + // calculate integer key for point coordinates + key_arg.x = + static_cast((point_arg.x - this->min_x_) / this->resolution_); + key_arg.y = + static_cast((point_arg.y - this->min_y_) / this->resolution_); + key_arg.z = + static_cast((point_arg.z - this->min_z_) / this->resolution_); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudAdjacency:: + addPointIdx(const int pointIdx_arg) +{ + OctreeKey key; + + assert(pointIdx_arg < static_cast(this->input_->points.size())); + + const PointT& point = this->input_->points[pointIdx_arg]; + if (!pcl::isFinite(point)) + return; + + // generate key + this->genOctreeKeyforPoint(point, key); + // add point to octree at key + LeafContainerT* container = this->createLeaf(key); + container->addPoint(point); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudAdjacency:: + computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container) +{ + // Make sure requested key is valid + if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y || + key_arg.z > this->max_key_.z) { + PCL_ERROR("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for " + "invalid octree key\n"); + return; + } + + OctreeKey neighbor_key; + int dx_min = (key_arg.x > 0) ? -1 : 0; + int dy_min = (key_arg.y > 0) ? -1 : 0; + int dz_min = (key_arg.z > 0) ? -1 : 0; + int dx_max = (key_arg.x == this->max_key_.x) ? 0 : 1; + int dy_max = (key_arg.y == this->max_key_.y) ? 0 : 1; + int dz_max = (key_arg.z == this->max_key_.z) ? 0 : 1; + + for (int dx = dx_min; dx <= dx_max; ++dx) { + for (int dy = dy_min; dy <= dy_max; ++dy) { + for (int dz = dz_min; dz <= dz_max; ++dz) { + neighbor_key.x = static_cast(key_arg.x + dx); + neighbor_key.y = static_cast(key_arg.y + dy); + neighbor_key.z = static_cast(key_arg.z + dz); + LeafContainerT* neighbor = this->findLeaf(neighbor_key); + if (neighbor) { + leaf_container->addNeighbor(neighbor); + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +LeafContainerT* +pcl::octree::OctreePointCloudAdjacency:: + getLeafContainerAtPoint(const PointT& point_arg) const +{ + OctreeKey key; + LeafContainerT* leaf = nullptr; + // generate key + this->genOctreeKeyforPoint(point_arg, key); + + leaf = this->findLeaf(key); + + return leaf; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudAdjacency:: + computeVoxelAdjacencyGraph(VoxelAdjacencyList& voxel_adjacency_graph) +{ + // TODO Change this to use leaf centers, not centroids! + + voxel_adjacency_graph.clear(); + // Add a vertex for each voxel, store ids in map + std::map leaf_vertex_id_map; + for (typename OctreeAdjacencyT::LeafNodeDepthFirstIterator leaf_itr = + this->leaf_depth_begin(); + leaf_itr != this->leaf_depth_end(); + ++leaf_itr) { + OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey(); + PointT centroid_point; + this->genLeafNodeCenterFromOctreeKey(leaf_key, centroid_point); + VoxelID node_id = add_vertex(voxel_adjacency_graph); + + voxel_adjacency_graph[node_id] = centroid_point; + LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer()); + leaf_vertex_id_map[leaf_container] = node_id; + } + + // Iterate through and add edges to adjacency graph + for (typename std::vector::iterator leaf_itr = leaf_vector_.begin(); + leaf_itr != leaf_vector_.end(); + ++leaf_itr) { + VoxelID u = (leaf_vertex_id_map.find(*leaf_itr))->second; + PointT p_u = voxel_adjacency_graph[u]; + for (auto neighbor_itr = (*leaf_itr)->cbegin(), neighbor_end = (*leaf_itr)->cend(); + neighbor_itr != neighbor_end; + ++neighbor_itr) { + LeafContainerT* neighbor_container = *neighbor_itr; + EdgeID edge; + bool edge_added; + VoxelID v = (leaf_vertex_id_map.find(neighbor_container))->second; + boost::tie(edge, edge_added) = add_edge(u, v, voxel_adjacency_graph); + + PointT p_v = voxel_adjacency_graph[v]; + float dist = (p_v.getVector3fMap() - p_u.getVector3fMap()).norm(); + voxel_adjacency_graph[edge] = dist; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloudAdjacency:: + testForOcclusion(const PointT& point_arg, const PointXYZ& camera_pos) +{ + OctreeKey key; + this->genOctreeKeyforPoint(point_arg, key); + // This code follows the method in Octree::PointCloud + Eigen::Vector3f sensor(camera_pos.x, camera_pos.y, camera_pos.z); + + Eigen::Vector3f leaf_centroid( + static_cast((static_cast(key.x) + 0.5f) * this->resolution_ + + this->min_x_), + static_cast((static_cast(key.y) + 0.5f) * this->resolution_ + + this->min_y_), + static_cast((static_cast(key.z) + 0.5f) * this->resolution_ + + this->min_z_)); + Eigen::Vector3f direction = sensor - leaf_centroid; + + float norm = direction.norm(); + direction.normalize(); + float precision = 1.0f; + const float step_size = static_cast(resolution_) * precision; + const int nsteps = std::max(1, static_cast(norm / step_size)); + + OctreeKey prev_key = key; + // Walk along the line segment with small steps. + Eigen::Vector3f p = leaf_centroid; + PointT octree_p; + for (int i = 0; i < nsteps; ++i) { + // Start at the leaf voxel, and move back towards sensor. + p += (direction * step_size); + + octree_p.x = p.x(); + octree_p.y = p.y(); + octree_p.z = p.z(); + // std::cout << octree_p<< "\n"; + OctreeKey key; + this->genOctreeKeyforPoint(octree_p, key); + + // Not a new key, still the same voxel (starts at self). + if ((key == prev_key)) + continue; + + prev_key = key; + + LeafContainerT* leaf = this->findLeaf(key); + // If the voxel is occupied, there is a possible occlusion + if (leaf) { + return true; + } + } + + // If we didn't run into a voxel on the way to this camera, it can't be occluded. + return false; +} + +#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp new file mode 100755 index 0000000..ed3f132 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp @@ -0,0 +1,141 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: octree_pointcloud_voxelcentroid.hpp 6459 2012-07-18 07:50:37Z dpb $ + */ + +#ifndef PCL_OCTREE_VOXELCENTROID_HPP +#define PCL_OCTREE_VOXELCENTROID_HPP + +/* + * OctreePointCloudVoxelcontroid is not precompiled, since it's used in other + * parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT + * used, octree_pointcloud_voxelcentroid.h includes this file but octree_pointcloud.h + * would not include the implementation because it's precompiled. So we need to + * include it here "manually". + */ +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloudVoxelCentroid:: + getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const +{ + OctreeKey key; + LeafContainerT* leaf = NULL; + + // generate key + genOctreeKeyforPoint(point_arg, key); + + leaf = this->findLeaf(key); + if (leaf) + leaf->getCentroid(voxel_centroid_arg); + + return (leaf != NULL); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +std::size_t +pcl::octree::OctreePointCloudVoxelCentroid:: + getVoxelCentroids( + typename OctreePointCloud:: + AlignedPointTVector& voxel_centroid_list_arg) const +{ + OctreeKey new_key; + + // reset output vector + voxel_centroid_list_arg.clear(); + voxel_centroid_list_arg.reserve(this->leaf_count_); + + getVoxelCentroidsRecursive(this->root_node_, new_key, voxel_centroid_list_arg); + + // return size of centroid vector + return (voxel_centroid_list_arg.size()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudVoxelCentroid:: + getVoxelCentroidsRecursive( + const BranchNode* branch_arg, + OctreeKey& key_arg, + typename OctreePointCloud:: + AlignedPointTVector& voxel_centroid_list_arg) const +{ + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + // if child exist + if (branch_arg->hasChild(child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); + + OctreeNode* child_node = branch_arg->getChildPtr(child_idx); + + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + getVoxelCentroidsRecursive(static_cast(child_node), + key_arg, + voxel_centroid_list_arg); + break; + } + case LEAF_NODE: { + PointT new_centroid; + + LeafNode* container = static_cast(child_node); + + container->getContainer().getCentroid(new_centroid); + + voxel_centroid_list_arg.push_back(new_centroid); + break; + } + default: + break; + } + + // pop current branch voxel from key + key_arg.popBranch(); + } + } +} + +#define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/impl/octree_search.hpp b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_search.hpp new file mode 100755 index 0000000..9a72e3d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/impl/octree_search.hpp @@ -0,0 +1,1071 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_OCTREE_SEARCH_IMPL_H_ +#define PCL_OCTREE_SEARCH_IMPL_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloudSearch:: + voxelSearch(const PointT& point, std::vector& point_idx_data) +{ + assert(isFinite(point) && + "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + OctreeKey key; + bool b_success = false; + + // generate key + this->genOctreeKeyforPoint(point, key); + + LeafContainerT* leaf = this->findLeaf(key); + + if (leaf) { + (*leaf).getPointIndices(point_idx_data); + b_success = true; + } + + return (b_success); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::octree::OctreePointCloudSearch:: + voxelSearch(const int index, std::vector& point_idx_data) +{ + const PointT search_point = this->getPointByIndex(index); + return (this->voxelSearch(search_point, point_idx_data)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + nearestKSearch(const PointT& p_q, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances) +{ + assert(this->leaf_count_ > 0); + assert(isFinite(p_q) && + "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + k_indices.clear(); + k_sqr_distances.clear(); + + if (k < 1) + return 0; + + prioPointQueueEntry point_entry; + std::vector point_candidates; + + OctreeKey key; + key.x = key.y = key.z = 0; + + // initialize smallest point distance in search with high value + double smallest_dist = std::numeric_limits::max(); + + getKNearestNeighborRecursive( + p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates); + + unsigned int result_count = static_cast(point_candidates.size()); + + k_indices.resize(result_count); + k_sqr_distances.resize(result_count); + + for (unsigned int i = 0; i < result_count; ++i) { + k_indices[i] = point_candidates[i].point_idx_; + k_sqr_distances[i] = point_candidates[i].point_distance_; + } + + return static_cast(k_indices.size()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + nearestKSearch(int index, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances) +{ + const PointT search_point = this->getPointByIndex(index); + return (nearestKSearch(search_point, k, k_indices, k_sqr_distances)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudSearch:: + approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance) +{ + assert(this->leaf_count_ > 0); + assert(isFinite(p_q) && + "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + OctreeKey key; + key.x = key.y = key.z = 0; + + approxNearestSearchRecursive( + p_q, this->root_node_, key, 1, result_index, sqr_distance); + + return; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudSearch:: + approxNearestSearch(int query_index, int& result_index, float& sqr_distance) +{ + const PointT search_point = this->getPointByIndex(query_index); + + return (approxNearestSearch(search_point, result_index, sqr_distance)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + radiusSearch(const PointT& p_q, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const +{ + assert(isFinite(p_q) && + "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + OctreeKey key; + key.x = key.y = key.z = 0; + + k_indices.clear(); + k_sqr_distances.clear(); + + getNeighborsWithinRadiusRecursive(p_q, + radius * radius, + this->root_node_, + key, + 1, + k_indices, + k_sqr_distances, + max_nn); + + return (static_cast(k_indices.size())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + radiusSearch(int index, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const +{ + const PointT search_point = this->getPointByIndex(index); + + return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + boxSearch(const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + std::vector& k_indices) const +{ + + OctreeKey key; + key.x = key.y = key.z = 0; + + k_indices.clear(); + + boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices); + + return (static_cast(k_indices.size())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +double +pcl::octree::OctreePointCloudSearch:: + getKNearestNeighborRecursive( + const PointT& point, + unsigned int K, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + const double squared_search_radius, + std::vector& point_candidates) const +{ + std::vector search_heap; + search_heap.resize(8); + + OctreeKey new_key; + + double smallest_squared_dist = squared_search_radius; + + // get spatial voxel information + double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth); + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (this->branchHasChild(*node, child_idx)) { + PointT voxel_center; + + search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); + + // generate voxel center point for voxel at key + this->genVoxelCenterFromOctreeKey( + search_heap[child_idx].key, tree_depth, voxel_center); + + // generate new priority queue element + search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx); + search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point); + } + else { + search_heap[child_idx].point_distance = std::numeric_limits::infinity(); + } + } + + std::sort(search_heap.begin(), search_heap.end()); + + // iterate over all children in priority queue + // check if the distance to search candidate is smaller than the best point distance + // (smallest_squared_dist) + while ((!search_heap.empty()) && + (search_heap.back().point_distance < + smallest_squared_dist + voxelSquaredDiameter / 4.0 + + sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) { + const OctreeNode* child_node; + + // read from priority queue element + child_node = search_heap.back().node; + new_key = search_heap.back().key; + + if (tree_depth < this->octree_depth_) { + // we have not reached maximum tree depth + smallest_squared_dist = + getKNearestNeighborRecursive(point, + K, + static_cast(child_node), + new_key, + tree_depth + 1, + smallest_squared_dist, + point_candidates); + } + else { + // we reached leaf node level + std::vector decoded_point_vector; + + const LeafNode* child_leaf = static_cast(child_node); + + // decode leaf node into decoded_point_vector + (*child_leaf)->getPointIndices(decoded_point_vector); + + // Linearly iterate over all decoded (unsorted) points + for (const int& point_index : decoded_point_vector) { + + const PointT& candidate_point = this->getPointByIndex(point_index); + + // calculate point distance to search point + float squared_dist = pointSquaredDist(candidate_point, point); + + // check if a closer match is found + if (squared_dist < smallest_squared_dist) { + prioPointQueueEntry point_entry; + + point_entry.point_distance_ = squared_dist; + point_entry.point_idx_ = point_index; + point_candidates.push_back(point_entry); + } + } + + std::sort(point_candidates.begin(), point_candidates.end()); + + if (point_candidates.size() > K) + point_candidates.resize(K); + + if (point_candidates.size() == K) + smallest_squared_dist = point_candidates.back().point_distance_; + } + // pop element from priority queue + search_heap.pop_back(); + } + + return (smallest_squared_dist); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudSearch:: + getNeighborsWithinRadiusRecursive(const PointT& point, + const double radiusSquared, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const +{ + // get spatial voxel information + double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth); + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (!this->branchHasChild(*node, child_idx)) + continue; + + const OctreeNode* child_node; + child_node = this->getBranchChildPtr(*node, child_idx); + + OctreeKey new_key; + PointT voxel_center; + float squared_dist; + + // generate new key for current branch voxel + new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); + + // generate voxel center point for voxel at key + this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center); + + // calculate distance to search point + squared_dist = pointSquaredDist(static_cast(voxel_center), point); + + // if distance is smaller than search radius + if (squared_dist + this->epsilon_ <= + voxel_squared_diameter / 4.0 + radiusSquared + + sqrt(voxel_squared_diameter * radiusSquared)) { + + if (tree_depth < this->octree_depth_) { + // we have not reached maximum tree depth + getNeighborsWithinRadiusRecursive(point, + radiusSquared, + static_cast(child_node), + new_key, + tree_depth + 1, + k_indices, + k_sqr_distances, + max_nn); + if (max_nn != 0 && k_indices.size() == static_cast(max_nn)) + return; + } + else { + // we reached leaf node level + const LeafNode* child_leaf = static_cast(child_node); + std::vector decoded_point_vector; + + // decode leaf node into decoded_point_vector + (*child_leaf)->getPointIndices(decoded_point_vector); + + // Linearly iterate over all decoded (unsorted) points + for (const int& index : decoded_point_vector) { + const PointT& candidate_point = this->getPointByIndex(index); + + // calculate point distance to search point + squared_dist = pointSquaredDist(candidate_point, point); + + // check if a match is found + if (squared_dist > radiusSquared) + continue; + + // add point to result vector + k_indices.push_back(index); + k_sqr_distances.push_back(squared_dist); + + if (max_nn != 0 && k_indices.size() == static_cast(max_nn)) + return; + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudSearch:: + approxNearestSearchRecursive(const PointT& point, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + int& result_index, + float& sqr_distance) +{ + OctreeKey minChildKey; + OctreeKey new_key; + + const OctreeNode* child_node; + + // set minimum voxel distance to maximum value + double min_voxel_center_distance = std::numeric_limits::max(); + + unsigned char min_child_idx = 0xFF; + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (!this->branchHasChild(*node, child_idx)) + continue; + + PointT voxel_center; + double voxelPointDist; + + new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); + + // generate voxel center point for voxel at key + this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center); + + voxelPointDist = pointSquaredDist(voxel_center, point); + + // search for child voxel with shortest distance to search point + if (voxelPointDist >= min_voxel_center_distance) + continue; + + min_voxel_center_distance = voxelPointDist; + min_child_idx = child_idx; + minChildKey = new_key; + } + + // make sure we found at least one branch child + assert(min_child_idx < 8); + + child_node = this->getBranchChildPtr(*node, min_child_idx); + + if (tree_depth < this->octree_depth_) { + // we have not reached maximum tree depth + approxNearestSearchRecursive(point, + static_cast(child_node), + minChildKey, + tree_depth + 1, + result_index, + sqr_distance); + } + else { + // we reached leaf node level + std::vector decoded_point_vector; + + const LeafNode* child_leaf = static_cast(child_node); + + double smallest_squared_dist = std::numeric_limits::max(); + + // decode leaf node into decoded_point_vector + (**child_leaf).getPointIndices(decoded_point_vector); + + // Linearly iterate over all decoded (unsorted) points + for (const int& index : decoded_point_vector) { + const PointT& candidate_point = this->getPointByIndex(index); + + // calculate point distance to search point + double squared_dist = pointSquaredDist(candidate_point, point); + + // check if a closer match is found + if (squared_dist >= smallest_squared_dist) + continue; + + result_index = index; + smallest_squared_dist = squared_dist; + sqr_distance = static_cast(squared_dist); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +float +pcl::octree::OctreePointCloudSearch:: + pointSquaredDist(const PointT& point_a, const PointT& point_b) const +{ + return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::octree::OctreePointCloudSearch:: + boxSearchRecursive(const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + std::vector& k_indices) const +{ + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + + const OctreeNode* child_node; + child_node = this->getBranchChildPtr(*node, child_idx); + + if (!child_node) + continue; + + OctreeKey new_key; + // generate new key for current branch voxel + new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); + + // voxel corners + Eigen::Vector3f lower_voxel_corner; + Eigen::Vector3f upper_voxel_corner; + // get voxel coordinates + this->genVoxelBoundsFromOctreeKey( + new_key, tree_depth, lower_voxel_corner, upper_voxel_corner); + + // test if search region overlap with voxel space + + if (!((lower_voxel_corner(0) > max_pt(0)) || (min_pt(0) > upper_voxel_corner(0)) || + (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) || + (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) { + + if (tree_depth < this->octree_depth_) { + // we have not reached maximum tree depth + boxSearchRecursive(min_pt, + max_pt, + static_cast(child_node), + new_key, + tree_depth + 1, + k_indices); + } + else { + // we reached leaf node level + std::vector decoded_point_vector; + + const LeafNode* child_leaf = static_cast(child_node); + + // decode leaf node into decoded_point_vector + (**child_leaf).getPointIndices(decoded_point_vector); + + // Linearly iterate over all decoded (unsorted) points + for (const int& index : decoded_point_vector) { + const PointT& candidate_point = this->getPointByIndex(index); + + // check if point falls within search box + bool bInBox = + ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) && + (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) && + (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2))); + + if (bInBox) + // add to result vector + k_indices.push_back(index); + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + getIntersectedVoxelCenters(Eigen::Vector3f origin, + Eigen::Vector3f direction, + AlignedPointTVector& voxel_center_list, + int max_voxel_count) const +{ + OctreeKey key; + key.x = key.y = key.z = 0; + + voxel_center_list.clear(); + + // Voxel child_idx remapping + unsigned char a = 0; + + double min_x, min_y, min_z, max_x, max_y, max_z; + + initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); + + if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z)) + return getIntersectedVoxelCentersRecursive(min_x, + min_y, + min_z, + max_x, + max_y, + max_z, + a, + this->root_node_, + key, + voxel_center_list, + max_voxel_count); + + return (0); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + getIntersectedVoxelIndices(Eigen::Vector3f origin, + Eigen::Vector3f direction, + std::vector& k_indices, + int max_voxel_count) const +{ + OctreeKey key; + key.x = key.y = key.z = 0; + + k_indices.clear(); + + // Voxel child_idx remapping + unsigned char a = 0; + double min_x, min_y, min_z, max_x, max_y, max_z; + + initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); + + if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z)) + return getIntersectedVoxelIndicesRecursive(min_x, + min_y, + min_z, + max_x, + max_y, + max_z, + a, + this->root_node_, + key, + k_indices, + max_voxel_count); + return (0); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + getIntersectedVoxelCentersRecursive(double min_x, + double min_y, + double min_z, + double max_x, + double max_y, + double max_z, + unsigned char a, + const OctreeNode* node, + const OctreeKey& key, + AlignedPointTVector& voxel_center_list, + int max_voxel_count) const +{ + if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0) + return (0); + + // If leaf node, get voxel center and increment intersection count + if (node->getNodeType() == LEAF_NODE) { + PointT newPoint; + + this->genLeafNodeCenterFromOctreeKey(key, newPoint); + + voxel_center_list.push_back(newPoint); + + return (1); + } + + // Voxel intersection count for branches children + int voxel_count = 0; + + // Voxel mid lines + double mid_x = 0.5 * (min_x + max_x); + double mid_y = 0.5 * (min_y + max_y); + double mid_z = 0.5 * (min_z + max_z); + + // First voxel node ray will intersect + int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z); + + // Child index, node and key + unsigned char child_idx; + OctreeKey child_key; + + do { + if (curr_node != 0) + child_idx = static_cast(curr_node ^ a); + else + child_idx = a; + + // child_node == 0 if child_node doesn't exist + const OctreeNode* child_node = + this->getBranchChildPtr(static_cast(*node), child_idx); + + // Generate new key for current branch voxel + child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2))); + child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1))); + child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0))); + + // Recursively call each intersected child node, selecting the next + // node intersected by the ray. Children that do not intersect will + // not be traversed. + + switch (curr_node) { + case 0: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(min_x, + min_y, + min_z, + mid_x, + mid_y, + mid_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1); + break; + + case 1: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(min_x, + min_y, + mid_z, + mid_x, + mid_y, + max_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8); + break; + + case 2: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(min_x, + mid_y, + min_z, + mid_x, + max_y, + mid_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3); + break; + + case 3: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(min_x, + mid_y, + mid_z, + mid_x, + max_y, + max_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8); + break; + + case 4: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(mid_x, + min_y, + min_z, + max_x, + mid_y, + mid_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5); + break; + + case 5: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(mid_x, + min_y, + mid_z, + max_x, + mid_y, + max_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8); + break; + + case 6: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(mid_x, + mid_y, + min_z, + max_x, + max_y, + mid_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7); + break; + + case 7: + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive(mid_x, + mid_y, + mid_z, + max_x, + max_y, + max_z, + a, + child_node, + child_key, + voxel_center_list, + max_voxel_count); + curr_node = 8; + break; + } + } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count)); + return (voxel_count); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::octree::OctreePointCloudSearch:: + getIntersectedVoxelIndicesRecursive(double min_x, + double min_y, + double min_z, + double max_x, + double max_y, + double max_z, + unsigned char a, + const OctreeNode* node, + const OctreeKey& key, + std::vector& k_indices, + int max_voxel_count) const +{ + if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0) + return (0); + + // If leaf node, get voxel center and increment intersection count + if (node->getNodeType() == LEAF_NODE) { + const LeafNode* leaf = static_cast(node); + + // decode leaf node into k_indices + (*leaf)->getPointIndices(k_indices); + + return (1); + } + + // Voxel intersection count for branches children + int voxel_count = 0; + + // Voxel mid lines + double mid_x = 0.5 * (min_x + max_x); + double mid_y = 0.5 * (min_y + max_y); + double mid_z = 0.5 * (min_z + max_z); + + // First voxel node ray will intersect + int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z); + + // Child index, node and key + unsigned char child_idx; + OctreeKey child_key; + do { + if (curr_node != 0) + child_idx = static_cast(curr_node ^ a); + else + child_idx = a; + + // child_node == 0 if child_node doesn't exist + const OctreeNode* child_node = + this->getBranchChildPtr(static_cast(*node), child_idx); + // Generate new key for current branch voxel + child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2))); + child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1))); + child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0))); + + // Recursively call each intersected child node, selecting the next + // node intersected by the ray. Children that do not intersect will + // not be traversed. + switch (curr_node) { + case 0: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(min_x, + min_y, + min_z, + mid_x, + mid_y, + mid_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1); + break; + + case 1: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(min_x, + min_y, + mid_z, + mid_x, + mid_y, + max_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8); + break; + + case 2: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(min_x, + mid_y, + min_z, + mid_x, + max_y, + mid_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3); + break; + + case 3: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(min_x, + mid_y, + mid_z, + mid_x, + max_y, + max_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8); + break; + + case 4: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(mid_x, + min_y, + min_z, + max_x, + mid_y, + mid_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5); + break; + + case 5: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(mid_x, + min_y, + mid_z, + max_x, + mid_y, + max_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8); + break; + + case 6: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(mid_x, + mid_y, + min_z, + max_x, + max_y, + mid_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7); + break; + + case 7: + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive(mid_x, + mid_y, + mid_z, + max_x, + max_y, + max_z, + a, + child_node, + child_key, + k_indices, + max_voxel_count); + curr_node = 8; + break; + } + } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count)); + + return (voxel_count); +} + +#define PCL_INSTANTIATE_OctreePointCloudSearch(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch; + +#endif // PCL_OCTREE_SEARCH_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree.h b/deps_install/include/pcl-1.10/pcl/octree/octree.h new file mode 100755 index 0000000..074ffbf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree.h @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree2buf_base.h b/deps_install/include/pcl-1.10/pcl/octree/octree2buf_base.h new file mode 100755 index 0000000..3915723 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree2buf_base.h @@ -0,0 +1,1009 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace pcl { +namespace octree { + +template +class BufferedBranchNode : public OctreeNode { + +public: + /** \brief Empty constructor. */ + BufferedBranchNode() : OctreeNode() { reset(); } + + /** \brief Copy constructor. */ + BufferedBranchNode(const BufferedBranchNode& source) : OctreeNode() + { + *this = source; + } + + /** \brief Copy operator. */ + inline BufferedBranchNode& + operator=(const BufferedBranchNode& source_arg) + { + memset(child_node_array_, 0, sizeof(child_node_array_)); + + for (unsigned char b = 0; b < 2; ++b) + for (unsigned char i = 0; i < 8; ++i) + if (source_arg.child_node_array_[b][i]) + child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy(); + + return (*this); + } + + /** \brief Empty constructor. */ + ~BufferedBranchNode() {} + + /** \brief Method to perform a deep copy of the octree */ + BufferedBranchNode* + deepCopy() const override + { + return new BufferedBranchNode(*this); + } + + /** \brief Get child pointer in current branch node + * \param buffer_arg: buffer selector + * \param index_arg: index of child in node + * \return pointer to child node + */ + inline OctreeNode* + getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const + { + assert((buffer_arg < 2) && (index_arg < 8)); + return child_node_array_[buffer_arg][index_arg]; + } + + /** \brief Set child pointer in current branch node + * \param buffer_arg: buffer selector + * \param index_arg: index of child in node + * \param newNode_arg: pointer to new child node + */ + inline void + setChildPtr(unsigned char buffer_arg, + unsigned char index_arg, + OctreeNode* newNode_arg) + { + assert((buffer_arg < 2) && (index_arg < 8)); + child_node_array_[buffer_arg][index_arg] = newNode_arg; + } + + /** \brief Check if branch is pointing to a particular child node + * \param buffer_arg: buffer selector + * \param index_arg: index of child in node + * \return "true" if pointer to child node exists; "false" otherwise + */ + inline bool + hasChild(unsigned char buffer_arg, unsigned char index_arg) const + { + assert((buffer_arg < 2) && (index_arg < 8)); + return (child_node_array_[buffer_arg][index_arg] != nullptr); + } + + /** \brief Get the type of octree node. Returns LEAVE_NODE type */ + node_type_t + getNodeType() const override + { + return BRANCH_NODE; + } + + /** \brief Reset branch node container for every branch buffer. */ + inline void + reset() + { + memset(&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2); + } + + /** \brief Get const pointer to container */ + const ContainerT* operator->() const { return &container_; } + + /** \brief Get pointer to container */ + ContainerT* operator->() { return &container_; } + + /** \brief Get const reference to container */ + const ContainerT& operator*() const { return container_; } + + /** \brief Get reference to container */ + ContainerT& operator*() { return container_; } + + /** \brief Get const reference to container */ + const ContainerT& + getContainer() const + { + return container_; + } + + /** \brief Get reference to container */ + ContainerT& + getContainer() + { + return container_; + } + + /** \brief Get const pointer to container */ + const ContainerT* + getContainerPtr() const + { + return &container_; + } + + /** \brief Get pointer to container */ + ContainerT* + getContainerPtr() + { + return &container_; + } + +protected: + ContainerT container_; + + OctreeNode* child_node_array_[2][8]; +}; + +/** \brief @b Octree double buffer class + * + * This octree implementation keeps two separate octree structures in memory + * which allows for differentially comparison of the octree structures (change + * detection, differential encoding). + * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should + * be initially defined). + * \note All leaf nodes are addressed by integer indices. + * \note The tree depth equates to the bit length of the voxel indices. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class Octree2BufBase { + +public: + using OctreeT = Octree2BufBase; + + // iterators are friends + friend class OctreeIteratorBase; + friend class OctreeDepthFirstIterator; + friend class OctreeBreadthFirstIterator; + friend class OctreeLeafNodeDepthFirstIterator; + friend class OctreeLeafNodeBreadthFirstIterator; + + using BranchNode = BufferedBranchNode; + using LeafNode = OctreeLeafNode; + + using BranchContainer = BranchContainerT; + using LeafContainer = LeafContainerT; + + // Octree default iterators + using Iterator = OctreeDepthFirstIterator; + using ConstIterator = const OctreeDepthFirstIterator; + Iterator + begin(unsigned int max_depth_arg = 0) + { + return Iterator(this, max_depth_arg); + }; + const Iterator + end() + { + return Iterator(); + }; + + // Octree leaf node iterators + // The previous deprecated names + // LeafNodeIterator and ConstLeafNodeIterator are deprecated. + // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead. + using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; + + PCL_DEPRECATED("use leaf_depth_begin() instead") + LeafNodeIterator + leaf_begin(unsigned int max_depth_arg = 0) + { + return LeafNodeIterator(this, max_depth_arg); + }; + + PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end() + { + return LeafNodeIterator(); + }; + + // The currently valide names + using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeDepthFirstIterator = + const OctreeLeafNodeDepthFirstIterator; + LeafNodeDepthFirstIterator + leaf_depth_begin(unsigned int max_depth_arg = 0) + { + return LeafNodeDepthFirstIterator(this, max_depth_arg); + }; + + const LeafNodeDepthFirstIterator + leaf_depth_end() + { + return LeafNodeDepthFirstIterator(); + }; + + // Octree depth-first iterators + using DepthFirstIterator = OctreeDepthFirstIterator; + using ConstDepthFirstIterator = const OctreeDepthFirstIterator; + DepthFirstIterator + depth_begin(unsigned int maxDepth_arg = 0) + { + return DepthFirstIterator(this, maxDepth_arg); + }; + const DepthFirstIterator + depth_end() + { + return DepthFirstIterator(); + }; + + // Octree breadth-first iterators + using BreadthFirstIterator = OctreeBreadthFirstIterator; + using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator; + BreadthFirstIterator + breadth_begin(unsigned int max_depth_arg = 0) + { + return BreadthFirstIterator(this, max_depth_arg); + }; + const BreadthFirstIterator + breadth_end() + { + return BreadthFirstIterator(); + }; + + // Octree leaf node iterators + using LeafNodeBreadthIterator = OctreeLeafNodeBreadthFirstIterator; + using ConstLeafNodeBreadthIterator = + const OctreeLeafNodeBreadthFirstIterator; + + LeafNodeBreadthIterator + leaf_breadth_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeBreadthIterator(this, + max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const LeafNodeBreadthIterator + leaf_breadth_end() + { + return LeafNodeBreadthIterator(this, 0, nullptr); + }; + + /** \brief Empty constructor. */ + Octree2BufBase(); + + /** \brief Empty deconstructor. */ + virtual ~Octree2BufBase(); + + /** \brief Copy constructor. */ + Octree2BufBase(const Octree2BufBase& source) + : leaf_count_(source.leaf_count_) + , branch_count_(source.branch_count_) + , root_node_(new (BranchNode)(*(source.root_node_))) + , depth_mask_(source.depth_mask_) + , max_key_(source.max_key_) + , buffer_selector_(source.buffer_selector_) + , tree_dirty_flag_(source.tree_dirty_flag_) + , octree_depth_(source.octree_depth_) + , dynamic_depth_enabled_(source.dynamic_depth_enabled_) + {} + + /** \brief Copy constructor. */ + inline Octree2BufBase& + operator=(const Octree2BufBase& source) + { + leaf_count_ = source.leaf_count_; + branch_count_ = source.branch_count_; + root_node_ = new (BranchNode)(*(source.root_node_)); + depth_mask_ = source.depth_mask_; + max_key_ = source.max_key_; + buffer_selector_ = source.buffer_selector_; + tree_dirty_flag_ = source.tree_dirty_flag_; + octree_depth_ = source.octree_depth_; + dynamic_depth_enabled_ = source.dynamic_depth_enabled_; + return (*this); + } + + /** \brief Set the maximum amount of voxels per dimension. + * \param max_voxel_index_arg: maximum amount of voxels per dimension + */ + void + setMaxVoxelIndex(unsigned int max_voxel_index_arg); + + /** \brief Set the maximum depth of the octree. + * \param depth_arg: maximum depth of octree + */ + void + setTreeDepth(unsigned int depth_arg); + + /** \brief Get the maximum depth of the octree. + * \return depth_arg: maximum depth of octree + */ + inline unsigned int + getTreeDepth() const + { + return this->octree_depth_; + } + + /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \note If leaf node already exist, this method returns the existing node + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + * \return pointer to new leaf node container. + */ + LeafContainerT* + createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \note If leaf node already exist, this method returns the existing node + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + * \return pointer to leaf node container if found, null pointer otherwise. + */ + LeafContainerT* + findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + * \return "true" if leaf node search is successful, otherwise it returns "false". + */ + bool + existLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) const; + + /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + */ + void + removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \brief Return the amount of existing leafs in the octree. + * \return amount of registered leaf nodes. + */ + inline std::size_t + getLeafCount() const + { + return (leaf_count_); + } + + /** \brief Return the amount of existing branches in the octree. + * \return amount of branch nodes. + */ + inline std::size_t + getBranchCount() const + { + return (branch_count_); + } + + /** \brief Delete the octree structure and its leaf nodes. + */ + void + deleteTree(); + + /** \brief Delete octree structure of previous buffer. */ + inline void + deletePreviousBuffer() + { + treeCleanUpRecursive(root_node_); + } + + /** \brief Delete the octree structure in the current buffer. */ + inline void + deleteCurrentBuffer() + { + buffer_selector_ = !buffer_selector_; + treeCleanUpRecursive(root_node_); + leaf_count_ = 0; + } + + /** \brief Switch buffers and reset current octree structure. */ + void + switchBuffers(); + + /** \brief Serialize octree into a binary output vector describing its branch node + * structure. + * \param binary_tree_out_arg: reference to output vector for writing binary + * tree structure. + * \param do_XOR_encoding_arg: select if binary tree structure should be generated + * based on current octree (false) of based on a XOR comparison between current and + * previous octree + **/ + void + serializeTree(std::vector& binary_tree_out_arg, + bool do_XOR_encoding_arg = false); + + /** \brief Serialize octree into a binary output vector describing its branch node + * structure and and push all DataT elements stored in the octree to a vector. + * \param binary_tree_out_arg: reference to output vector for writing binary tree + * structure. + * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the + * octree + * \param do_XOR_encoding_arg: select if binary tree structure should be + * generated based on current octree (false) of based on a XOR comparison between + * current and previous octree + **/ + void + serializeTree(std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_encoding_arg = false); + + /** \brief Outputs a vector of all DataT elements that are stored within the octree + * leaf nodes. + * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects + * in the octree + */ + void + serializeLeafs(std::vector& leaf_container_vector_arg); + + /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist + * in the previous octree buffer. + * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects + * in the octree + */ + void + serializeNewLeafs(std::vector& leaf_container_vector_arg); + + /** \brief Deserialize a binary octree description vector and create a corresponding + * octree structure. Leaf nodes are initialized with getDataTByKey(..). + * \param binary_tree_in_arg: reference to input vector for reading binary tree + * structure. + * \param do_XOR_decoding_arg: select if binary tree structure is based on current + * octree (false) of based on a XOR comparison between current and previous octree + */ + void + deserializeTree(std::vector& binary_tree_in_arg, + bool do_XOR_decoding_arg = false); + + /** \brief Deserialize a binary octree description and create a corresponding octree + * structure. Leaf nodes are initialized with DataT elements from the dataVector. + * \param binary_tree_in_arg: reference to inpvectoream for reading binary tree + * structure. + * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects + * in the octree + * \param do_XOR_decoding_arg: select if binary tree structure is based on current + * octree (false) of based on a XOR comparison between current and previous octree + */ + void + deserializeTree(std::vector& binary_tree_in_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_decoding_arg = false); + +protected: + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Protected octree methods based on octree keys + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Retrieve root node */ + OctreeNode* + getRootNode() const + { + return (this->root_node_); + } + + /** \brief Find leaf node + * \param key_arg: octree key addressing a leaf node. + * \return pointer to leaf container. If leaf node is not found, this pointer returns + * 0. + */ + inline LeafContainerT* + findLeaf(const OctreeKey& key_arg) const + { + LeafContainerT* result = nullptr; + findLeafRecursive(key_arg, depth_mask_, root_node_, result); + return result; + } + + /** \brief Create a leaf node. + * \note If the leaf node at the given octree node does not exist, it will be created + * and added to the tree. + * \param key_arg: octree key addressing a leaf node. + * \return pointer to an existing or created leaf container. + */ + inline LeafContainerT* + createLeaf(const OctreeKey& key_arg) + { + LeafNode* leaf_node; + BranchNode* leaf_node_parent; + + createLeafRecursive( + key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent, false); + + LeafContainerT* ret = leaf_node->getContainerPtr(); + + return ret; + } + + /** \brief Check if leaf doesn't exist in the octree + * \param key_arg: octree key addressing a leaf node. + * \return "true" if leaf node is found; "false" otherwise + */ + inline bool + existLeaf(const OctreeKey& key_arg) const + { + return (findLeaf(key_arg) != nullptr); + } + + /** \brief Remove leaf node from octree + * \param key_arg: octree key addressing a leaf node. + */ + inline void + removeLeaf(const OctreeKey& key_arg) + { + if (key_arg <= max_key_) { + deleteLeafRecursive(key_arg, depth_mask_, root_node_); + + // we changed the octree structure -> dirty + tree_dirty_flag_ = true; + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Branch node accessor inline functions + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Check if branch is pointing to a particular child node + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return "true" if pointer to child node exists; "false" otherwise + */ + inline bool + branchHasChild(const BranchNode& branch_arg, unsigned char child_idx_arg) const + { + // test occupancyByte for child existence + return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != nullptr); + } + + /** \brief Retrieve a child node pointer for child node at child_idx. + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer to octree child node class + */ + inline OctreeNode* + getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const + { + return branch_arg.getChildPtr(buffer_selector_, child_idx_arg); + } + + /** \brief Assign new child node to branch + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \param new_child_arg: pointer to new child node + */ + inline void + setBranchChildPtr(BranchNode& branch_arg, + unsigned char child_idx_arg, + OctreeNode* new_child_arg) + { + branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_child_arg); + } + + /** \brief Generate bit pattern reflecting the existence of child node pointers for + * current buffer + * \param branch_arg: reference to octree branch class + * \return a single byte with 8 bits of child node information + */ + inline char + getBranchBitPattern(const BranchNode& branch_arg) const + { + char node_bits; + + // create bit pattern + node_bits = 0; + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i); + node_bits |= static_cast((!!child) << i); + } + + return (node_bits); + } + + /** \brief Generate bit pattern reflecting the existence of child node pointers in + * specific buffer + * \param branch_arg: reference to octree branch class + * \param bufferSelector_arg: buffer selector + * \return a single byte with 8 bits of child node information + */ + inline char + getBranchBitPattern(const BranchNode& branch_arg, + unsigned char bufferSelector_arg) const + { + char node_bits; + + // create bit pattern + node_bits = 0; + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i); + node_bits |= static_cast((!!child) << i); + } + + return (node_bits); + } + + /** \brief Generate XOR bit pattern reflecting differences between the two octree + * buffers + * \param branch_arg: reference to octree branch class + * \return a single byte with 8 bits of child node XOR difference information + */ + inline char + getBranchXORBitPattern(const BranchNode& branch_arg) const + { + char node_bits[2]; + + // create bit pattern for both buffers + node_bits[0] = node_bits[1] = 0; + + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* childA = branch_arg.getChildPtr(0, i); + const OctreeNode* childB = branch_arg.getChildPtr(1, i); + + node_bits[0] |= static_cast((!!childA) << i); + node_bits[1] |= static_cast((!!childB) << i); + } + + return node_bits[0] ^ node_bits[1]; + } + + /** \brief Test if branch changed between previous and current buffer + * \param branch_arg: reference to octree branch class + * \return "true", if child node information differs between current and previous + * octree buffer + */ + inline bool + hasBranchChanges(const BranchNode& branch_arg) const + { + return (getBranchXORBitPattern(branch_arg) > 0); + } + + /** \brief Delete child node and all its subchilds from octree in specific buffer + * \param branch_arg: reference to octree branch class + * \param buffer_selector_arg: buffer selector + * \param child_idx_arg: index to child node + */ + inline void + deleteBranchChild(BranchNode& branch_arg, + unsigned char buffer_selector_arg, + unsigned char child_idx_arg) + { + if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) { + OctreeNode* branchChild = + branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg); + + switch (branchChild->getNodeType()) { + case BRANCH_NODE: { + // free child branch recursively + deleteBranch(*static_cast(branchChild)); + + // delete unused branch + delete (branchChild); + break; + } + + case LEAF_NODE: { + // push unused leaf to branch pool + delete (branchChild); + break; + } + default: + break; + } + + // set branch child pointer to 0 + branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, nullptr); + } + } + + /** \brief Delete child node and all its subchilds from octree in current buffer + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + */ + inline void + deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg); + } + + /** \brief Delete branch and all its subchilds from octree (both buffers) + * \param branch_arg: reference to octree branch class + */ + inline void + deleteBranch(BranchNode& branch_arg) + { + // delete all branch node children + for (char i = 0; i < 8; i++) { + + if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i)) { + // reference was copied - there is only one child instance to be deleted + deleteBranchChild(branch_arg, 0, i); + + // remove pointers from both buffers + branch_arg.setChildPtr(0, i, nullptr); + branch_arg.setChildPtr(1, i, nullptr); + } + else { + deleteBranchChild(branch_arg, 0, i); + deleteBranchChild(branch_arg, 1, i); + } + } + } + + /** \brief Fetch and add a new branch child to a branch class in current buffer + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer of new branch child to this reference + */ + inline BranchNode* + createBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + BranchNode* new_branch_child = new BranchNode(); + + branch_arg.setChildPtr( + buffer_selector_, child_idx_arg, static_cast(new_branch_child)); + + return new_branch_child; + } + + /** \brief Fetch and add a new leaf child to a branch class + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer of new leaf child to this reference + */ + inline LeafNode* + createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + LeafNode* new_leaf_child = new LeafNode(); + + branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child); + + return new_leaf_child; + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Recursive octree methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Create a leaf node at octree key. If leaf node does already exist, it is + * returned. + * \param key_arg: reference to an octree key + * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth + * indicator + * \param branch_arg: current branch node + * \param return_leaf_arg: return pointer to leaf container + * \param parent_of_leaf_arg: return pointer to parent of leaf node + * \param branch_reset_arg: Reset pointer array of current branch + * \return depth mask at which leaf node was created/found + **/ + unsigned int + createLeafRecursive(const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg, + bool branch_reset_arg = false); + + /** \brief Recursively search for a given leaf node and return a pointer. + * \note If leaf node does not exist, a 0 pointer is returned. + * \param key_arg: reference to an octree key + * \param depth_mask_arg: depth mask used for octree key analysis and for branch + * depth indicator + * \param branch_arg: current branch node + * \param result_arg: pointer to leaf container class + **/ + void + findLeafRecursive(const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const; + + /** \brief Recursively search and delete leaf node + * \param key_arg: reference to an octree key + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth + * indicator + * \param branch_arg: current branch node + * \return "true" if branch does not contain any childs; "false" otherwise. This + * indicates if current branch can be deleted. + **/ + bool + deleteLeafRecursive(const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg); + + /** \brief Recursively explore the octree and output binary octree description + * together with a vector of leaf node DataT content. + * \param branch_arg: current branch node + * \param key_arg: reference to an octree key + * \param binary_tree_out_arg: binary output vector + * \param leaf_container_vector_arg: vector to return pointers to all leaf container + * in the tree. + * \param do_XOR_encoding_arg: select if binary tree structure should be generated + * based on current octree (false) of based on a XOR comparison between current and + * previous octree + * \param new_leafs_filter_arg: execute callback only for leaf nodes that did not + * exist in preceding buffer + **/ + void + serializeTreeRecursive( + BranchNode* branch_arg, + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg, + bool do_XOR_encoding_arg = false, + bool new_leafs_filter_arg = false); + + /** \brief Rebuild an octree based on binary XOR octree description and DataT objects + * for leaf node initialization. + * \param branch_arg: current branch node + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth + * indicator + * \param key_arg: reference to an octree key + * \param binary_tree_in_it_arg iterator of binary input data + * \param binary_tree_in_it_end_arg + * \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers + * to be added to a leaf node + * \param leaf_container_vector_it_end_arg: iterator pointing to leaf container + * pointers pointing to last object in input container. + * \param branch_reset_arg: Reset pointer array of current branch + * \param do_XOR_decoding_arg: select if binary tree structure is based on current + * octree (false) of based on a XOR comparison between current and previous octree + **/ + void + deserializeTreeRecursive( + BranchNode* branch_arg, + unsigned int depth_mask_arg, + OctreeKey& key_arg, + typename std::vector::const_iterator& binary_tree_in_it_arg, + typename std::vector::const_iterator& binary_tree_in_it_end_arg, + typename std::vector::const_iterator* + leaf_container_vector_it_arg, + typename std::vector::const_iterator* + leaf_container_vector_it_end_arg, + bool branch_reset_arg = false, + bool do_XOR_decoding_arg = false); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Serialization callbacks + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Callback executed for every leaf node data during serialization + **/ + virtual void + serializeTreeCallback(LeafContainerT&, const OctreeKey&) + {} + + /** \brief Callback executed for every leaf node data during deserialization + **/ + virtual void + deserializeTreeCallback(LeafContainerT&, const OctreeKey&) + {} + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Helpers + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Recursively explore the octree and remove unused branch and leaf nodes + * \param branch_arg: current branch node + **/ + void + treeCleanUpRecursive(BranchNode* branch_arg); + + /** \brief Helper function to calculate the binary logarithm + * \param n_arg: some value + * \return binary logarithm (log2) of argument n_arg + */ + PCL_DEPRECATED("use std::log2 instead") inline double Log2(double n_arg) + { + return std::log2(n_arg); + } + + /** \brief Test if octree is able to dynamically change its depth. This is required + * for adaptive bounding box adjustment. + * \return "false" - not resizeable due to XOR serialization + **/ + inline bool + octreeCanResize() + { + return (false); + } + + /** \brief Prints binary representation of a byte - used for debugging + * \param data_arg - byte to be printed to stdout + **/ + inline void + printBinary(char data_arg) + { + unsigned char mask = 1; // Bit mask + + // Extract the bits + for (int i = 0; i < 8; i++) { + // Mask each bit in the byte and print it + std::cout << ((data_arg & (mask << i)) ? "1" : "0"); + } + std::cout << std::endl; + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Globals + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Amount of leaf nodes **/ + std::size_t leaf_count_; + + /** \brief Amount of branch nodes **/ + std::size_t branch_count_; + + /** \brief Pointer to root branch node of octree **/ + BranchNode* root_node_; + + /** \brief Depth mask based on octree depth **/ + unsigned int depth_mask_; + + /** \brief key range */ + OctreeKey max_key_; + + /** \brief Currently active octree buffer **/ + unsigned char buffer_selector_; + + // flags indicating if unused branches and leafs might exist in previous buffer + bool tree_dirty_flag_; + + /** \brief Octree depth */ + unsigned int octree_depth_; + + /** \brief Enable dynamic_depth + * \note Note that this parameter is ignored in octree2buf! */ + bool dynamic_depth_enabled_; +}; +} // namespace octree +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_base.h b/deps_install/include/pcl-1.10/pcl/octree/octree_base.h new file mode 100755 index 0000000..026d65d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_base.h @@ -0,0 +1,709 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace pcl { +namespace octree { +/** \brief Octree class + * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should + * be initially defined). + * \note All leaf nodes are addressed by integer indices. + * \note The tree depth equates to the bit length of the voxel indices. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeBase { +public: + using OctreeT = OctreeBase; + + using BranchNode = OctreeBranchNode; + using LeafNode = OctreeLeafNode; + + using BranchContainer = BranchContainerT; + using LeafContainer = LeafContainerT; + +protected: + /////////////////////////////////////////////////////////////////////// + // Members + /////////////////////////////////////////////////////////////////////// + + /** \brief Amount of leaf nodes **/ + std::size_t leaf_count_; + + /** \brief Amount of branch nodes **/ + std::size_t branch_count_; + + /** \brief Pointer to root branch node of octree **/ + BranchNode* root_node_; + + /** \brief Depth mask based on octree depth **/ + unsigned int depth_mask_; + + /** \brief Octree depth */ + unsigned int octree_depth_; + + /** \brief Enable dynamic_depth **/ + bool dynamic_depth_enabled_; + + /** \brief key range */ + OctreeKey max_key_; + +public: + // iterators are friends + friend class OctreeIteratorBase; + friend class OctreeDepthFirstIterator; + friend class OctreeBreadthFirstIterator; + friend class OctreeFixedDepthIterator; + friend class OctreeLeafNodeDepthFirstIterator; + friend class OctreeLeafNodeBreadthFirstIterator; + + // Octree default iterators + using Iterator = OctreeDepthFirstIterator; + using ConstIterator = const OctreeDepthFirstIterator; + + Iterator + begin(unsigned int max_depth_arg = 0u) + { + return Iterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const Iterator + end() + { + return Iterator(this, 0, nullptr); + }; + + // Octree leaf node iterators + // The previous deprecated names + // LeafNodeIterator and ConstLeafNodeIterator are deprecated. + // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead. + using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; + + PCL_DEPRECATED("use leaf_depth_begin() instead") + LeafNodeIterator + leaf_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeIterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end() + { + return LeafNodeIterator(this, 0, nullptr); + }; + + // The currently valide names + using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeDepthFirstIterator = + const OctreeLeafNodeDepthFirstIterator; + + LeafNodeDepthFirstIterator + leaf_depth_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeDepthFirstIterator( + this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const LeafNodeDepthFirstIterator + leaf_depth_end() + { + return LeafNodeDepthFirstIterator(this, 0, nullptr); + }; + + // Octree depth-first iterators + using DepthFirstIterator = OctreeDepthFirstIterator; + using ConstDepthFirstIterator = const OctreeDepthFirstIterator; + + DepthFirstIterator + depth_begin(unsigned int max_depth_arg = 0u) + { + return DepthFirstIterator(this, + max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const DepthFirstIterator + depth_end() + { + return DepthFirstIterator(this, 0, nullptr); + }; + + // Octree breadth-first iterators + using BreadthFirstIterator = OctreeBreadthFirstIterator; + using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator; + + BreadthFirstIterator + breadth_begin(unsigned int max_depth_arg = 0u) + { + return BreadthFirstIterator(this, + max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const BreadthFirstIterator + breadth_end() + { + return BreadthFirstIterator(this, 0, nullptr); + }; + + // Octree breadth iterators at a given depth + using FixedDepthIterator = OctreeFixedDepthIterator; + using ConstFixedDepthIterator = const OctreeFixedDepthIterator; + + FixedDepthIterator + fixed_depth_begin(unsigned int fixed_depth_arg = 0u) + { + return FixedDepthIterator(this, fixed_depth_arg); + }; + + const FixedDepthIterator + fixed_depth_end() + { + return FixedDepthIterator(this, 0, nullptr); + }; + + // Octree leaf node iterators + using LeafNodeBreadthFirstIterator = OctreeLeafNodeBreadthFirstIterator; + using ConstLeafNodeBreadthFirstIterator = + const OctreeLeafNodeBreadthFirstIterator; + + LeafNodeBreadthFirstIterator + leaf_breadth_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeBreadthFirstIterator( + this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const LeafNodeBreadthFirstIterator + leaf_breadth_end() + { + return LeafNodeBreadthFirstIterator(this, 0, nullptr); + }; + + /** \brief Empty constructor. */ + OctreeBase(); + + /** \brief Empty deconstructor. */ + virtual ~OctreeBase(); + + /** \brief Copy constructor. */ + OctreeBase(const OctreeBase& source) + : leaf_count_(source.leaf_count_) + , branch_count_(source.branch_count_) + , root_node_(new (BranchNode)(*(source.root_node_))) + , depth_mask_(source.depth_mask_) + , octree_depth_(source.octree_depth_) + , dynamic_depth_enabled_(source.dynamic_depth_enabled_) + , max_key_(source.max_key_) + {} + + /** \brief Copy operator. */ + OctreeBase& + operator=(const OctreeBase& source) + { + leaf_count_ = source.leaf_count_; + branch_count_ = source.branch_count_; + root_node_ = new (BranchNode)(*(source.root_node_)); + depth_mask_ = source.depth_mask_; + max_key_ = source.max_key_; + octree_depth_ = source.octree_depth_; + return (*this); + } + + /** \brief Set the maximum amount of voxels per dimension. + * \param[in] max_voxel_index_arg maximum amount of voxels per dimension + */ + void + setMaxVoxelIndex(unsigned int max_voxel_index_arg); + + /** \brief Set the maximum depth of the octree. + * \param max_depth_arg: maximum depth of octree + */ + void + setTreeDepth(unsigned int max_depth_arg); + + /** \brief Get the maximum depth of the octree. + * \return depth_arg: maximum depth of octree + */ + unsigned int + getTreeDepth() const + { + return this->octree_depth_; + } + + /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \note If leaf node already exist, this method returns the existing node + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + * \return pointer to new leaf node container. + */ + LeafContainerT* + createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \note If leaf node already exist, this method returns the existing node + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + * \return pointer to leaf node container if found, null pointer otherwise. + */ + LeafContainerT* + findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \brief idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg, + * idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + * \return "true" if leaf node search is successful, otherwise it returns "false". + */ + bool + existLeaf(unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) const; + + /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. + */ + void + removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \brief Return the amount of existing leafs in the octree. + * \return amount of registered leaf nodes. + */ + std::size_t + getLeafCount() const + { + return leaf_count_; + } + + /** \brief Return the amount of existing branch nodes in the octree. + * \return amount of branch nodes. + */ + std::size_t + getBranchCount() const + { + return branch_count_; + } + + /** \brief Delete the octree structure and its leaf nodes. + */ + void + deleteTree(); + + /** \brief Serialize octree into a binary output vector describing its branch node + * structure. + * \param binary_tree_out_arg: reference to output vector for writing binary tree + * structure. + */ + void + serializeTree(std::vector& binary_tree_out_arg); + + /** \brief Serialize octree into a binary output vector describing its branch node + * structure and push all LeafContainerT elements stored in the octree to a vector. + * \param binary_tree_out_arg: reference to output vector for writing binary tree + * structure. + * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the + * octree + */ + void + serializeTree(std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg); + + /** \brief Outputs a vector of all LeafContainerT elements that are stored within the + * octree leaf nodes. + * \param leaf_container_vector_arg: pointers to LeafContainerT vector that receives a + * copy of all LeafContainerT objects in the octree. + */ + void + serializeLeafs(std::vector& leaf_container_vector_arg); + + /** \brief Deserialize a binary octree description vector and create a corresponding + * octree structure. Leaf nodes are initialized with getDataTByKey(..). + * \param binary_tree_input_arg: reference to input vector for reading binary tree + * structure. + */ + void + deserializeTree(std::vector& binary_tree_input_arg); + + /** \brief Deserialize a binary octree description and create a corresponding octree + * structure. Leaf nodes are initialized with LeafContainerT elements from the + * dataVector. + * \param binary_tree_input_arg: reference to input vector for reading binary tree + * structure. \param leaf_container_vector_arg: pointer to container vector. + */ + void + deserializeTree(std::vector& binary_tree_input_arg, + std::vector& leaf_container_vector_arg); + +protected: + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Protected octree methods based on octree keys + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Create a leaf node + * \param key_arg: octree key addressing a leaf node. + * \return pointer to leaf node + */ + LeafContainerT* + createLeaf(const OctreeKey& key_arg) + { + + LeafNode* leaf_node; + BranchNode* leaf_node_parent; + + createLeafRecursive(key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent); + + LeafContainerT* ret = leaf_node->getContainerPtr(); + + return ret; + } + + /** \brief Find leaf node + * \param key_arg: octree key addressing a leaf node. + * \return pointer to leaf node. If leaf node is not found, this pointer returns 0. + */ + LeafContainerT* + findLeaf(const OctreeKey& key_arg) const + { + LeafContainerT* result = nullptr; + findLeafRecursive(key_arg, depth_mask_, root_node_, result); + return result; + } + + /** \brief Check for existence of a leaf node in the octree + * \param key_arg: octree key addressing a leaf node. + * \return "true" if leaf node is found; "false" otherwise + */ + bool + existLeaf(const OctreeKey& key_arg) const + { + return (findLeaf(key_arg) != nullptr); + } + + /** \brief Remove leaf node from octree + * \param key_arg: octree key addressing a leaf node. + */ + void + removeLeaf(const OctreeKey& key_arg) + { + if (key_arg <= max_key_) + deleteLeafRecursive(key_arg, depth_mask_, root_node_); + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Branch node access functions + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Retrieve root node */ + OctreeNode* + getRootNode() const + { + return this->root_node_; + } + + /** \brief Check if branch is pointing to a particular child node + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return "true" if pointer to child node exists; "false" otherwise + */ + bool + branchHasChild(const BranchNode& branch_arg, unsigned char child_idx_arg) const + { + // test occupancyByte for child existence + return (branch_arg.getChildPtr(child_idx_arg) != nullptr); + } + + /** \brief Retrieve a child node pointer for child node at child_idx. + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer to octree child node class + */ + OctreeNode* + getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const + { + return branch_arg.getChildPtr(child_idx_arg); + } + + /** \brief Assign new child node to branch + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \param new_child_arg: pointer to new child node + */ + void + setBranchChildPtr(BranchNode& branch_arg, + unsigned char child_idx_arg, + OctreeNode* new_child_arg) + { + branch_arg[child_idx_arg] = new_child_arg; + } + + /** \brief Generate bit pattern reflecting the existence of child node pointers + * \param branch_arg: reference to octree branch class + * \return a single byte with 8 bits of child node information + */ + char + getBranchBitPattern(const BranchNode& branch_arg) const + { + char node_bits; + + // create bit pattern + node_bits = 0; + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* child = branch_arg.getChildPtr(i); + node_bits |= static_cast((!!child) << i); + } + + return (node_bits); + } + + /** \brief Delete child node and all its subchilds from octree + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + */ + void + deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + if (branch_arg.hasChild(child_idx_arg)) { + OctreeNode* branch_child = branch_arg[child_idx_arg]; + + switch (branch_child->getNodeType()) { + case BRANCH_NODE: { + // free child branch recursively + deleteBranch(*static_cast(branch_child)); + // delete branch node + delete branch_child; + } break; + + case LEAF_NODE: { + // delete leaf node + delete branch_child; + break; + } + default: + break; + } + + // set branch child pointer to 0 + branch_arg[child_idx_arg] = nullptr; + } + } + + /** \brief Delete branch and all its subchilds from octree + * \param branch_arg: reference to octree branch class + */ + void + deleteBranch(BranchNode& branch_arg) + { + // delete all branch node children + for (char i = 0; i < 8; i++) + deleteBranchChild(branch_arg, i); + } + + /** \brief Create and add a new branch child to a branch class + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer of new branch child to this reference + */ + BranchNode* + createBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + BranchNode* new_branch_child = new BranchNode(); + branch_arg[child_idx_arg] = static_cast(new_branch_child); + + return new_branch_child; + } + + /** \brief Create and add a new leaf child to a branch class + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer of new leaf child to this reference + */ + LeafNode* + createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + LeafNode* new_leaf_child = new LeafNode(); + branch_arg[child_idx_arg] = static_cast(new_leaf_child); + + return new_leaf_child; + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Recursive octree methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Create a leaf node at octree key. If leaf node does already exist, it is + * returned. + * \param key_arg: reference to an octree key + * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth + * indicator + * \param branch_arg: current branch node + * \param return_leaf_arg: return pointer to leaf node + * \param parent_of_leaf_arg: return pointer to parent of leaf node + * \return depth mask at which leaf node was created + **/ + unsigned int + createLeafRecursive(const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg); + + /** \brief Recursively search for a given leaf node and return a pointer. + * \note If leaf node does not exist, a 0 pointer is returned. + * \param key_arg: reference to an octree key + * \param depth_mask_arg: depth mask used for octree key analysis and for branch + * depth indicator + * \param branch_arg: current branch node + * \param result_arg: pointer to leaf node class + **/ + void + findLeafRecursive(const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const; + + /** \brief Recursively search and delete leaf node + * \param key_arg: reference to an octree key + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth + * indicator + * \param branch_arg: current branch node + * \return "true" if branch does not contain any childs; "false" otherwise. This + * indicates if current branch can be deleted, too. + **/ + bool + deleteLeafRecursive(const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg); + + /** \brief Recursively explore the octree and output binary octree description + * together with a vector of leaf node LeafContainerTs. + * \param branch_arg: current branch node + * \param key_arg: reference to an octree key + * \param binary_tree_out_arg: binary output vector + * \param leaf_container_vector_arg: writes LeafContainerT pointers to this + *LeafContainerT* vector. + **/ + void + serializeTreeRecursive( + const BranchNode* branch_arg, + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg) const; + + /** \brief Recursive method for deserializing octree structure + * \param branch_arg: current branch node + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth + * indicator + * \param key_arg: reference to an octree key + * \param binary_tree_input_it_arg: iterator to binary input vector + * \param binary_tree_input_it_end_arg: end iterator of binary input vector + * \param leaf_container_vector_it_arg: iterator pointing to current LeafContainerT + * object to be added to a leaf node + * \param leaf_container_vector_it_end_arg: iterator pointing to last object in + * LeafContainerT input vector. + **/ + void + deserializeTreeRecursive( + BranchNode* branch_arg, + unsigned int depth_mask_arg, + OctreeKey& key_arg, + typename std::vector::const_iterator& binary_tree_input_it_arg, + typename std::vector::const_iterator& binary_tree_input_it_end_arg, + typename std::vector::const_iterator* + leaf_container_vector_it_arg, + typename std::vector::const_iterator* + leaf_container_vector_it_end_arg); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Serialization callbacks + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Callback executed for every leaf node during serialization + **/ + virtual void + serializeTreeCallback(LeafContainerT&, const OctreeKey&) const + {} + + /** \brief Callback executed for every leaf node during deserialization + **/ + virtual void + deserializeTreeCallback(LeafContainerT&, const OctreeKey&) + {} + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Helpers + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Helper function to calculate the binary logarithm + * \param n_arg: some value + * \return binary logarithm (log2) of argument n_arg + */ + PCL_DEPRECATED("use std::log2 instead") double Log2(double n_arg) + { + return std::log2(n_arg); + } + + /** \brief Test if octree is able to dynamically change its depth. This is required + *for adaptive bounding box adjustment. + * \return "true" + **/ + bool + octreeCanResize() + { + return (true); + } +}; +} // namespace octree +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_container.h b/deps_install/include/pcl-1.10/pcl/octree/octree_container.h new file mode 100755 index 0000000..8323800 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_container.h @@ -0,0 +1,336 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: octree_nodes.h 5596 2012-04-17 15:09:31Z jkammerl $ + */ + +#pragma once + +#include +#include + +#include + +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that can serve as a base to construct own leaf node + * container classes. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreeContainerBase { +public: + virtual ~OctreeContainerBase() = default; + + /** \brief Equal comparison operator + */ + virtual bool + operator==(const OctreeContainerBase&) const + { + return false; + } + + /** \brief Inequal comparison operator + * \param[in] other OctreeContainerBase to compare with + */ + bool + operator!=(const OctreeContainerBase& other) const + { + return (!operator==(other)); + } + + /** \brief Pure abstract method to get size of container (number of indices) + * \return number of points/indices stored in leaf node container. + */ + virtual std::size_t + getSize() const + { + return 0u; + } + + /** \brief Pure abstract reset leaf node implementation. */ + virtual void + reset() = 0; + + /** \brief Empty addPointIndex implementation. This leaf node does not store any point + * indices. + */ + void + addPointIndex(const int&) + {} + + /** \brief Empty getPointIndex implementation as this leaf node does not store any + * point indices. + */ + void + getPointIndex(int&) const + {} + + /** \brief Empty getPointIndices implementation as this leaf node does not store any + * data. \ + */ + void + getPointIndices(std::vector&) const + {} +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that does not store any information. + * \note Can be used for occupancy trees that are used for checking only the existence + * of leaf nodes in the tree + * \author Julius Kammerl (julius@kammerl.de) + */ + +class OctreeContainerEmpty : public OctreeContainerBase { +public: + /** \brief Octree deep copy method */ + virtual OctreeContainerEmpty* + deepCopy() const + { + return (new OctreeContainerEmpty(*this)); + } + + /** \brief Abstract get size of container (number of DataT objects) + * \return number of DataT elements in leaf node container. + */ + std::size_t + getSize() const override + { + return 0; + } + + /** \brief Abstract reset leaf node implementation. */ + void + reset() override + {} + + /** \brief Empty addPointIndex implementation. This leaf node does not store any point + * indices. + */ + void + addPointIndex(int) + {} + + /** \brief Empty getPointIndex implementation as this leaf node does not store any + * point indices. + */ + int + getPointIndex() const + { + assert("getPointIndex: undefined point index"); + return -1; + } + + /** \brief Empty getPointIndices implementation as this leaf node does not store any + * data. + */ + void + getPointIndices(std::vector&) const + {} +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that does store a single point index. + * \note Enables the octree to store a single DataT element within its leaf nodes. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreeContainerPointIndex : public OctreeContainerBase { +public: + /** \brief Empty constructor. */ + OctreeContainerPointIndex() { reset(); } + + /** \brief Octree deep copy method */ + virtual OctreeContainerPointIndex* + deepCopy() const + { + return (new OctreeContainerPointIndex(*this)); + } + + /** \brief Equal comparison operator + * \param[in] other OctreeContainerBase to compare with + */ + bool + operator==(const OctreeContainerBase& other) const override + { + const OctreeContainerPointIndex* otherConDataT = + dynamic_cast(&other); + + return (this->data_ == otherConDataT->data_); + } + + /** \brief Add point index to container memory. This container stores a only a single + * point index. + * \param[in] data_arg index to be stored within leaf node. + */ + void + addPointIndex(int data_arg) + { + data_ = data_arg; + } + + /** \brief Retrieve point index from container. This container stores a only a single + * point index + * \return index stored within container. + */ + int + getPointIndex() const + { + return data_; + } + + /** \brief Retrieve point indices from container. This container stores only a single + * point index + * \param[out] data_vector_arg vector of point indices to be stored within + * data vector + */ + void + getPointIndices(std::vector& data_vector_arg) const + { + if (data_ >= 0) + data_vector_arg.push_back(data_); + } + + /** \brief Get size of container (number of DataT objects) + * \return number of DataT elements in leaf node container. + */ + std::size_t + getSize() const override + { + return data_ < 0 ? 0 : 1; + } + + /** \brief Reset leaf node memory to zero. */ + void + reset() override + { + data_ = -1; + } + +protected: + /** \brief Point index stored in octree. */ + int data_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that does store a vector of point indices. + * \note Enables the octree to store multiple DataT elements within its leaf nodes. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreeContainerPointIndices : public OctreeContainerBase { +public: + /** \brief Octree deep copy method */ + virtual OctreeContainerPointIndices* + deepCopy() const + { + return (new OctreeContainerPointIndices(*this)); + } + + /** \brief Equal comparison operator + * \param[in] other OctreeContainerDataTVector to compare with + */ + bool + operator==(const OctreeContainerBase& other) const override + { + const OctreeContainerPointIndices* otherConDataTVec = + dynamic_cast(&other); + + return (this->leafDataTVector_ == otherConDataTVec->leafDataTVector_); + } + + /** \brief Add point index to container memory. This container stores a vector of + * point indices. + * \param[in] data_arg index to be stored within leaf node. + */ + void + addPointIndex(int data_arg) + { + leafDataTVector_.push_back(data_arg); + } + + /** \brief Retrieve point index from container. This container stores a vector of + * point indices. + * \return index stored within container. + */ + int + getPointIndex() const + { + return leafDataTVector_.back(); + } + + /** \brief Retrieve point indices from container. This container stores a vector of + * point indices. + * \param[out] data_vector_arg vector of point indices to be stored + * within data vector + */ + void + getPointIndices(std::vector& data_vector_arg) const + { + data_vector_arg.insert( + data_vector_arg.end(), leafDataTVector_.begin(), leafDataTVector_.end()); + } + + /** \brief Retrieve reference to point indices vector. This container stores a vector + * of point indices. + * \return reference to vector of point indices to be stored within data vector + */ + std::vector& + getPointIndicesVector() + { + return leafDataTVector_; + } + + /** \brief Get size of container (number of indices) + * \return number of point indices in container. + */ + std::size_t + getSize() const override + { + return leafDataTVector_.size(); + } + + /** \brief Reset leaf node. Clear DataT vector.*/ + void + reset() override + { + leafDataTVector_.clear(); + } + +protected: + /** \brief Leaf node DataT vector. */ + std::vector leafDataTVector_; +}; + +} // namespace octree +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_impl.h b/deps_install/include/pcl-1.10/pcl/octree/octree_impl.h new file mode 100755 index 0000000..c8fa320 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_impl.h @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_iterator.h b/deps_install/include/pcl-1.10/pcl/octree/octree_iterator.h new file mode 100755 index 0000000..2d20cf2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_iterator.h @@ -0,0 +1,810 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2017-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include +#include + +#include +#include + +#include + +// Ignore warnings in the above headers +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +namespace pcl { +namespace octree { + +// Octree iterator state pushed on stack/list +struct IteratorState { + OctreeNode* node_; + OctreeKey key_; + unsigned int depth_; +}; + +/** \brief @b Abstract octree iterator class + * \note Octree iterator base class + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeIteratorBase : public std::iterator { +public: + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + using LeafContainer = typename OctreeT::LeafContainer; + using BranchContainer = typename OctreeT::BranchContainer; + + /** \brief Empty constructor. + */ + explicit OctreeIteratorBase(unsigned int max_depth_arg = 0) + : octree_(0), current_state_(0), max_octree_depth_(max_depth_arg) + { + this->reset(); + } + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeIteratorBase(OctreeT* octree_arg, unsigned int max_depth_arg = 0) + : octree_(octree_arg), current_state_(0), max_octree_depth_(max_depth_arg) + { + this->reset(); + } + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeIteratorBase(OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state) + : octree_(octree_arg), current_state_(current_state), max_octree_depth_(max_depth_arg) + {} + + /** \brief Empty deconstructor. */ + virtual ~OctreeIteratorBase() {} + + /** \brief Equal comparison operator + * \param[in] other OctreeIteratorBase to compare with + */ + bool + operator==(const OctreeIteratorBase& other) const + { + if (this == &other) // same object + return true; + if (octree_ != other.octree_) // refer to different octrees + return false; + if (!current_state_ && !other.current_state_) // both are end iterators + return true; + if (max_octree_depth_ == other.max_octree_depth_ && current_state_ && + other.current_state_ && // null dereference protection + current_state_->key_ == other.current_state_->key_) + return true; + return false; + } + + /** \brief Inequal comparison operator + * \param[in] other OctreeIteratorBase to compare with + */ + bool + operator!=(const OctreeIteratorBase& other) const + { + return !operator==(other); + } + + /** \brief Reset iterator */ + inline void + reset() + { + current_state_ = 0; + if (octree_ && (!max_octree_depth_)) { + max_octree_depth_ = octree_->getTreeDepth(); + } + } + + /** \brief Get octree key for the current iterator octree node + * \return octree key of current node + */ + inline const OctreeKey& + getCurrentOctreeKey() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->key_); + } + + /** \brief Get the current depth level of octree + * \return depth level + */ + inline unsigned int + getCurrentOctreeDepth() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->depth_); + } + + /** \brief Get the current octree node + * \return pointer to current octree node + */ + inline OctreeNode* + getCurrentOctreeNode() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->node_); + } + + /** \brief check if current node is a branch node + * \return true if current node is a branch node, false otherwise + */ + inline bool + isBranchNode() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->node_->getNodeType() == BRANCH_NODE); + } + + /** \brief check if current node is a branch node + * \return true if current node is a branch node, false otherwise + */ + inline bool + isLeafNode() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->node_->getNodeType() == LEAF_NODE); + } + + /** \brief *operator. + * \return pointer to the current octree node + */ + inline OctreeNode* operator*() const + { // return designated object + if (octree_ && current_state_) { + return (current_state_->node_); + } + else { + return 0; + } + } + + /** \brief Get bit pattern of children configuration of current node + * \return bit pattern (byte) describing the existence of 8 children of the current + * node + */ + inline char + getNodeConfiguration() const + { + char ret = 0; + + assert(octree_ != 0); + assert(current_state_ != 0); + + if (isBranchNode()) { + + // current node is a branch node + const BranchNode* current_branch = + static_cast(current_state_->node_); + + // get child configuration bit pattern + ret = octree_->getBranchBitPattern(*current_branch); + } + + return (ret); + } + + /** \brief Method for retrieving a single leaf container from the octree leaf node + * \return Reference to container class of leaf node. + */ + const LeafContainer& + getLeafContainer() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isLeafNode()); + + LeafNode* leaf_node = static_cast(current_state_->node_); + + return leaf_node->getContainer(); + } + + /** \brief Method for retrieving a single leaf container from the octree leaf node + * \return Reference to container class of leaf node. + */ + LeafContainer& + getLeafContainer() + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isLeafNode()); + + LeafNode* leaf_node = static_cast(current_state_->node_); + + return leaf_node->getContainer(); + } + + /** \brief Method for retrieving the container from an octree branch node + * \return BranchContainer. + */ + const BranchContainer& + getBranchContainer() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isBranchNode()); + + BranchNode* branch_node = static_cast(current_state_->node_); + + return branch_node->getContainer(); + } + + /** \brief Method for retrieving the container from an octree branch node + * \return BranchContainer. + */ + BranchContainer& + getBranchContainer() + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isBranchNode()); + + BranchNode* branch_node = static_cast(current_state_->node_); + + return branch_node->getContainer(); + } + + /** \brief get a integer identifier for current node (note: identifier depends on tree + * depth). \return node id. + */ + virtual unsigned long + getNodeID() const + { + unsigned long id = 0; + + assert(octree_ != 0); + assert(current_state_ != 0); + + if (current_state_) { + const OctreeKey& key = getCurrentOctreeKey(); + // calculate integer id with respect to octree key + unsigned int depth = octree_->getTreeDepth(); + id = static_cast(key.x) << (depth * 2) | + static_cast(key.y) << (depth * 1) | + static_cast(key.z) << (depth * 0); + } + + return id; + } + +protected: + /** \brief Reference to octree class. */ + OctreeT* octree_; + + /** \brief Pointer to current iterator state. */ + IteratorState* current_state_; + + /** \brief Maximum octree depth */ + unsigned int max_octree_depth_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree iterator class + * \note This class implements a forward iterator for traversing octrees in a + * depth-first manner. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeDepthFirstIterator : public OctreeIteratorBase { + +public: + using LeafNode = typename OctreeIteratorBase::LeafNode; + using BranchNode = typename OctreeIteratorBase::BranchNode; + + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeDepthFirstIterator(unsigned int max_depth_arg = 0); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeDepthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg = 0); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeDepthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::vector& stack = std::vector()) + : OctreeIteratorBase(octree_arg, max_depth_arg, current_state), stack_(stack) + {} + + /** \brief Copy Constructor. + * \param[in] other Another OctreeDepthFirstIterator to copy from + */ + OctreeDepthFirstIterator(const OctreeDepthFirstIterator& other) + : OctreeIteratorBase(other), stack_(other.stack_) + { + this->current_state_ = stack_.size() ? &stack_.back() : NULL; + } + + /** \brief Copy assignment + * \param[in] src the iterator to copy into this + */ + inline OctreeDepthFirstIterator& + operator=(const OctreeDepthFirstIterator& src) + { + + OctreeIteratorBase::operator=(src); + + stack_ = src.stack_; + + if (stack_.size()) { + this->current_state_ = &stack_.back(); + } + else { + this->current_state_ = 0; + } + + return (*this); + } + + /** \brief Reset the iterator to the root node of the octree + */ + virtual void + reset(); + + /** \brief Preincrement operator. + * \note recursively step to next octree node + */ + OctreeDepthFirstIterator& + operator++(); + + /** \brief postincrement operator. + * \note recursively step to next octree node + */ + inline OctreeDepthFirstIterator + operator++(int) + { + OctreeDepthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); + } + + /** \brief Skip all child voxels of current node and return to parent node. + */ + void + skipChildVoxels(); + +protected: + /** Stack structure. */ + std::vector stack_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree iterator class + * \note This class implements a forward iterator for traversing octrees in a + * breadth-first manner. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeBreadthFirstIterator : public OctreeIteratorBase { +public: + // public typedefs + using BranchNode = typename OctreeIteratorBase::BranchNode; + using LeafNode = typename OctreeIteratorBase::LeafNode; + + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeBreadthFirstIterator(unsigned int max_depth_arg = 0); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeBreadthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg = 0); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeBreadthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::deque& fifo = std::deque()) + : OctreeIteratorBase(octree_arg, max_depth_arg, current_state), FIFO_(fifo) + {} + + /** \brief Copy Constructor. + * \param[in] other Another OctreeBreadthFirstIterator to copy from + */ + OctreeBreadthFirstIterator(const OctreeBreadthFirstIterator& other) + : OctreeIteratorBase(other), FIFO_(other.FIFO_) + { + this->current_state_ = FIFO_.size() ? &FIFO_.front() : NULL; + } + + /** \brief Copy operator. + * \param[in] src the iterator to copy into this + */ + inline OctreeBreadthFirstIterator& + operator=(const OctreeBreadthFirstIterator& src) + { + + OctreeIteratorBase::operator=(src); + + FIFO_ = src.FIFO_; + + if (FIFO_.size()) { + this->current_state_ = &FIFO_.front(); + } + else { + this->current_state_ = 0; + } + + return (*this); + } + + /** \brief Reset the iterator to the root node of the octree + */ + void + reset(); + + /** \brief Preincrement operator. + * \note step to next octree node + */ + OctreeBreadthFirstIterator& + operator++(); + + /** \brief postincrement operator. + * \note step to next octree node + */ + inline OctreeBreadthFirstIterator + operator++(int) + { + OctreeBreadthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); + } + +protected: + /** FIFO list */ + std::deque FIFO_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree iterator class + * \note Iterator over all existing nodes at a given depth. It walks across an octree + * in a breadth-first manner. + * \ingroup octree + * \author Fabien Rozar (fabien.rozar@gmail.com) + */ +template +class OctreeFixedDepthIterator : public OctreeBreadthFirstIterator { +public: + // public typedefs + using typename OctreeBreadthFirstIterator::BranchNode; + using typename OctreeBreadthFirstIterator::LeafNode; + + /** \brief Empty constructor. + */ + OctreeFixedDepthIterator(); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] fixed_depth_arg Depth level during traversal + */ + explicit OctreeFixedDepthIterator(OctreeT* octree_arg, + unsigned int fixed_depth_arg = 0); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] fixed_depth_arg Depth level during traversal + * \param[in] current_state A pointer to the current iterator state + * \param[in] fifo Internal container of octree node to go through + * + * \warning For advanced users only. + */ + OctreeFixedDepthIterator( + OctreeT* octree_arg, + unsigned int fixed_depth_arg, + IteratorState* current_state, + const std::deque& fifo = std::deque()) + : OctreeBreadthFirstIterator( + octree_arg, fixed_depth_arg, current_state, fifo) + , fixed_depth_(fixed_depth_arg) + {} + + /** \brief Copy Constructor. + * \param[in] other Another OctreeFixedDepthIterator to copy from + */ + OctreeFixedDepthIterator(const OctreeFixedDepthIterator& other) + : OctreeBreadthFirstIterator(other) + { + this->fixed_depth_ = other.fixed_depth_; + } + + /** \brief Copy assignment. + * \param[in] src the iterator to copy into this + * \return pointer to the current octree node + */ + inline OctreeFixedDepthIterator& + operator=(const OctreeFixedDepthIterator& src) + { + OctreeBreadthFirstIterator::operator=(src); + this->fixed_depth_ = src.fixed_depth_; + + return (*this); + } + + /** \brief Reset the iterator to the first node at the depth given as parameter + * \param[in] fixed_depth_arg Depth level during traversal + */ + void + reset(unsigned int fixed_depth_arg); + + /** \brief Reset the iterator to the first node at the current depth + */ + void + reset() + { + this->reset(fixed_depth_); + } + +protected: + using OctreeBreadthFirstIterator::FIFO_; + + /** \brief Given level of the node to be iterated */ + unsigned int fixed_depth_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Octree leaf node iterator class + * \note This class implements a forward iterator for traversing the leaf nodes of an + * octree data structure. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OctreeLeafNodeDepthFirstIterator : public OctreeDepthFirstIterator { + using BranchNode = typename OctreeDepthFirstIterator::BranchNode; + using LeafNode = typename OctreeDepthFirstIterator::LeafNode; + +public: + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeLeafNodeDepthFirstIterator(unsigned int max_depth_arg = 0) + : OctreeDepthFirstIterator(max_depth_arg) + { + reset(); + } + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeLeafNodeDepthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg = 0) + : OctreeDepthFirstIterator(octree_arg, max_depth_arg) + { + reset(); + } + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeLeafNodeDepthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::vector& stack = std::vector()) + : OctreeDepthFirstIterator(octree_arg, max_depth_arg, current_state, stack) + {} + + /** \brief Reset the iterator to the root node of the octree + */ + inline void + reset() + { + OctreeDepthFirstIterator::reset(); + this->operator++(); + } + + /** \brief Preincrement operator. + * \note recursively step to next octree leaf node + */ + inline OctreeLeafNodeDepthFirstIterator& + operator++() + { + do { + OctreeDepthFirstIterator::operator++(); + } while ((this->current_state_) && + (this->current_state_->node_->getNodeType() != LEAF_NODE)); + + return (*this); + } + + /** \brief postincrement operator. + * \note step to next octree node + */ + inline OctreeLeafNodeDepthFirstIterator + operator++(int) + { + OctreeLeafNodeDepthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); + } + + /** \brief *operator. + * \return pointer to the current octree leaf node + */ + OctreeNode* operator*() const + { + // return designated object + OctreeNode* ret = 0; + + if (this->current_state_ && + (this->current_state_->node_->getNodeType() == LEAF_NODE)) + ret = this->current_state_->node_; + return (ret); + } +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Octree leaf node iterator class + * \note This class implements a forward iterator for traversing the leaf nodes of an + * octree data structure in the breadth first way. + * \ingroup octree + * \author Fabien Rozar + * (fabien.rozar@gmail.com) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OctreeLeafNodeBreadthFirstIterator : public OctreeBreadthFirstIterator { + using BranchNode = typename OctreeBreadthFirstIterator::BranchNode; + using LeafNode = typename OctreeBreadthFirstIterator::LeafNode; + +public: + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeLeafNodeBreadthFirstIterator(unsigned int max_depth_arg = 0); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeLeafNodeBreadthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg = 0); + + /** \brief Copy constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] max_depth_arg Depth limitation during traversal + * \param[in] current_state A pointer to the current iterator state + * \param[in] fifo Internal container of octree node to go through + * + * \warning For advanced users only. + */ + explicit OctreeLeafNodeBreadthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::deque& fifo = std::deque()); + + /** \brief Reset the iterator to the first leaf in the breadth first way. + */ + inline void + reset(); + + /** \brief Preincrement operator. + * \note recursively step to next octree leaf node + */ + inline OctreeLeafNodeBreadthFirstIterator& + operator++(); + + /** \brief Postincrement operator. + * \note step to next octree node + */ + inline OctreeLeafNodeBreadthFirstIterator + operator++(int); +}; + +} // namespace octree +} // namespace pcl + +/* + * Note: Since octree iterators depend on octrees, don't precompile them. + */ +#include diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_key.h b/deps_install/include/pcl-1.10/pcl/octree/octree_key.h new file mode 100755 index 0000000..5c75ad5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_key.h @@ -0,0 +1,157 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree key class + * \note Octree keys contain integer indices for each coordinate axis in order to + * address an octree leaf node. + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +class OctreeKey { +public: + /** \brief Empty constructor. */ + OctreeKey() : x(0), y(0), z(0) {} + + /** \brief Constructor for key initialization. */ + OctreeKey(unsigned int keyX, unsigned int keyY, unsigned int keyZ) + : x(keyX), y(keyY), z(keyZ) + {} + + /** \brief Copy constructor. */ + OctreeKey(const OctreeKey& source) { memcpy(key_, source.key_, sizeof(key_)); } + + OctreeKey& + operator=(const OctreeKey&) = default; + + /** \brief Operator== for comparing octree keys with each other. + * \return "true" if leaf node indices are identical; "false" otherwise. + * */ + bool + operator==(const OctreeKey& b) const + { + return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z)); + } + + /** \brief Inequal comparison operator + * \param[in] other OctreeIteratorBase to compare with + * \return "true" if the current and other iterators are different ; "false" + * otherwise. + */ + bool + operator!=(const OctreeKey& other) const + { + return !operator==(other); + } + + /** \brief Operator<= for comparing octree keys with each other. + * \return "true" if key indices are not greater than the key indices of b ; "false" + * otherwise. + * */ + bool + operator<=(const OctreeKey& b) const + { + return ((b.x >= this->x) && (b.y >= this->y) && (b.z >= this->z)); + } + + /** \brief Operator>= for comparing octree keys with each other. + * \return "true" if key indices are not smaller than the key indices of b ; "false" + * otherwise. + * */ + bool + operator>=(const OctreeKey& b) const + { + return ((b.x <= this->x) && (b.y <= this->y) && (b.z <= this->z)); + } + + /** \brief push a child node to the octree key + * \param[in] childIndex index of child node to be added (0-7) + * */ + inline void + pushBranch(unsigned char childIndex) + { + this->x = (this->x << 1) | (!!(childIndex & (1 << 2))); + this->y = (this->y << 1) | (!!(childIndex & (1 << 1))); + this->z = (this->z << 1) | (!!(childIndex & (1 << 0))); + } + + /** \brief pop child node from octree key + * */ + inline void + popBranch() + { + this->x >>= 1; + this->y >>= 1; + this->z >>= 1; + } + + /** \brief get child node index using depthMask + * \param[in] depthMask bit mask with single bit set at query depth + * \return child node index + * */ + inline unsigned char + getChildIdxWithDepthMask(unsigned int depthMask) const + { + return static_cast(((!!(this->x & depthMask)) << 2) | + ((!!(this->y & depthMask)) << 1) | + (!!(this->z & depthMask))); + } + + /* \brief maximum depth that can be addressed */ + static const unsigned char maxDepth = + static_cast(sizeof(std::uint32_t) * 8); + + // Indices addressing a voxel at (X, Y, Z) + + union { + struct { + std::uint32_t x; + std::uint32_t y; + std::uint32_t z; + }; + std::uint32_t key_[3]; + }; +}; +} // namespace octree +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_nodes.h b/deps_install/include/pcl-1.10/pcl/octree/octree_nodes.h new file mode 100755 index 0000000..0277fc9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_nodes.h @@ -0,0 +1,335 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include + +#include + +#include + +#include "octree_container.h" + +namespace pcl { +namespace octree { + +// enum of node types within the octree +enum node_type_t { BRANCH_NODE, LEAF_NODE }; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Abstract octree node class + * \note Every octree node should implement the getNodeType () method + * \author Julius Kammerl (julius@kammerl.de) + */ +class PCL_EXPORTS OctreeNode { +public: + OctreeNode() {} + + virtual ~OctreeNode() {} + /** \brief Pure virtual method for receiving the type of octree node (branch or leaf) + */ + virtual node_type_t + getNodeType() const = 0; + + /** \brief Pure virtual method to perform a deep copy of the octree */ + virtual OctreeNode* + deepCopy() const = 0; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Abstract octree leaf class + * \note Octree leafs may collect data of type DataT + * \author Julius Kammerl (julius@kammerl.de) + */ + +template +class OctreeLeafNode : public OctreeNode { +public: + /** \brief Empty constructor. */ + OctreeLeafNode() : OctreeNode() {} + + /** \brief Copy constructor. */ + OctreeLeafNode(const OctreeLeafNode& source) : OctreeNode() + { + container_ = source.container_; + } + + /** \brief Empty deconstructor. */ + + ~OctreeLeafNode() {} + + /** \brief Method to perform a deep copy of the octree */ + OctreeLeafNode* + deepCopy() const override + { + return new OctreeLeafNode(*this); + } + + /** \brief Get the type of octree node. Returns LEAVE_NODE type */ + node_type_t + getNodeType() const override + { + return LEAF_NODE; + } + + /** \brief Get const pointer to container */ + const ContainerT* operator->() const { return &container_; } + + /** \brief Get pointer to container */ + ContainerT* operator->() { return &container_; } + + /** \brief Get const reference to container */ + const ContainerT& operator*() const { return container_; } + + /** \brief Get reference to container */ + ContainerT& operator*() { return container_; } + + /** \brief Get const reference to container */ + const ContainerT& + getContainer() const + { + return container_; + } + + /** \brief Get reference to container */ + ContainerT& + getContainer() + { + return container_; + } + + /** \brief Get const pointer to container */ + const ContainerT* + getContainerPtr() const + { + return &container_; + } + + /** \brief Get pointer to container */ + ContainerT* + getContainerPtr() + { + return &container_; + } + +protected: + ContainerT container_; + +public: + // Type ContainerT may have fixed-size Eigen objects inside + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Abstract octree branch class + * \note Octree branch classes may collect data of type DataT + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeBranchNode : public OctreeNode { +public: + /** \brief Empty constructor. */ + OctreeBranchNode() : OctreeNode() + { + // reset pointer to child node vectors + memset(child_node_array_, 0, sizeof(child_node_array_)); + } + + /** \brief Empty constructor. */ + OctreeBranchNode(const OctreeBranchNode& source) : OctreeNode() + { + memset(child_node_array_, 0, sizeof(child_node_array_)); + + for (unsigned char i = 0; i < 8; ++i) + if (source.child_node_array_[i]) + child_node_array_[i] = source.child_node_array_[i]->deepCopy(); + } + + /** \brief Copy operator. */ + inline OctreeBranchNode& + operator=(const OctreeBranchNode& source) + { + memset(child_node_array_, 0, sizeof(child_node_array_)); + + for (unsigned char i = 0; i < 8; ++i) + if (source.child_node_array_[i]) + child_node_array_[i] = source.child_node_array_[i]->deepCopy(); + return (*this); + } + + /** \brief Octree deep copy method */ + OctreeBranchNode* + deepCopy() const override + { + return (new OctreeBranchNode(*this)); + } + + /** \brief Empty deconstructor. */ + + ~OctreeBranchNode() {} + + /** \brief Access operator. + * \param child_idx_arg: index to child node + * \return OctreeNode pointer + * */ + inline OctreeNode*& operator[](unsigned char child_idx_arg) + { + assert(child_idx_arg < 8); + return child_node_array_[child_idx_arg]; + } + + /** \brief Get pointer to child + * \param child_idx_arg: index to child node + * \return OctreeNode pointer + * */ + inline OctreeNode* + getChildPtr(unsigned char child_idx_arg) const + { + assert(child_idx_arg < 8); + return child_node_array_[child_idx_arg]; + } + + /** \brief Get pointer to child + * \return OctreeNode pointer + * */ + inline void + setChildPtr(OctreeNode* child, unsigned char index) + { + assert(index < 8); + child_node_array_[index] = child; + } + + /** \brief Check if branch is pointing to a particular child node + * \param child_idx_arg: index to child node + * \return "true" if pointer to child node exists; "false" otherwise + * */ + inline bool + hasChild(unsigned char child_idx_arg) const + { + return (child_node_array_[child_idx_arg] != nullptr); + } + + /** \brief Check if branch can be pruned + * \note if all children are leaf nodes AND contain identical containers, branch can + * be pruned + * \return "true" if branch can be pruned; "false" otherwise + **/ + /* inline bool isPrunable () const + { + const OctreeNode* firstChild = child_node_array_[0]; + if (!firstChild || firstChild->getNodeType()==BRANCH_NODE) + return false; + + bool prunable = true; + for (unsigned char i = 1; i < 8 && prunable; ++i) + { + const OctreeNode* child = child_node_array_[i]; + if ( (!child) || + (child->getNodeType()==BRANCH_NODE) || + ((*static_cast(child)) == (*static_cast(child)) ) ) prunable = false; + } + + return prunable; + }*/ + + /** \brief Get the type of octree node. Returns LEAVE_NODE type */ + node_type_t + getNodeType() const override + { + return BRANCH_NODE; + } + + // reset node + void + reset() + { + memset(child_node_array_, 0, sizeof(child_node_array_)); + container_.reset(); + } + + /** \brief Get const pointer to container */ + const ContainerT* operator->() const { return &container_; } + + /** \brief Get pointer to container */ + ContainerT* operator->() { return &container_; } + + /** \brief Get const reference to container */ + const ContainerT& operator*() const { return container_; } + + /** \brief Get reference to container */ + ContainerT& operator*() { return container_; } + + /** \brief Get const reference to container */ + const ContainerT& + getContainer() const + { + return container_; + } + + /** \brief Get reference to container */ + ContainerT& + getContainer() + { + return container_; + } + + /** \brief Get const pointer to container */ + const ContainerT* + getContainerPtr() const + { + return &container_; + } + + /** \brief Get pointer to container */ + ContainerT* + getContainerPtr() + { + return &container_; + } + +protected: + OctreeNode* child_node_array_[8]; + + ContainerT container_; +}; +} // namespace octree +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud.h new file mode 100755 index 0000000..929c3c4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud.h @@ -0,0 +1,608 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include +#include + +#include + +namespace pcl { +namespace octree { + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud class + * \note Octree implementation for pointclouds. Only indices are stored by the octree + * leaf nodes (zero-copy). + * \note The octree pointcloud class needs to be initialized with its voxel resolution. + * Its bounding box is automatically adjusted + * \note according to the pointcloud dimension or it can be predefined. + * \note Note: The tree depth equates to the resolution and the bounding box dimensions + * of the octree. + * \tparam PointT: type of point used in pointcloud + * \tparam LeafContainerT: leaf node container + * \tparam BranchContainerT: branch node container + * \tparam OctreeT: octree implementation + * \ingroup octree + * \author Julius Kammerl * (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template > + +class OctreePointCloud : public OctreeT { +public: + using Base = OctreeT; + + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + /** \brief Octree pointcloud constructor. + * \param[in] resolution_arg octree resolution at lowest octree level + */ + OctreePointCloud(const double resolution_arg); + + // public typedefs + using IndicesPtr = shared_ptr>; + using IndicesConstPtr = shared_ptr>; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + // public typedefs for single/double buffering + using SingleBuffer = OctreePointCloud>; + // using DoubleBuffer = OctreePointCloud >; + + // Boost shared pointers + using Ptr = + shared_ptr>; + using ConstPtr = shared_ptr< + const OctreePointCloud>; + + // Eigen aligned allocator + using AlignedPointTVector = std::vector>; + using AlignedPointXYZVector = + std::vector>; + + /** \brief Provide a pointer to the input data set. + * \param[in] cloud_arg the const boost shared pointer to a PointCloud message + * \param[in] indices_arg the point indices subset that is to be used from \a cloud - + * if 0 the whole point cloud is used + */ + inline void + setInputCloud(const PointCloudConstPtr& cloud_arg, + const IndicesConstPtr& indices_arg = IndicesConstPtr()) + { + input_ = cloud_arg; + indices_ = indices_arg; + } + + /** \brief Get a pointer to the vector of indices used. + * \return pointer to vector of indices used. + */ + inline IndicesConstPtr const + getIndices() const + { + return (indices_); + } + + /** \brief Get a pointer to the input point cloud dataset. + * \return pointer to pointcloud input class. + */ + inline PointCloudConstPtr + getInputCloud() const + { + return (input_); + } + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors + * searches. + * \param[in] eps precision (error bound) for nearest neighbors searches + */ + inline void + setEpsilon(double eps) + { + epsilon_ = eps; + } + + /** \brief Get the search epsilon precision (error bound) for nearest neighbors + * searches. */ + inline double + getEpsilon() const + { + return (epsilon_); + } + + /** \brief Set/change the octree voxel resolution + * \param[in] resolution_arg side length of voxels at lowest tree level + */ + inline void + setResolution(double resolution_arg) + { + // octree needs to be empty to change its resolution + assert(this->leaf_count_ == 0); + + resolution_ = resolution_arg; + + getKeyBitSize(); + } + + /** \brief Get octree voxel resolution + * \return voxel resolution at lowest tree level + */ + inline double + getResolution() const + { + return (resolution_); + } + + /** \brief Get the maximum depth of the octree. + * \return depth_arg: maximum depth of octree + * */ + inline unsigned int + getTreeDepth() const + { + return this->octree_depth_; + } + + /** \brief Add points from input point cloud to octree. */ + void + addPointsFromInputCloud(); + + /** \brief Add point at given index from input point cloud to octree. Index will be + * also added to indices vector. + * \param[in] point_idx_arg index of point to be added + * \param[in] indices_arg pointer to indices vector of the dataset (given by \a + * setInputCloud) + */ + void + addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg); + + /** \brief Add point simultaneously to octree and input point cloud. + * \param[in] point_arg point to be added + * \param[in] cloud_arg pointer to input point cloud dataset (given by \a + * setInputCloud) + */ + void + addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg); + + /** \brief Add point simultaneously to octree and input point cloud. A corresponding + * index will be added to the indices vector. + * \param[in] point_arg point to be added + * \param[in] cloud_arg pointer to input point cloud dataset (given by \a + * setInputCloud) + * \param[in] indices_arg pointer to indices vector of the dataset (given by \a + * setInputCloud) + */ + void + addPointToCloud(const PointT& point_arg, + PointCloudPtr cloud_arg, + IndicesPtr indices_arg); + + /** \brief Check if voxel at given point exist. + * \param[in] point_arg point to be checked + * \return "true" if voxel exist; "false" otherwise + */ + bool + isVoxelOccupiedAtPoint(const PointT& point_arg) const; + + /** \brief Delete the octree structure and its leaf nodes. + * */ + void + deleteTree() + { + // reset bounding box + min_x_ = min_y_ = max_y_ = min_z_ = max_z_ = 0; + this->bounding_box_defined_ = false; + + OctreeT::deleteTree(); + } + + /** \brief Check if voxel at given point coordinates exist. + * \param[in] point_x_arg X coordinate of point to be checked + * \param[in] point_y_arg Y coordinate of point to be checked + * \param[in] point_z_arg Z coordinate of point to be checked + * \return "true" if voxel exist; "false" otherwise + */ + bool + isVoxelOccupiedAtPoint(const double point_x_arg, + const double point_y_arg, + const double point_z_arg) const; + + /** \brief Check if voxel at given point from input cloud exist. + * \param[in] point_idx_arg point to be checked + * \return "true" if voxel exist; "false" otherwise + */ + bool + isVoxelOccupiedAtPoint(const int& point_idx_arg) const; + + /** \brief Get a PointT vector of centers of all occupied voxels. + * \param[out] voxel_center_list_arg results are written to this vector of PointT + * elements + * \return number of occupied voxels + */ + int + getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const; + + /** \brief Get a PointT vector of centers of voxels intersected by a line segment. + * This returns a approximation of the actual intersected voxels by walking + * along the line with small steps. Voxels are ordered, from closest to + * furthest w.r.t. the origin. + * \param[in] origin origin of the line segment + * \param[in] end end of the line segment + * \param[out] voxel_center_list results are written to this vector of PointT elements + * \param[in] precision determines the size of the steps: step_size = + * octree_resolution x precision + * \return number of intersected voxels + */ + int + getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin, + const Eigen::Vector3f& end, + AlignedPointTVector& voxel_center_list, + float precision = 0.2); + + /** \brief Delete leaf node / voxel at given point + * \param[in] point_arg point addressing the voxel to be deleted. + */ + void + deleteVoxelAtPoint(const PointT& point_arg); + + /** \brief Delete leaf node / voxel at given point from input cloud + * \param[in] point_idx_arg index of point addressing the voxel to be deleted. + */ + void + deleteVoxelAtPoint(const int& point_idx_arg); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Bounding box methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Investigate dimensions of pointcloud data set and define corresponding + * bounding box for octree. */ + void + defineBoundingBox(); + + /** \brief Define bounding box for octree + * \note Bounding box cannot be changed once the octree contains elements. + * \param[in] min_x_arg X coordinate of lower bounding box corner + * \param[in] min_y_arg Y coordinate of lower bounding box corner + * \param[in] min_z_arg Z coordinate of lower bounding box corner + * \param[in] max_x_arg X coordinate of upper bounding box corner + * \param[in] max_y_arg Y coordinate of upper bounding box corner + * \param[in] max_z_arg Z coordinate of upper bounding box corner + */ + void + defineBoundingBox(const double min_x_arg, + const double min_y_arg, + const double min_z_arg, + const double max_x_arg, + const double max_y_arg, + const double max_z_arg); + + /** \brief Define bounding box for octree + * \note Lower bounding box point is set to (0, 0, 0) + * \note Bounding box cannot be changed once the octree contains elements. + * \param[in] max_x_arg X coordinate of upper bounding box corner + * \param[in] max_y_arg Y coordinate of upper bounding box corner + * \param[in] max_z_arg Z coordinate of upper bounding box corner + */ + void + defineBoundingBox(const double max_x_arg, + const double max_y_arg, + const double max_z_arg); + + /** \brief Define bounding box cube for octree + * \note Lower bounding box corner is set to (0, 0, 0) + * \note Bounding box cannot be changed once the octree contains elements. + * \param[in] cubeLen_arg side length of bounding box cube. + */ + void + defineBoundingBox(const double cubeLen_arg); + + /** \brief Get bounding box for octree + * \note Bounding box cannot be changed once the octree contains elements. + * \param[in] min_x_arg X coordinate of lower bounding box corner + * \param[in] min_y_arg Y coordinate of lower bounding box corner + * \param[in] min_z_arg Z coordinate of lower bounding box corner + * \param[in] max_x_arg X coordinate of upper bounding box corner + * \param[in] max_y_arg Y coordinate of upper bounding box corner + * \param[in] max_z_arg Z coordinate of upper bounding box corner + */ + void + getBoundingBox(double& min_x_arg, + double& min_y_arg, + double& min_z_arg, + double& max_x_arg, + double& max_y_arg, + double& max_z_arg) const; + + /** \brief Calculates the squared diameter of a voxel at given tree depth + * \param[in] tree_depth_arg depth/level in octree + * \return squared diameter + */ + double + getVoxelSquaredDiameter(unsigned int tree_depth_arg) const; + + /** \brief Calculates the squared diameter of a voxel at leaf depth + * \return squared diameter + */ + inline double + getVoxelSquaredDiameter() const + { + return getVoxelSquaredDiameter(this->octree_depth_); + } + + /** \brief Calculates the squared voxel cube side length at given tree depth + * \param[in] tree_depth_arg depth/level in octree + * \return squared voxel cube side length + */ + double + getVoxelSquaredSideLen(unsigned int tree_depth_arg) const; + + /** \brief Calculates the squared voxel cube side length at leaf level + * \return squared voxel cube side length + */ + inline double + getVoxelSquaredSideLen() const + { + return getVoxelSquaredSideLen(this->octree_depth_); + } + + /** \brief Generate bounds of the current voxel of an octree iterator + * \param[in] iterator: octree iterator + * \param[out] min_pt lower bound of voxel + * \param[out] max_pt upper bound of voxel + */ + inline void + getVoxelBounds(const OctreeIteratorBase& iterator, + Eigen::Vector3f& min_pt, + Eigen::Vector3f& max_pt) const + { + this->genVoxelBoundsFromOctreeKey(iterator.getCurrentOctreeKey(), + iterator.getCurrentOctreeDepth(), + min_pt, + max_pt); + } + + /** \brief Enable dynamic octree structure + * \note Leaf nodes are kept as close to the root as possible and are only expanded + * if the number of DataT objects within a leaf node exceeds a fixed limit. + * \param maxObjsPerLeaf: maximum number of DataT objects per leaf + * */ + inline void + enableDynamicDepth(std::size_t maxObjsPerLeaf) + { + assert(this->leaf_count_ == 0); + max_objs_per_leaf_ = maxObjsPerLeaf; + + this->dynamic_depth_enabled_ = max_objs_per_leaf_ > 0; + } + +protected: + /** \brief Add point at index from input pointcloud dataset to octree + * \param[in] point_idx_arg the index representing the point in the dataset given by + * \a setInputCloud to be added + */ + virtual void + addPointIdx(const int point_idx_arg); + + /** \brief Add point at index from input pointcloud dataset to octree + * \param[in] leaf_node to be expanded + * \param[in] parent_branch parent of leaf node to be expanded + * \param[in] child_idx child index of leaf node (in parent branch) + * \param[in] depth_mask of leaf node to be expanded + */ + void + expandLeafNode(LeafNode* leaf_node, + BranchNode* parent_branch, + unsigned char child_idx, + unsigned int depth_mask); + + /** \brief Get point at index from input pointcloud dataset + * \param[in] index_arg index representing the point in the dataset given by \a + * setInputCloud + * \return PointT from input pointcloud dataset + */ + const PointT& + getPointByIndex(const unsigned int index_arg) const; + + /** \brief Find octree leaf node at a given point + * \param[in] point_arg query point + * \return pointer to leaf node. If leaf node does not exist, pointer is 0. + */ + LeafContainerT* + findLeafAtPoint(const PointT& point_arg) const + { + OctreeKey key; + + // generate key for point + this->genOctreeKeyforPoint(point_arg, key); + + return (this->findLeaf(key)); + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Protected octree methods based on octree keys + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Define octree key setting and octree depth based on defined bounding box. + */ + void + getKeyBitSize(); + + /** \brief Grow the bounding box/octree until point fits + * \param[in] point_idx_arg point that should be within bounding box; + */ + void + adoptBoundingBoxToPoint(const PointT& point_idx_arg); + + /** \brief Checks if given point is within the bounding box of the octree + * \param[in] point_idx_arg point to be checked for bounding box violations + * \return "true" - no bound violation + */ + inline bool + isPointWithinBoundingBox(const PointT& point_idx_arg) const + { + return (!((point_idx_arg.x < min_x_) || (point_idx_arg.y < min_y_) || + (point_idx_arg.z < min_z_) || (point_idx_arg.x >= max_x_) || + (point_idx_arg.y >= max_y_) || (point_idx_arg.z >= max_z_))); + } + + /** \brief Generate octree key for voxel at a given point + * \param[in] point_arg the point addressing a voxel + * \param[out] key_arg write octree key to this reference + */ + void + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const; + + /** \brief Generate octree key for voxel at a given point + * \param[in] point_x_arg X coordinate of point addressing a voxel + * \param[in] point_y_arg Y coordinate of point addressing a voxel + * \param[in] point_z_arg Z coordinate of point addressing a voxel + * \param[out] key_arg write octree key to this reference + */ + void + genOctreeKeyforPoint(const double point_x_arg, + const double point_y_arg, + const double point_z_arg, + OctreeKey& key_arg) const; + + /** \brief Virtual method for generating octree key for a given point index. + * \note This method enables to assign indices to leaf nodes during octree + * deserialization. + * \param[in] data_arg index value representing a point in the dataset given by \a + * setInputCloud + * \param[out] key_arg write octree key to this reference \return "true" - octree keys + * are assignable + */ + virtual bool + genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const; + + /** \brief Generate a point at center of leaf node voxel + * \param[in] key_arg octree key addressing a leaf node. + * \param[out] point_arg write leaf node voxel center to this point reference + */ + void + genLeafNodeCenterFromOctreeKey(const OctreeKey& key_arg, PointT& point_arg) const; + + /** \brief Generate a point at center of octree voxel at given tree level + * \param[in] key_arg octree key addressing an octree node. + * \param[in] tree_depth_arg octree depth of query voxel + * \param[out] point_arg write leaf node center point to this reference + */ + void + genVoxelCenterFromOctreeKey(const OctreeKey& key_arg, + unsigned int tree_depth_arg, + PointT& point_arg) const; + + /** \brief Generate bounds of an octree voxel using octree key and tree depth + * arguments + * \param[in] key_arg octree key addressing an octree node. + * \param[in] tree_depth_arg octree depth of query voxel + * \param[out] min_pt lower bound of voxel + * \param[out] max_pt upper bound of voxel + */ + void + genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg, + unsigned int tree_depth_arg, + Eigen::Vector3f& min_pt, + Eigen::Vector3f& max_pt) const; + + /** \brief Recursively search the tree for all leaf nodes and return a vector of voxel + * centers. + * \param[in] node_arg current octree node to be explored + * \param[in] key_arg octree key addressing a leaf node. + * \param[out] voxel_center_list_arg results are written to this vector of PointT + * elements + * \return number of voxels found + */ + int + getOccupiedVoxelCentersRecursive(const BranchNode* node_arg, + const OctreeKey& key_arg, + AlignedPointTVector& voxel_center_list_arg) const; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Globals + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Pointer to input point cloud dataset. */ + PointCloudConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + IndicesConstPtr indices_; + + /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ + double epsilon_; + + /** \brief Octree resolution. */ + double resolution_; + + // Octree bounding box coordinates + double min_x_; + double max_x_; + + double min_y_; + double max_y_; + + double min_z_; + double max_z_; + + /** \brief Flag indicating if octree has defined bounding box. */ + bool bounding_box_defined_; + + /** \brief Amount of DataT objects per leafNode before expanding branch + * \note zero indicates a fixed/maximum depth octree structure + * **/ + std::size_t max_objs_per_leaf_; +}; + +} // namespace octree +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_adjacency.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_adjacency.h new file mode 100755 index 0000000..64db980 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_adjacency.h @@ -0,0 +1,250 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Jeremie Papon + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : jpapon@gmail.com + * Email : jpapon@gmail.com + */ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace pcl { + +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud voxel class which maintains adjacency information for + * its voxels. + * + * This pointcloud octree class generates an octree from a point cloud (zero-copy). The + * octree pointcloud is initialized with its voxel resolution. Its bounding box is + * automatically adjusted or can be predefined. + * + * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes. + * + * An optional transform function can be provided which changes how the voxel grid is + * computed - this can be used to, for example, make voxel bins larger as they increase + * in distance from the origin (camera). \note See SupervoxelClustering for an example + * of how to provide a transform function. + * + * If used in academic work, please cite: + * + * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter + * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds + * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition + * (CVPR) 2013 + * + * \ingroup octree + * \author Jeremie Papon (jpapon@gmail.com) */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template , + typename BranchContainerT = OctreeContainerEmpty> +class OctreePointCloudAdjacency +: public OctreePointCloud { + +public: + using OctreeBaseT = OctreeBase; + + using OctreeAdjacencyT = + OctreePointCloudAdjacency; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using OctreePointCloudT = + OctreePointCloud; + using LeafNode = typename OctreePointCloudT::LeafNode; + using BranchNode = typename OctreePointCloudT::BranchNode; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + // BGL graph + using VoxelAdjacencyList = boost:: + adjacency_list; + using VoxelID = typename VoxelAdjacencyList::vertex_descriptor; + using EdgeID = typename VoxelAdjacencyList::edge_descriptor; + + // Leaf vector - pointers to all leaves + using LeafVectorT = std::vector; + + // Fast leaf iterators that don't require traversing tree + using iterator = typename LeafVectorT::iterator; + using const_iterator = typename LeafVectorT::const_iterator; + + inline iterator + begin() + { + return (leaf_vector_.begin()); + } + inline iterator + end() + { + return (leaf_vector_.end()); + } + inline LeafContainerT* + at(std::size_t idx) + { + return leaf_vector_.at(idx); + } + + // Size of neighbors + inline std::size_t + size() const + { + return leaf_vector_.size(); + } + + /** \brief Constructor. + * + * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */ + OctreePointCloudAdjacency(const double resolution_arg); + + /** \brief Adds points from cloud to the octree. + * + * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */ + void + addPointsFromInputCloud(); + + /** \brief Gets the leaf container for a given point. + * + * \param[in] point_arg Point to search for + * + * \returns Pointer to the leaf container - null if no leaf container found. */ + LeafContainerT* + getLeafContainerAtPoint(const PointT& point_arg) const; + + /** \brief Computes an adjacency graph of voxel relations. + * + * \warning This slows down rapidly as cloud size increases due to the number of + * edges. + * + * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel + * touching relationships. Vertices are PointT, edges represent touching, and edge + * lengths are the distance between the points. */ + void + computeVoxelAdjacencyGraph(VoxelAdjacencyList& voxel_adjacency_graph); + + /** \brief Sets a point transform (and inverse) used to transform the space of the + * input cloud. + * + * This is useful for changing how adjacency is calculated - such as relaxing the + * adjacency criterion for points further from the camera. + * + * \param[in] transform_func A boost:function pointer to the transform to be used. The + * transform must have one parameter (a point) which it modifies in place. */ + void + setTransformFunction(std::function transform_func) + { + transform_func_ = transform_func; + } + + /** \brief Tests whether input point is occluded from specified camera point by other + * voxels. + * + * \param[in] point_arg Point to test for + * \param[in] camera_pos Position of camera, defaults to origin + * + * \returns True if path to camera is blocked by a voxel, false otherwise. */ + bool + testForOcclusion(const PointT& point_arg, + const PointXYZ& camera_pos = PointXYZ(0, 0, 0)); + +protected: + /** \brief Add point at index from input pointcloud dataset to octree. + * + * \param[in] point_idx_arg The index representing the point in the dataset given by + * setInputCloud() to be added + * + * \note This virtual implementation allows the use of a transform function to compute + * keys. */ + void + addPointIdx(const int point_idx_arg) override; + + /** \brief Fills in the neighbors fields for new voxels. + * + * \param[in] key_arg Key of the voxel to check neighbors for + * \param[in] leaf_container Pointer to container of the leaf to check neighbors for + */ + void + computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container); + + /** \brief Generates octree key for specified point (uses transform if provided). + * + * \param[in] point_arg Point to generate key for + * \param[out] key_arg Resulting octree key */ + void + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const; + +private: + /** \brief Add point at given index from input point cloud to octree. + * + * Index will be also added to indices vector. This functionality is not enabled for + * adjacency octree. */ + using OctreePointCloudT::addPointFromCloud; + + /** \brief Add point simultaneously to octree and input point cloud. + * + * This functionality is not enabled for adjacency octree. */ + using OctreePointCloudT::addPointToCloud; + + using OctreePointCloudT::input_; + using OctreePointCloudT::max_x_; + using OctreePointCloudT::max_y_; + using OctreePointCloudT::max_z_; + using OctreePointCloudT::min_x_; + using OctreePointCloudT::min_y_; + using OctreePointCloudT::min_z_; + using OctreePointCloudT::resolution_; + + /// Local leaf pointer vector used to make iterating through leaves fast. + LeafVectorT leaf_vector_; + + std::function transform_func_; +}; + +} // namespace octree + +} // namespace pcl + +// Note: Do not precompile this octree type because it is typically used with custom +// leaf containers. +#include diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_adjacency_container.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_adjacency_container.h new file mode 100755 index 0000000..b6e5ad6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_adjacency_container.h @@ -0,0 +1,224 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Jeremie Papon + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : jpapon@gmail.com + * Email : jpapon@gmail.com + */ + +#pragma once + +namespace pcl { + +namespace octree { +/** \brief @b Octree adjacency leaf container class- stores a list of pointers to + * neighbors, number of points added, and a DataT value \note This class implements a + * leaf node that stores pointers to neighboring leaves \note This class also has a + * virtual computeData function, which is called by + * octreePointCloudAdjacency::addPointsFromInputCloud. \note You should make explicit + * instantiations of it for your pointtype/datatype combo (if needed) see + * supervoxel_clustering.hpp for an example of this + */ +template +class OctreePointCloudAdjacencyContainer : public OctreeContainerBase { + template + friend class OctreePointCloudAdjacency; + +public: + using NeighborListT = std::list*>; + using const_iterator = typename NeighborListT::const_iterator; + // const iterators to neighbors + inline const_iterator + cbegin() const + { + return (neighbors_.begin()); + } + inline const_iterator + cend() const + { + return (neighbors_.end()); + } + // size of neighbors + inline std::size_t + size() const + { + return neighbors_.size(); + } + + /** \brief Class initialization. */ + OctreePointCloudAdjacencyContainer() : OctreeContainerBase() { this->reset(); } + + /** \brief Empty class deconstructor. */ + ~OctreePointCloudAdjacencyContainer() {} + + /** \brief Returns the number of neighbors this leaf has + * \returns number of neighbors + */ + std::size_t + getNumNeighbors() const + { + return neighbors_.size(); + } + + /** \brief Gets the number of points contributing to this leaf */ + int + getPointCounter() const + { + return num_points_; + } + + /** \brief Returns a reference to the data member to access it without copying */ + DataT& + getData() + { + return data_; + } + + /** \brief Sets the data member + * \param[in] data_arg New value for data + */ + void + setData(const DataT& data_arg) + { + data_ = data_arg; + } + + /** \brief virtual method to get size of container + * \return number of points added to leaf node container. + */ + std::size_t + getSize() const override + { + return num_points_; + } + +protected: + // iterators to neighbors + using iterator = typename NeighborListT::iterator; + inline iterator + begin() + { + return (neighbors_.begin()); + } + inline iterator + end() + { + return (neighbors_.end()); + } + + /** \brief deep copy function */ + virtual OctreePointCloudAdjacencyContainer* + deepCopy() const + { + OctreePointCloudAdjacencyContainer* new_container = + new OctreePointCloudAdjacencyContainer; + new_container->setNeighbors(this->neighbors_); + new_container->setPointCounter(this->num_points_); + return new_container; + } + + /** \brief Add new point to container- this just counts points + * \note To actually store data in the leaves, need to specialize this + * for your point and data type as in supervoxel_clustering.hpp + */ + // param[in] new_point the new point to add + void + addPoint(const PointInT& /*new_point*/) + { + using namespace pcl::common; + ++num_points_; + } + + /** \brief Function for working on data added. Base implementation does nothing + * */ + void + computeData() + {} + + /** \brief Sets the number of points contributing to this leaf */ + void + setPointCounter(int points_arg) + { + num_points_ = points_arg; + } + + /** \brief Clear the voxel centroid */ + void + reset() override + { + neighbors_.clear(); + num_points_ = 0; + data_ = DataT(); + } + + /** \brief Add new neighbor to voxel. + * \param[in] neighbor the new neighbor to add + */ + void + addNeighbor(OctreePointCloudAdjacencyContainer* neighbor) + { + neighbors_.push_back(neighbor); + } + + /** \brief Remove neighbor from neighbor set. + * \param[in] neighbor the neighbor to remove + */ + void + removeNeighbor(OctreePointCloudAdjacencyContainer* neighbor) + { + for (iterator neighb_it = neighbors_.begin(); neighb_it != neighbors_.end(); + ++neighb_it) { + if (*neighb_it == neighbor) { + neighbors_.erase(neighb_it); + return; + } + } + } + + /** \brief Sets the whole neighbor set + * \param[in] neighbor_arg the new set + */ + void + setNeighbors(const NeighborListT& neighbor_arg) + { + neighbors_ = neighbor_arg; + } + +private: + int num_points_; + NeighborListT neighbors_; + DataT data_; +}; +} // namespace octree +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_changedetector.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_changedetector.h new file mode 100755 index 0000000..8d2a3b4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_changedetector.h @@ -0,0 +1,114 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { +namespace octree { + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud change detector class + * \note This pointcloud octree class generate an octrees from a point cloud + * (zero-copy). It allows to detect new leaf nodes and serialize their point indices + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template + +class OctreePointCloudChangeDetector +: public OctreePointCloud> + +{ + +public: + using Ptr = shared_ptr< + OctreePointCloudChangeDetector>; + using ConstPtr = shared_ptr< + const OctreePointCloudChangeDetector>; + + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudChangeDetector(const double resolution_arg) + : OctreePointCloud>(resolution_arg) + {} + + /** \brief Get a indices from all leaf nodes that did not exist in previous buffer. + * \param indicesVector_arg: results are written to this vector of int indices + * \param minPointsPerLeaf_arg: minimum amount of points required within leaf node to + * become serialized. + * \return number of point indices + */ + std::size_t + getPointIndicesFromNewVoxels(std::vector& indicesVector_arg, + const int minPointsPerLeaf_arg = 0) + { + + std::vector leaf_containers; + this->serializeNewLeafs(leaf_containers); + + for (const auto& leaf_container : leaf_containers) { + if (static_cast(leaf_container->getSize()) >= minPointsPerLeaf_arg) + leaf_container->getPointIndices(indicesVector_arg); + } + + return (indicesVector_arg.size()); + } +}; +} // namespace octree +} // namespace pcl + +#define PCL_INSTANTIATE_OctreePointCloudChangeDetector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudChangeDetector; diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_density.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_density.h new file mode 100755 index 0000000..19e97d0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_density.h @@ -0,0 +1,153 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +namespace pcl { +namespace octree { +/** \brief @b Octree pointcloud density leaf node class + * \note This class implements a leaf node that counts the amount of points which fall + * into its voxel space. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreePointCloudDensityContainer : public OctreeContainerBase { +public: + /** \brief Class initialization. */ + OctreePointCloudDensityContainer() : point_counter_(0) {} + + /** \brief Empty class deconstructor. */ + ~OctreePointCloudDensityContainer() {} + + /** \brief deep copy function */ + virtual OctreePointCloudDensityContainer* + deepCopy() const + { + return (new OctreePointCloudDensityContainer(*this)); + } + + /** \brief Equal comparison operator + * \param[in] other OctreePointCloudDensityContainer to compare with + */ + bool + operator==(const OctreeContainerBase& other) const override + { + const OctreePointCloudDensityContainer* otherContainer = + dynamic_cast(&other); + + return (this->point_counter_ == otherContainer->point_counter_); + } + + /** \brief Read input data. Only an internal counter is increased. + */ + void + addPointIndex(int) + { + point_counter_++; + } + + /** \brief Return point counter. + * \return Amount of points + */ + unsigned int + getPointCounter() + { + return (point_counter_); + } + + /** \brief Reset leaf node. */ + void + reset() override + { + point_counter_ = 0; + } + +private: + unsigned int point_counter_; +}; + +/** \brief @b Octree pointcloud density class + * \note This class generate an octrees from a point cloud (zero-copy). Only the amount + * of points that fall into the leaf node voxel are stored. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreePointCloudDensity +: public OctreePointCloud { +public: + /** \brief OctreePointCloudDensity class constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudDensity(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} + + /** \brief Empty class deconstructor. */ + + ~OctreePointCloudDensity() {} + + /** \brief Get the amount of points within a leaf node voxel which is addressed by a + * point + * \param[in] point_arg: a point addressing a voxel \return amount of points + * that fall within leaf node voxel + */ + unsigned int + getVoxelDensityAtPoint(const PointT& point_arg) const + { + unsigned int point_count = 0; + + OctreePointCloudDensityContainer* leaf = this->findLeafAtPoint(point_arg); + + if (leaf) + point_count = leaf->getPointCounter(); + + return (point_count); + } +}; +} // namespace octree +} // namespace pcl + +#define PCL_INSTANTIATE_OctreePointCloudDensity(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudDensity; diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_occupancy.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_occupancy.h new file mode 100755 index 0000000..5c7d110 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_occupancy.h @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +namespace pcl { +namespace octree { + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud occupancy class + * \tparam PointT type of point used in pointcloud + * This pointcloud octree class generate an octrees from a point cloud (zero-copy). No + * information is stored at the lead nodes. It can be used of occupancy checks. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OctreePointCloudOccupancy +: public OctreePointCloud> + +{ + +public: + // public typedefs for single/double buffering + using SingleBuffer = + OctreePointCloudOccupancy; + using DoubleBuffer = + OctreePointCloudOccupancy; + + // public point cloud typedefs + using PointCloud = + typename OctreePointCloud::PointCloud; + using PointCloudPtr = typename OctreePointCloud::PointCloudPtr; + using PointCloudConstPtr = + typename OctreePointCloud:: + PointCloudConstPtr; + + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudOccupancy(const double resolution_arg) + : OctreePointCloud>(resolution_arg) + {} + + /** \brief Empty class constructor. */ + + ~OctreePointCloudOccupancy() {} + + /** \brief Set occupied voxel at point. + * \param point_arg: input point + * */ + void + setOccupiedVoxelAtPoint(const PointT& point_arg) + { + OctreeKey key; + + // make sure bounding box is big enough + this->adoptBoundingBoxToPoint(point_arg); + + // generate key + this->genOctreeKeyforPoint(point_arg, key); + + // add point to octree at key + this->createLeaf(key); + } + + /** \brief Set occupied voxels at all points from point cloud. + * \param cloud_arg: input point cloud + * */ + void + setOccupiedVoxelsAtPointsFromCloud(PointCloudPtr cloud_arg) + { + for (std::size_t i = 0; i < cloud_arg->points.size(); i++) { + // check for NaNs + if (isFinite(cloud_arg->points[i])) { + // set voxel at point + this->setOccupiedVoxelAtPoint(cloud_arg->points[i]); + } + } + } +}; +} // namespace octree + +} // namespace pcl + +#define PCL_INSTANTIATE_OctreePointCloudOccupancy(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudOccupancy; diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_pointvector.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_pointvector.h new file mode 100755 index 0000000..47cf807 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_pointvector.h @@ -0,0 +1,89 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +namespace pcl { +namespace octree { + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud point vector class + * \note This pointcloud octree class generate an octrees from a point cloud + * (zero-copy). Every leaf node contains a list of point indices of the dataset given by + * \a setInputCloud. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template > +class OctreePointCloudPointVector +: public OctreePointCloud { + +public: + // public typedefs for single/double buffering + using SingleBuffer = + OctreePointCloudPointVector>; + // typedef OctreePointCloudPointVector > DoubleBuffer; + + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudPointVector(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} + + /** \brief Empty class constructor. */ + ~OctreePointCloudPointVector() {} +}; +} // namespace octree +} // namespace pcl + +#define PCL_INSTANTIATE_OctreePointCloudPointVector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudPointVector; diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_singlepoint.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_singlepoint.h new file mode 100755 index 0000000..a307bc8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_singlepoint.h @@ -0,0 +1,90 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud single point class + * \note This pointcloud octree class generate an octrees from a point cloud + * (zero-copy). Every leaf node contains a single point index from the dataset given by + * \a setInputCloud. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template > + +class OctreePointCloudSinglePoint +: public OctreePointCloud { + +public: + // public typedefs for single/double buffering + using SingleBuffer = + OctreePointCloudSinglePoint>; + // typedef OctreePointCloudSinglePoint > DoubleBuffer; + + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudSinglePoint(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} + + /** \brief Empty class constructor. */ + ~OctreePointCloudSinglePoint() {} +}; + +} // namespace octree +} // namespace pcl + +#define PCL_INSTANTIATE_OctreePointCloudSinglePoint(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudSinglePoint; diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_voxelcentroid.h b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_voxelcentroid.h new file mode 100755 index 0000000..5510049 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_pointcloud_voxelcentroid.h @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +namespace pcl { +namespace octree { +/** \brief @b Octree pointcloud voxel centroid leaf node class + * \note This class implements a leaf node that calculates the mean centroid of all + * points added this octree container. + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase { +public: + /** \brief Class initialization. */ + OctreePointCloudVoxelCentroidContainer() { this->reset(); } + + /** \brief Empty class deconstructor. */ + ~OctreePointCloudVoxelCentroidContainer() {} + + /** \brief deep copy function */ + virtual OctreePointCloudVoxelCentroidContainer* + deepCopy() const + { + return (new OctreePointCloudVoxelCentroidContainer(*this)); + } + + /** \brief Equal comparison operator - set to false + */ + // param[in] OctreePointCloudVoxelCentroidContainer to compare with + bool + operator==(const OctreeContainerBase&) const override + { + return (false); + } + + /** \brief Add new point to voxel. + * \param[in] new_point the new point to add + */ + void + addPoint(const PointT& new_point) + { + using namespace pcl::common; + + ++point_counter_; + + point_sum_ += new_point; + } + + /** \brief Calculate centroid of voxel. + * \param[out] centroid_arg the resultant centroid of the voxel + */ + void + getCentroid(PointT& centroid_arg) const + { + using namespace pcl::common; + + if (point_counter_) { + centroid_arg = point_sum_; + centroid_arg /= static_cast(point_counter_); + } + else { + centroid_arg *= 0.0f; + } + } + + /** \brief Reset leaf container. */ + void + reset() override + { + using namespace pcl::common; + + point_counter_ = 0; + point_sum_ *= 0.0f; + } + +private: + unsigned int point_counter_; + PointT point_sum_; +}; + +/** \brief @b Octree pointcloud voxel centroid class + * \note This class generate an octrees from a point cloud (zero-copy). It provides a + * vector of centroids for all occupied voxels. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template , + typename BranchContainerT = OctreeContainerEmpty> +class OctreePointCloudVoxelCentroid +: public OctreePointCloud { +public: + using Ptr = shared_ptr>; + using ConstPtr = + shared_ptr>; + + using OctreeT = OctreePointCloud; + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + /** \brief OctreePointCloudVoxelCentroids class constructor. + * \param[in] resolution_arg octree resolution at lowest octree level + */ + OctreePointCloudVoxelCentroid(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} + + /** \brief Empty class deconstructor. */ + + ~OctreePointCloudVoxelCentroid() {} + + /** \brief Add DataT object to leaf node at octree key. + * \param pointIdx_arg + */ + void + addPointIdx(const int pointIdx_arg) override + { + OctreeKey key; + + assert(pointIdx_arg < static_cast(this->input_->points.size())); + + const PointT& point = this->input_->points[pointIdx_arg]; + + // make sure bounding box is big enough + this->adoptBoundingBoxToPoint(point); + + // generate key + this->genOctreeKeyforPoint(point, key); + + // add point to octree at key + LeafContainerT* container = this->createLeaf(key); + container->addPoint(point); + } + + /** \brief Get centroid for a single voxel addressed by a PointT point. + * \param[in] point_arg point addressing a voxel in octree + * \param[out] voxel_centroid_arg centroid is written to this PointT reference + * \return "true" if voxel is found; "false" otherwise + */ + bool + getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const; + + /** \brief Get centroid for a single voxel addressed by a PointT point from input + * cloud. + * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree + * \param[out] voxel_centroid_arg centroid is written to this PointT reference + * \return "true" if voxel is found; "false" otherwise + */ + inline bool + getVoxelCentroidAtPoint(const int& point_idx_arg, PointT& voxel_centroid_arg) const + { + // get centroid at point + return (this->getVoxelCentroidAtPoint(this->input_->points[point_idx_arg], + voxel_centroid_arg)); + } + + /** \brief Get PointT vector of centroids for all occupied voxels. + * \param[out] voxel_centroid_list_arg results are written to this vector of PointT + * elements + * \return number of occupied voxels + */ + std::size_t + getVoxelCentroids( + typename OctreePointCloud:: + AlignedPointTVector& voxel_centroid_list_arg) const; + + /** \brief Recursively explore the octree and output a PointT vector of centroids for + * all occupied voxels. + * \param[in] branch_arg: current branch node + * \param[in] key_arg: current key + * \param[out] voxel_centroid_list_arg results are written to this vector of PointT + * elements + */ + void + getVoxelCentroidsRecursive( + const BranchNode* branch_arg, + OctreeKey& key_arg, + typename OctreePointCloud:: + AlignedPointTVector& voxel_centroid_list_arg) const; +}; +} // namespace octree +} // namespace pcl + +// Note: Don't precompile this octree type to speed up compilation. It's probably rarely +// used. +#include diff --git a/deps_install/include/pcl-1.10/pcl/octree/octree_search.h b/deps_install/include/pcl-1.10/pcl/octree/octree_search.h new file mode 100755 index 0000000..47b7818 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/octree/octree_search.h @@ -0,0 +1,659 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include + +namespace pcl { +namespace octree { +/** \brief @b Octree pointcloud search class + * \note This class provides several methods for spatial neighbor search based on octree + * structure + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreePointCloudSearch +: public OctreePointCloud { +public: + // public typedefs + using IndicesPtr = shared_ptr>; + using IndicesConstPtr = shared_ptr>; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + // Boost shared pointers + using Ptr = + shared_ptr>; + using ConstPtr = shared_ptr< + const OctreePointCloudSearch>; + + // Eigen aligned allocator + using AlignedPointTVector = std::vector>; + + using OctreeT = OctreePointCloud; + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + /** \brief Constructor. + * \param[in] resolution octree resolution at lowest octree level + */ + OctreePointCloudSearch(const double resolution) + : OctreePointCloud(resolution) + {} + + /** \brief Search for neighbors within a voxel at given point + * \param[in] point point addressing a leaf node voxel + * \param[out] point_idx_data the resultant indices of the neighboring voxel points + * \return "true" if leaf node exist; "false" otherwise + */ + bool + voxelSearch(const PointT& point, std::vector& point_idx_data); + + /** \brief Search for neighbors within a voxel at given point referenced by a point + * index + * \param[in] index the index in input cloud defining the query point + * \param[out] point_idx_data the resultant indices of the neighboring voxel points + * \return "true" if leaf node exist; "false" otherwise + */ + bool + voxelSearch(const int index, std::vector& point_idx_data); + + /** \brief Search for k-nearest neighbors at the query point. + * \param[in] cloud the point cloud data + * \param[in] index the index in \a cloud representing the query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be + * resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points (must be resized to \a k a priori!) + * \return number of neighbors found + */ + inline int + nearestKSearch(const PointCloud& cloud, + int index, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances) + { + return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances)); + } + + /** \brief Search for k-nearest neighbors at given query point. + * \param[in] p_q the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be + * resized to k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points (must be resized to k a priori!) + * \return number of neighbors found + */ + int + nearestKSearch(const PointT& p_q, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances); + + /** \brief Search for k-nearest neighbors at query point + * \param[in] index index representing the query point in the dataset given by \a + * setInputCloud. If indices were given in setInputCloud, index will be the position + * in the indices vector. + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be + * resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points (must be resized to \a k a priori!) + * \return number of neighbors found + */ + int + nearestKSearch(int index, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances); + + /** \brief Search for approx. nearest neighbor at the query point. + * \param[in] cloud the point cloud data + * \param[in] query_index the index in \a cloud representing the query point + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + * \return number of neighbors found + */ + inline void + approxNearestSearch(const PointCloud& cloud, + int query_index, + int& result_index, + float& sqr_distance) + { + return (approxNearestSearch(cloud.points[query_index], result_index, sqr_distance)); + } + + /** \brief Search for approx. nearest neighbor at the query point. + * \param[in] p_q the given query point + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + */ + void + approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance); + + /** \brief Search for approx. nearest neighbor at the query point. + * \param[in] query_index index representing the query point in the dataset given by + * \a setInputCloud. If indices were given in setInputCloud, index will be the + * position in the indices vector. + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + * \return number of neighbors found + */ + void + approxNearestSearch(int query_index, int& result_index, float& sqr_distance); + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] cloud the point cloud data + * \param[in] index the index in \a cloud representing the query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + int + radiusSearch(const PointCloud& cloud, + int index, + double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn = 0) + { + return ( + radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); + } + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] p_q the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + int + radiusSearch(const PointT& p_q, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn = 0) const; + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] index index representing the query point in the dataset given by \a + * setInputCloud. If indices were given in setInputCloud, index will be the position + * in the indices vector + * \param[in] radius radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + int + radiusSearch(int index, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn = 0) const; + + /** \brief Get a PointT vector of centers of all voxels that intersected by a ray + * (origin, direction). + * \param[in] origin ray origin + * \param[in] direction ray direction vector + * \param[out] voxel_center_list results are written to this vector of PointT elements + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: + * disable) + * \return number of intersected voxels + */ + int + getIntersectedVoxelCenters(Eigen::Vector3f origin, + Eigen::Vector3f direction, + AlignedPointTVector& voxel_center_list, + int max_voxel_count = 0) const; + + /** \brief Get indices of all voxels that are intersected by a ray (origin, + * direction). + * \param[in] origin ray origin \param[in] direction ray direction vector + * \param[out] k_indices resulting point indices from intersected voxels + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: + * disable) + * \return number of intersected voxels + */ + int + getIntersectedVoxelIndices(Eigen::Vector3f origin, + Eigen::Vector3f direction, + std::vector& k_indices, + int max_voxel_count = 0) const; + + /** \brief Search for points within rectangular search area + * Points exactly on the edges of the search rectangle are included. + * \param[in] min_pt lower corner of search area + * \param[in] max_pt upper corner of search area + * \param[out] k_indices the resultant point indices + * \return number of points found within search area + */ + int + boxSearch(const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + std::vector& k_indices) const; + +protected: + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Octree-based search routines & helpers + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b Priority queue entry for branch nodes + * \note This class defines priority queue entries for the nearest neighbor search. + * \author Julius Kammerl (julius@kammerl.de) + */ + class prioBranchQueueEntry { + public: + /** \brief Empty constructor */ + prioBranchQueueEntry() : node(), point_distance(0) {} + + /** \brief Constructor for initializing priority queue entry. + * \param _node pointer to octree node + * \param _key octree key addressing voxel in octree structure + * \param[in] _point_distance distance of query point to voxel center + */ + prioBranchQueueEntry(OctreeNode* _node, OctreeKey& _key, float _point_distance) + : node(_node), point_distance(_point_distance), key(_key) + {} + + /** \brief Operator< for comparing priority queue entries with each other. + * \param[in] rhs the priority queue to compare this against + */ + bool + operator<(const prioBranchQueueEntry rhs) const + { + return (this->point_distance > rhs.point_distance); + } + + /** \brief Pointer to octree node. */ + const OctreeNode* node; + + /** \brief Distance to query point. */ + float point_distance; + + /** \brief Octree key. */ + OctreeKey key; + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b Priority queue entry for point candidates + * \note This class defines priority queue entries for the nearest neighbor point + * candidates. + * \author Julius Kammerl (julius@kammerl.de) + */ + class prioPointQueueEntry { + public: + /** \brief Empty constructor */ + prioPointQueueEntry() : point_idx_(0), point_distance_(0) {} + + /** \brief Constructor for initializing priority queue entry. + * \param[in] point_idx an index representing a point in the dataset given by \a + * setInputCloud \param[in] point_distance distance of query point to voxel center + */ + prioPointQueueEntry(unsigned int& point_idx, float point_distance) + : point_idx_(point_idx), point_distance_(point_distance) + {} + + /** \brief Operator< for comparing priority queue entries with each other. + * \param[in] rhs priority queue to compare this against + */ + bool + operator<(const prioPointQueueEntry& rhs) const + { + return (this->point_distance_ < rhs.point_distance_); + } + + /** \brief Index representing a point in the dataset given by \a setInputCloud. */ + int point_idx_; + + /** \brief Distance to query point. */ + float point_distance_; + }; + + /** \brief Helper function to calculate the squared distance between two points + * \param[in] point_a point A + * \param[in] point_b point B + * \return squared distance between point A and point B + */ + float + pointSquaredDist(const PointT& point_a, const PointT& point_b) const; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Recursive search routine methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Recursive search method that explores the octree and finds neighbors within + * a given radius + * \param[in] point query point \param[in] radiusSquared squared search radius + * \param[in] node current octree node to be explored + * \param[in] key octree key addressing a leaf node. + * \param[in] tree_depth current depth/level in the octree + * \param[out] k_indices vector of indices found to be neighbors of query point + * \param[out] k_sqr_distances squared distances of neighbors to query point + * \param[in] max_nn maximum of neighbors to be found + */ + void + getNeighborsWithinRadiusRecursive(const PointT& point, + const double radiusSquared, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const; + + /** \brief Recursive search method that explores the octree and finds the K nearest + * neighbors + * \param[in] point query point + * \param[in] K amount of nearest neighbors to be found + * \param[in] node current octree node to be explored + * \param[in] key octree key addressing a leaf node. + * \param[in] tree_depth current depth/level in the octree + * \param[in] squared_search_radius squared search radius distance + * \param[out] point_candidates priority queue of nearest neigbor point candidates + * \return squared search radius based on current point candidate set found + */ + double + getKNearestNeighborRecursive( + const PointT& point, + unsigned int K, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + const double squared_search_radius, + std::vector& point_candidates) const; + + /** \brief Recursive search method that explores the octree and finds the approximate + * nearest neighbor + * \param[in] point query point + * \param[in] node current octree node to be explored + * \param[in] key octree key addressing a leaf node. + * \param[in] tree_depth current depth/level in the octree + * \param[out] result_index result index is written to this reference + * \param[out] sqr_distance squared distance to search + */ + void + approxNearestSearchRecursive(const PointT& point, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + int& result_index, + float& sqr_distance); + + /** \brief Recursively search the tree for all intersected leaf nodes and return a + * vector of voxel centers. This algorithm is based off the paper An Efficient + * Parametric Algorithm for Octree Traversal: + * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] max_x octree nodes X coordinate of upper bounding box corner + * \param[in] max_y octree nodes Y coordinate of upper bounding box corner + * \param[in] max_z octree nodes Z coordinate of upper bounding box corner + * \param[in] a + * \param[in] node current octree node to be explored + * \param[in] key octree key addressing a leaf node. + * \param[out] voxel_center_list results are written to this vector of PointT elements + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: + * disable) + * \return number of voxels found + */ + int + getIntersectedVoxelCentersRecursive(double min_x, + double min_y, + double min_z, + double max_x, + double max_y, + double max_z, + unsigned char a, + const OctreeNode* node, + const OctreeKey& key, + AlignedPointTVector& voxel_center_list, + int max_voxel_count) const; + + /** \brief Recursive search method that explores the octree and finds points within a + * rectangular search area + * \param[in] min_pt lower corner of search area + * \param[in] max_pt upper corner of search area + * \param[in] node current octree node to be explored + * \param[in] key octree key addressing a leaf node. + * \param[in] tree_depth current depth/level in the octree + * \param[out] k_indices the resultant point indices + */ + void + boxSearchRecursive(const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + std::vector& k_indices) const; + + /** \brief Recursively search the tree for all intersected leaf nodes and return a + * vector of indices. This algorithm is based off the paper An Efficient Parametric + * Algorithm for Octree Traversal: http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] max_x octree nodes X coordinate of upper bounding box corner + * \param[in] max_y octree nodes Y coordinate of upper bounding box corner + * \param[in] max_z octree nodes Z coordinate of upper bounding box corner + * \param[in] a + * \param[in] node current octree node to be explored + * \param[in] key octree key addressing a leaf node. + * \param[out] k_indices resulting indices + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: + * disable) + * \return number of voxels found + */ + int + getIntersectedVoxelIndicesRecursive(double min_x, + double min_y, + double min_z, + double max_x, + double max_y, + double max_z, + unsigned char a, + const OctreeNode* node, + const OctreeKey& key, + std::vector& k_indices, + int max_voxel_count) const; + + /** \brief Initialize raytracing algorithm + * \param origin + * \param direction + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] max_x octree nodes X coordinate of upper bounding box corner + * \param[in] max_y octree nodes Y coordinate of upper bounding box corner + * \param[in] max_z octree nodes Z coordinate of upper bounding box corner + * \param a + */ + inline void + initIntersectedVoxel(Eigen::Vector3f& origin, + Eigen::Vector3f& direction, + double& min_x, + double& min_y, + double& min_z, + double& max_x, + double& max_y, + double& max_z, + unsigned char& a) const + { + // Account for division by zero when direction vector is 0.0 + const float epsilon = 1e-10f; + if (direction.x() == 0.0) + direction.x() = epsilon; + if (direction.y() == 0.0) + direction.y() = epsilon; + if (direction.z() == 0.0) + direction.z() = epsilon; + + // Voxel childIdx remapping + a = 0; + + // Handle negative axis direction vector + if (direction.x() < 0.0) { + origin.x() = static_cast(this->min_x_) + static_cast(this->max_x_) - + origin.x(); + direction.x() = -direction.x(); + a |= 4; + } + if (direction.y() < 0.0) { + origin.y() = static_cast(this->min_y_) + static_cast(this->max_y_) - + origin.y(); + direction.y() = -direction.y(); + a |= 2; + } + if (direction.z() < 0.0) { + origin.z() = static_cast(this->min_z_) + static_cast(this->max_z_) - + origin.z(); + direction.z() = -direction.z(); + a |= 1; + } + min_x = (this->min_x_ - origin.x()) / direction.x(); + max_x = (this->max_x_ - origin.x()) / direction.x(); + min_y = (this->min_y_ - origin.y()) / direction.y(); + max_y = (this->max_y_ - origin.y()) / direction.y(); + min_z = (this->min_z_ - origin.z()) / direction.z(); + max_z = (this->max_z_ - origin.z()) / direction.z(); + } + + /** \brief Find first child node ray will enter + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] mid_x octree nodes X coordinate of bounding box mid line + * \param[in] mid_y octree nodes Y coordinate of bounding box mid line + * \param[in] mid_z octree nodes Z coordinate of bounding box mid line + * \return the first child node ray will enter + */ + inline int + getFirstIntersectedNode(double min_x, + double min_y, + double min_z, + double mid_x, + double mid_y, + double mid_z) const + { + int currNode = 0; + + if (min_x > min_y) { + if (min_x > min_z) { + // max(min_x, min_y, min_z) is min_x. Entry plane is YZ. + if (mid_y < min_x) + currNode |= 2; + if (mid_z < min_x) + currNode |= 1; + } + else { + // max(min_x, min_y, min_z) is min_z. Entry plane is XY. + if (mid_x < min_z) + currNode |= 4; + if (mid_y < min_z) + currNode |= 2; + } + } + else { + if (min_y > min_z) { + // max(min_x, min_y, min_z) is min_y. Entry plane is XZ. + if (mid_x < min_y) + currNode |= 4; + if (mid_z < min_y) + currNode |= 1; + } + else { + // max(min_x, min_y, min_z) is min_z. Entry plane is XY. + if (mid_x < min_z) + currNode |= 4; + if (mid_y < min_z) + currNode |= 2; + } + } + + return currNode; + } + + /** \brief Get the next visited node given the current node upper + * bounding box corner. This function accepts three float values, and + * three int values. The function returns the ith integer where the + * ith float value is the minimum of the three float values. + * \param[in] x current nodes X coordinate of upper bounding box corner + * \param[in] y current nodes Y coordinate of upper bounding box corner + * \param[in] z current nodes Z coordinate of upper bounding box corner + * \param[in] a next node if exit Plane YZ + * \param[in] b next node if exit Plane XZ + * \param[in] c next node if exit Plane XY + * \return the next child node ray will enter or 8 if exiting + */ + inline int + getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const + { + if (x < y) { + if (x < z) + return a; + return c; + } + if (y < z) + return b; + return c; + } +}; +} // namespace octree +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/pcl_base.h b/deps_install/include/pcl-1.10/pcl/pcl_base.h new file mode 100755 index 0000000..ac13ca3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/pcl_base.h @@ -0,0 +1,262 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +// Include PCL macros such as PCL_ERROR, PCL_MAKE_ALIGNED_OPERATOR_NEW, etc +#include + +#include +#include +#include + +// Point Cloud message includes. Needed everywhere. +#include +#include +#include + +namespace pcl +{ + // definitions used everywhere + using Indices = std::vector; + using IndicesPtr = shared_ptr; + using IndicesConstPtr = shared_ptr; + + ///////////////////////////////////////////////////////////////////////////////////////// + /** \brief PCL base class. Implements methods that are used by most PCL algorithms. + * \ingroup common + */ + template + class PCLBase + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + /** \brief Empty constructor. */ + PCLBase (); + + /** \brief Copy constructor. */ + PCLBase (const PCLBase& base); + + /** \brief Destructor. */ + virtual ~PCLBase () + { + input_.reset (); + indices_.reset (); + } + + /** \brief Provide a pointer to the input dataset + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + virtual void + setInputCloud (const PointCloudConstPtr &cloud); + + /** \brief Get a pointer to the input point cloud dataset. */ + inline PointCloudConstPtr const + getInputCloud () const { return (input_); } + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + virtual void + setIndices (const IndicesPtr &indices); + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + virtual void + setIndices (const IndicesConstPtr &indices); + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + virtual void + setIndices (const PointIndicesConstPtr &indices); + + /** \brief Set the indices for the points laying within an interest region of + * the point cloud. + * \note you shouldn't call this method on unorganized point clouds! + * \param[in] row_start the offset on rows + * \param[in] col_start the offset on columns + * \param[in] nb_rows the number of rows to be considered row_start included + * \param[in] nb_cols the number of columns to be considered col_start included + */ + virtual void + setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols); + + /** \brief Get a pointer to the vector of indices used. */ + inline IndicesPtr const + getIndices () { return (indices_); } + + /** \brief Get a pointer to the vector of indices used. */ + inline IndicesConstPtr const + getIndices () const { return (indices_); } + + /** \brief Override PointCloud operator[] to shorten code + * \note this method can be called instead of (*input_)[(*indices_)[pos]] + * or input_->points[(*indices_)[pos]] + * \param[in] pos position in indices_ vector + */ + inline const PointT& operator[] (std::size_t pos) const + { + return ((*input_)[(*indices_)[pos]]); + } + + protected: + /** \brief The input point cloud dataset. */ + PointCloudConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + IndicesPtr indices_; + + /** \brief Set to true if point indices are used. */ + bool use_indices_; + + /** \brief If no set of indices are given, we construct a set of fake indices that mimic the input PointCloud. */ + bool fake_indices_; + + /** \brief This method should get called before starting the actual computation. + * + * Internally, initCompute() does the following: + * - checks if an input dataset is given, and returns false otherwise + * - checks whether a set of input indices has been given. Returns true if yes. + * - if no input indices have been given, a fake set is created, which will be used until: + * - either a new set is given via setIndices(), or + * - a new cloud is given that has a different set of points. This will trigger an update on the set of fake indices + */ + bool + initCompute (); + + /** \brief This method should get called after finishing the actual computation. + */ + bool + deinitCompute (); + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + ///////////////////////////////////////////////////////////////////////////////////////// + template <> + class PCL_EXPORTS PCLBase + { + public: + using PCLPointCloud2 = pcl::PCLPointCloud2; + using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; + using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + /** \brief Empty constructor. */ + PCLBase (); + + /** \brief destructor. */ + virtual ~PCLBase() + { + input_.reset (); + indices_.reset (); + } + + /** \brief Provide a pointer to the input dataset + * \param cloud the const boost shared pointer to a PointCloud message + */ + void + setInputCloud (const PCLPointCloud2ConstPtr &cloud); + + /** \brief Get a pointer to the input point cloud dataset. */ + inline PCLPointCloud2ConstPtr const + getInputCloud () const { return (input_); } + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + void + setIndices (const IndicesPtr &indices); + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the indices that represent the input data. + */ + void + setIndices (const PointIndicesConstPtr &indices); + + /** \brief Get a pointer to the vector of indices used. */ + inline IndicesPtr const + getIndices () const { return (indices_); } + + protected: + /** \brief The input point cloud dataset. */ + PCLPointCloud2ConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + IndicesPtr indices_; + + /** \brief Set to true if point indices are used. */ + bool use_indices_; + + /** \brief If no set of indices are given, we construct a set of fake indices that mimic the input PointCloud. */ + bool fake_indices_; + + /** \brief The size of each individual field. */ + std::vector field_sizes_; + + /** \brief The x-y-z fields indices. */ + int x_idx_, y_idx_, z_idx_; + + /** \brief The desired x-y-z field names. */ + std::string x_field_name_, y_field_name_, z_field_name_; + + bool initCompute (); + bool deinitCompute (); + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/pcl_config.h b/deps_install/include/pcl-1.10/pcl/pcl_config.h new file mode 100755 index 0000000..2ff121a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/pcl_config.h @@ -0,0 +1,83 @@ +/* pcl_config.h. Generated by CMake for PCL. */ + +// Ensure the compiler is meeting the minimum C++ standard +// MSVC is not checked via __cplusplus due to +// https://developercommunity.visualstudio.com/content/problem/120156/-cplusplus-macro-still-defined-as-pre-c11-value.html +#if (!defined(_MSC_VER) && __cplusplus < 201402L) || (defined(_MSC_VER) && _MSC_VER < 1900) + #error PCL requires C++14 or above +#endif + +#define BUILD_RelWithDebInfo +/* PCL version information */ +#define PCL_MAJOR_VERSION 1 +#define PCL_MINOR_VERSION 10 +#define PCL_REVISION_VERSION 1 +#define PCL_DEV_VERSION 0 +#define PCL_VERSION_PRETTY "1.10.1" +#define PCL_VERSION_CALC(MAJ, MIN, PATCH) (MAJ*100000+MIN*100+PATCH) +#define PCL_VERSION \ + PCL_VERSION_CALC(PCL_MAJOR_VERSION, PCL_MINOR_VERSION, PCL_REVISION_VERSION) +#define PCL_VERSION_COMPARE(OP, MAJ, MIN, PATCH) \ + (PCL_VERSION*10+PCL_DEV_VERSION OP PCL_VERSION_CALC(MAJ, MIN, PATCH)*10) + +/* #undef HAVE_TBB */ + +#define HAVE_OPENNI 1 + +#define HAVE_OPENNI2 1 + +#define HAVE_QHULL 1 + +#define HAVE_QHULL_2011 1 + +/* #undef HAVE_CUDA */ + +/* #undef HAVE_ENSENSO */ + +/* #undef HAVE_DAVIDSDK */ + +// SSE macros +#define HAVE_POSIX_MEMALIGN +#define HAVE_MM_MALLOC +#define HAVE_SSE4_2_EXTENSIONS +#define HAVE_SSE4_1_EXTENSIONS +#define HAVE_SSSE3_EXTENSIONS +#define HAVE_SSE3_EXTENSIONS +#define HAVE_SSE2_EXTENSIONS +#define HAVE_SSE_EXTENSIONS + +#define HAVE_PNG + +/* Precompile for a minimal set of point types instead of all. */ +/* #undef PCL_ONLY_CORE_POINT_TYPES */ + +/* Do not precompile for any point types at all. */ +/* #undef PCL_NO_PRECOMPILE */ + +#ifdef DISABLE_OPENNI +#undef HAVE_OPENNI +#endif + +#ifdef DISABLE_OPENNI2 +#undef HAVE_OPENNI2 +#endif + +#ifdef DISABLE_QHULL +#undef HAVE_QHULL +#endif + +/* Verbosity level defined by user through ccmake. */ +/* #undef VERBOSITY_LEVEL_ALWAYS */ +/* #undef VERBOSITY_LEVEL_ERROR */ +/* #undef VERBOSITY_LEVEL_WARN */ +#define VERBOSITY_LEVEL_INFO +/* #undef VERBOSITY_LEVEL_DEBUG */ +/* #undef VERBOSITY_LEVEL_VERBOSE */ + +/* Address the cases where on MacOS and OpenGL and GLUT are not frameworks */ +/* #undef OPENGL_IS_A_FRAMEWORK */ +/* #undef GLUT_IS_A_FRAMEWORK */ + +/* Version of OpenGL used by VTK as rendering backend */ +#define VTK_RENDERING_BACKEND_OPENGL_VERSION 1 + diff --git a/deps_install/include/pcl-1.10/pcl/pcl_exports.h b/deps_install/include/pcl-1.10/pcl/pcl_exports.h new file mode 100755 index 0000000..b92818a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/pcl_exports.h @@ -0,0 +1,49 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +// This header is created to include to NVCC compiled sources. +// Header 'pcl_macros' is not suitable since it includes , +// which can't be eaten by nvcc (it's too weak) + +#if defined WIN32 || defined _WIN32 || defined WINCE || defined __MINGW32__ + #ifdef PCLAPI_EXPORTS + #define PCL_EXPORTS __declspec(dllexport) + #else + #define PCL_EXPORTS + #endif +#else + #define PCL_EXPORTS +#endif diff --git a/deps_install/include/pcl-1.10/pcl/pcl_macros.h b/deps_install/include/pcl-1.10/pcl/pcl_macros.h new file mode 100755 index 0000000..b512dad --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/pcl_macros.h @@ -0,0 +1,416 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +/** + * \file pcl/pcl_macros.h + * + * \brief Defines all the PCL and non-PCL macros used + * \ingroup common + */ + +#if defined __INTEL_COMPILER + #pragma warning disable 2196 2536 279 +#endif + +#if defined _MSC_VER + // 4244 : conversion from 'type1' to 'type2', possible loss of data + // 4661 : no suitable definition provided for explicit template instantiation reques + // 4503 : decorated name length exceeded, name was truncated + // 4146 : unary minus operator applied to unsigned type, result still unsigned + #pragma warning (disable: 4018 4244 4267 4521 4251 4661 4305 4503 4146) +#endif + +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#include +#include +#include +#include +#include +#include + +#include +#include + +//Eigen has an enum that clashes with X11 Success define, which is ultimately included by pcl +#ifdef Success + #undef Success +#endif +#include + +#include + +// It seems that __has_cpp_attribute doesn't work correctly +// when compiling with some versions of nvcc so we +// additionally check if nvcc is used before setting the +// PCL_DEPRECATED macro to [[deprecated]]. +#if defined(__has_cpp_attribute) && __has_cpp_attribute(deprecated) && !defined(__CUDACC__) + #define PCL_DEPRECATED(message) [[deprecated(message)]] +#elif defined(__GNUC__) || defined(__clang__) + #define PCL_DEPRECATED(message) __attribute__((deprecated(message))) +#elif defined(_MSC_VER) + // Until Visual Studio 2013 you had to use __declspec(deprecated). + // However, we decided to ignore the deprecation for these version because + // of simplicity reasons. See PR #3634 for the details. + #define PCL_DEPRECATED(message) +#else + #warning "You need to implement PCL_DEPRECATED for this compiler" + #define PCL_DEPRECATED(message) +#endif + +namespace pcl +{ + /** + * \brief Alias for boost::shared_ptr + * + * For ease of switching from boost::shared_ptr to std::shared_ptr + * + * \see pcl::make_shared + * \tparam T Type of the object stored inside the shared_ptr + */ + template + using shared_ptr = boost::shared_ptr; + + using uint8_t PCL_DEPRECATED("use std::uint8_t instead of pcl::uint8_t") = std::uint8_t; + using int8_t PCL_DEPRECATED("use std::int8_t instead of pcl::int8_t") = std::int8_t; + using uint16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::uint16_t") = std::uint16_t; + using int16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::int16_t") = std::int16_t; + using uint32_t PCL_DEPRECATED("use std::uint32_t instead of pcl::uint32_t") = std::uint32_t; + using int32_t PCL_DEPRECATED("use std::int32_t instead of pcl::int32_t") = std::int32_t; + using uint64_t PCL_DEPRECATED("use std::uint64_t instead of pcl::uint64_t") = std::uint64_t; + using int64_t PCL_DEPRECATED("use std::int64_t instead of pcl::int64_t") = std::int64_t; + using int_fast16_t PCL_DEPRECATED("use std::int_fast16_t instead of pcl::int_fast16_t") = std::int_fast16_t; +} + +#if defined _WIN32 && defined _MSC_VER + +// Define math constants, without including math.h, to prevent polluting global namespace with old math methods +// Copied from math.h +#ifndef _MATH_DEFINES_DEFINED + #define _MATH_DEFINES_DEFINED + + #define M_E 2.71828182845904523536 // e + #define M_LOG2E 1.44269504088896340736 // log2(e) + #define M_LOG10E 0.434294481903251827651 // log10(e) + #define M_LN2 0.693147180559945309417 // ln(2) + #define M_LN10 2.30258509299404568402 // ln(10) + #define M_PI 3.14159265358979323846 // pi + #define M_PI_2 1.57079632679489661923 // pi/2 + #define M_PI_4 0.785398163397448309616 // pi/4 + #define M_1_PI 0.318309886183790671538 // 1/pi + #define M_2_PI 0.636619772367581343076 // 2/pi + #define M_2_SQRTPI 1.12837916709551257390 // 2/sqrt(pi) + #define M_SQRT2 1.41421356237309504880 // sqrt(2) + #define M_SQRT1_2 0.707106781186547524401 // 1/sqrt(2) +#endif + +// Stupid. This should be removed when all the PCL dependencies have min/max fixed. +#ifndef NOMINMAX +# define NOMINMAX +#endif + +# define __PRETTY_FUNCTION__ __FUNCTION__ +# define __func__ __FUNCTION__ + +#endif //defined _WIN32 && defined _MSC_VER + + +template +PCL_DEPRECATED("use std::isnan instead of pcl_isnan") +bool pcl_isnan (T&& x) { return std::isnan (std::forward (x)); } + +template +PCL_DEPRECATED("use std::isfinite instead of pcl_isfinite") +bool pcl_isfinite (T&& x) { return std::isfinite (std::forward (x)); } + +template +PCL_DEPRECATED("use std::isinf instead of pcl_isinf") +bool pcl_isinf (T&& x) { return std::isinf (std::forward (x)); } + + +#ifndef DEG2RAD +#define DEG2RAD(x) ((x)*0.017453293) +#endif + +#ifndef RAD2DEG +#define RAD2DEG(x) ((x)*57.29578) +#endif + +/** \brief Macro that maps version information given by major.minor.patch to a linear integer value to enable easy comparison + */ +#define PCL_LINEAR_VERSION(major,minor,patch) ((major)<<16|(minor)<<8|(patch)) + +/** Win32 doesn't seem to have rounding functions. + * Therefore implement our own versions of these functions here. + */ + +__inline double +pcl_round (double number) +{ + return (number < 0.0 ? std::ceil (number - 0.5) : std::floor (number + 0.5)); +} +__inline float +pcl_round (float number) +{ + return (number < 0.0f ? std::ceil (number - 0.5f) : std::floor (number + 0.5f)); +} + +#ifdef __GNUC__ +#define pcl_lrint(x) (lrint(static_cast (x))) +#define pcl_lrintf(x) (lrintf(static_cast (x))) +#else +#define pcl_lrint(x) (static_cast(pcl_round(x))) +#define pcl_lrintf(x) (static_cast(pcl_round(x))) +#endif + +#ifdef WIN32 +#define pcl_sleep(x) Sleep(1000*(x)) +#else +#define pcl_sleep(x) sleep(x) +#endif + +#ifndef PVAR + #define PVAR(s) \ + #s << " = " << (s) << std::flush +#endif +#ifndef PVARN +#define PVARN(s) \ + #s << " = " << (s) << "\n" +#endif +#ifndef PVARC +#define PVARC(s) \ + #s << " = " << (s) << ", " << std::flush +#endif +#ifndef PVARS +#define PVARS(s) \ + #s << " = " << (s) << " " << std::flush +#endif +#ifndef PVARA +#define PVARA(s) \ + #s << " = " << RAD2DEG(s) << "deg" << std::flush +#endif +#ifndef PVARAN +#define PVARAN(s) \ + #s << " = " << RAD2DEG(s) << "deg\n" +#endif +#ifndef PVARAC +#define PVARAC(s) \ + #s << " = " << RAD2DEG(s) << "deg, " << std::flush +#endif +#ifndef PVARAS +#define PVARAS(s) \ + #s << " = " << RAD2DEG(s) << "deg " << std::flush +#endif + +#define FIXED(s) \ + std::fixed << s << std::resetiosflags(std::ios_base::fixed) + +#ifndef ERASE_STRUCT +#define ERASE_STRUCT(var) memset(&var, 0, sizeof(var)) +#endif + +#ifndef ERASE_ARRAY +#define ERASE_ARRAY(var, size) memset(var, 0, size*sizeof(*var)) +#endif + +#ifndef SET_ARRAY +#define SET_ARRAY(var, value, size) { for (decltype(size) i = 0; i < size; ++i) var[i]=value; } +#endif + +#ifndef PCL_EXTERN_C + #ifdef __cplusplus + #define PCL_EXTERN_C extern "C" + #else + #define PCL_EXTERN_C + #endif +#endif + +#if defined WIN32 || defined _WIN32 || defined WINCE || defined __MINGW32__ + #ifdef PCLAPI_EXPORTS + #define PCL_EXPORTS __declspec(dllexport) + #else + #define PCL_EXPORTS + #endif +#else + #define PCL_EXPORTS +#endif + +#if defined WIN32 || defined _WIN32 + #define PCL_CDECL __cdecl + #define PCL_STDCALL __stdcall +#else + #define PCL_CDECL + #define PCL_STDCALL +#endif + +#ifndef PCLAPI + #define PCLAPI(rettype) PCL_EXTERN_C PCL_EXPORTS rettype PCL_CDECL +#endif + +// Macro for pragma operator +#if (defined (__GNUC__) || defined(__clang__)) + #define PCL_PRAGMA(x) _Pragma (#x) +#elif _MSC_VER + #define PCL_PRAGMA(x) __pragma (#x) +#else + #define PCL_PRAGMA +#endif + +// Macro for emitting pragma warning +#if (defined (__GNUC__) || defined(__clang__)) + #define PCL_PRAGMA_WARNING(x) PCL_PRAGMA (GCC warning x) +#elif _MSC_VER + #define PCL_PRAGMA_WARNING(x) PCL_PRAGMA (warning (x)) +#else + #define PCL_PRAGMA_WARNING +#endif + +//for clang cf. http://clang.llvm.org/docs/LanguageExtensions.html +#ifndef __has_extension + #define __has_extension(x) 0 // Compatibility with pre-3.0 compilers. +#endif + +#if defined (__GNUC__) || defined (__PGI) || defined (__IBMCPP__) || defined (__SUNPRO_CC) + #define PCL_ALIGN(alignment) __attribute__((aligned(alignment))) +#elif defined (_MSC_VER) + #define PCL_ALIGN(alignment) __declspec(align(alignment)) +#else + #error Alignment not supported on your platform +#endif + +#if defined(__GLIBC__) && PCL_LINEAR_VERSION(__GLIBC__,__GLIBC_MINOR__,0)>PCL_LINEAR_VERSION(2,8,0) + #define GLIBC_MALLOC_ALIGNED 1 +#else + #define GLIBC_MALLOC_ALIGNED 0 +#endif + +#if defined(__FreeBSD__) && !defined(__arm__) && !defined(__mips__) + #define FREEBSD_MALLOC_ALIGNED 1 +#else + #define FREEBSD_MALLOC_ALIGNED 0 +#endif + +#if defined(__APPLE__) || defined(_WIN64) || GLIBC_MALLOC_ALIGNED || FREEBSD_MALLOC_ALIGNED + #define MALLOC_ALIGNED 1 +#endif + +#if defined (HAVE_MM_MALLOC) + // Intel compiler defines an incompatible _mm_malloc signature + #if defined(__INTEL_COMPILER) + #include + #else + #include + #endif +#endif + +inline void* +aligned_malloc (std::size_t size) +{ + void *ptr; +#if defined (MALLOC_ALIGNED) + ptr = std::malloc (size); +#elif defined (HAVE_POSIX_MEMALIGN) + if (posix_memalign (&ptr, 16, size)) + ptr = 0; +#elif defined (HAVE_MM_MALLOC) + ptr = _mm_malloc (size, 16); +#elif defined (_MSC_VER) + ptr = _aligned_malloc (size, 16); +#elif defined (ANDROID) + ptr = memalign (16, size); +#else + #error aligned_malloc not supported on your platform + ptr = 0; +#endif + return (ptr); +} + +inline void +aligned_free (void* ptr) +{ +#if defined (MALLOC_ALIGNED) || defined (HAVE_POSIX_MEMALIGN) + std::free (ptr); +#elif defined (HAVE_MM_MALLOC) + _mm_free (ptr); +#elif defined (_MSC_VER) + _aligned_free (ptr); +#elif defined (ANDROID) + free (ptr); +#else + #error aligned_free not supported on your platform +#endif +} + +/** + * \brief Macro to signal a class requires a custom allocator + * + * It's an implementation detail to have pcl::has_custom_allocator work, a + * thin wrapper over Eigen's own macro + * + * \see pcl::has_custom_allocator, pcl::make_shared + * \ingroup common + */ +#define PCL_MAKE_ALIGNED_OPERATOR_NEW \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + using _custom_allocator_type_trait = void; + +/** + * \brief Macro to add a no-op or a fallthrough attribute based on compiler feature + * + * \ingroup common + */ +#if (__cplusplus >= 201703L) || (defined(_MSC_VER) && (_MSC_VER >= 1910) && (_MSVC_LANG >= 201703L)) + #define PCL_FALLTHROUGH [[fallthrough]]; +#elif defined(__clang__) + #define PCL_FALLTHROUGH [[clang::fallthrough]]; +#elif defined(__GNUC__) && (__GNUC__ >= 7) + #define PCL_FALLTHROUGH [[gnu::fallthrough]]; +#else + #define PCL_FALLTHROUGH +#endif + +#if (__cplusplus >= 201703L) || (defined(_MSC_VER) && (_MSC_VER >= 1911) && (_MSVC_LANG >= 201703L)) + #define PCL_NODISCARD [[nodiscard]] +#elif defined(__clang__) && (PCL_LINEAR_VERSION(__clang_major__, __clang_minor__, 0) >= PCL_LINEAR_VERSION(3, 9, 0)) + #define PCL_NODISCARD [[clang::warn_unused_result]] +#elif defined(__GNUC__) + #define PCL_NODISCARD [[gnu::warn_unused_result]] +#else + #define PCL_NODISCARD +#endif diff --git a/deps_install/include/pcl-1.10/pcl/pcl_tests.h b/deps_install/include/pcl-1.10/pcl/pcl_tests.h new file mode 100755 index 0000000..2b7f5e4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/pcl_tests.h @@ -0,0 +1,392 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +/** \file pcl_tests.h + * Helper macros for testing equality of various data fields in PCL points */ + +namespace pcl +{ + + /** test_macros.h provide helper macros for testing vectors, matrices etc. + * We took some liberty with upcasing names to make them look like googletest + * macros names so that reader is not confused. + * + * This file also provides a family of googletest-style macros for asserting + * equality or nearness of xyz, normal, and rgba fields. + * + * \author Nizar Sallem, Sergey Alexandrov + */ + + namespace test + { + + template + void EXPECT_EQ_VECTORS (const V1& v1, const V2& v2) + { + SCOPED_TRACE("EXPECT_EQ_VECTORS failed"); + EXPECT_EQ (v1.size (), v2.size ()); + std::size_t length = v1.size (); + for (std::size_t i = 0; i < length; ++i) + EXPECT_EQ (v1[i], v2[i]); + } + + template + void EXPECT_NEAR_VECTORS (const V1& v1, const V2& v2, const Scalar& epsilon) + { + SCOPED_TRACE("EXPECT_NEAR_VECTORS failed"); + EXPECT_EQ (v1.size (), v2.size ()); + std::size_t length = v1.size (); + for (std::size_t i = 0; i < length; ++i) + EXPECT_NEAR (v1[i], v2[i], epsilon); + } + + namespace internal + { + + template + ::testing::AssertionResult XYZEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getVector3fMap ().cwiseEqual ((p2).getVector3fMap ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getVector3fMap ()" << std::endl + << " Actual: " << p2.getVector3fMap ().transpose () << std::endl + << "Expected: " << expr1 << ".getVector3fMap ()" << std::endl + << "Which is: " << p1.getVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult XYZNear (const char* expr1, + const char* expr2, + const char* abs_error_expr, + const Point1T& p1, + const Point2T& p2, + double abs_error) + { + const Eigen::Vector3f diff = ((p1).getVector3fMap () - + (p2).getVector3fMap ()).cwiseAbs (); + if ((diff.array () < abs_error).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Some of the element-wise differences exceed " << abs_error_expr + << " (which evaluates to " << abs_error << ")" << std::endl + << "Difference: " << diff.transpose () << std::endl + << " Value of: " << expr2 << ".getVector3fMap ()" << std::endl + << " Actual: " << p2.getVector3fMap ().transpose () << std::endl + << " Expected: " << expr1 << ".getVector3fMap ()" << std::endl + << " Which is: " << p1.getVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult NormalEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getNormalVector3fMap ().cwiseEqual ((p2).getNormalVector3fMap ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl + << " Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl + << "Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl + << "Which is: " << p1.getNormalVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult NormalNear (const char* expr1, + const char* expr2, + const char* abs_error_expr, + const Point1T& p1, + const Point2T& p2, + double abs_error) + { + const Eigen::Vector3f diff = ((p1).getNormalVector3fMap () - + (p2).getNormalVector3fMap ()).cwiseAbs (); + if ((diff.array () < abs_error).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Some of the element-wise differences exceed " << abs_error_expr + << " (which evaluates to " << abs_error << ")" << std::endl + << "Difference: " << diff.transpose () << std::endl + << " Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl + << " Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl + << " Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl + << " Which is: " << p1.getNormalVector3fMap ().transpose (); + } + + template + ::testing::AssertionResult RGBEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getRGBVector3i ().cwiseEqual ((p2).getRGBVector3i ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getRGBVector3i ()" << std::endl + << " Actual: " << p2.getRGBVector3i ().transpose () << std::endl + << "Expected: " << expr1 << ".getRGBVector3i ()" << std::endl + << "Which is: " << p1.getRGBVector3i ().transpose (); + } + + template + ::testing::AssertionResult RGBAEQ (const char* expr1, + const char* expr2, + const Point1T& p1, + const Point2T& p2) + { + if ((p1).getRGBAVector4i ().cwiseEqual ((p2).getRGBAVector4i ()).all ()) + return ::testing::AssertionSuccess (); + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".getRGBAVector4i ()" << std::endl + << " Actual: " << p2.getRGBAVector4i ().transpose () << std::endl + << "Expected: " << expr1 << ".getRGBAVector4i ()" << std::endl + << "Which is: " << p1.getRGBAVector4i ().transpose (); + } + + template + ::testing::AssertionResult MetaDataEQ (const char* expr1, + const char* expr2, + const PointCloud1T& p1, + const PointCloud2T& p2) + { + if (!(p1.header == p2.header)) + return ::testing::AssertionFailure () << "Headers are different"; + if (p1.width != p2.width) + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".width" << std::endl + << " Actual: " << p2.width << std::endl + << "Expected: " << expr1 << ".width" << std::endl + << "Which is: " << p1.width << std::endl; + if (p1.height != p2.height) + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".height" << std::endl + << " Actual: " << p2.height << std::endl + << "Expected: " << expr1 << ".height" << std::endl + << "Which is: " << p1.height << std::endl; + if (p1.is_dense != p2.is_dense) + return ::testing::AssertionFailure () + << "Value of: " << expr2 << ".is_dense" << std::endl + << " Actual: " << p2.is_dense << std::endl + << "Expected: " << expr1 << ".is_dense" << std::endl + << "Which is: " << p1.is_dense << std::endl; + if (p1.sensor_origin_ != p2.sensor_origin_) + return ::testing::AssertionFailure () << "Sensor origins are different"; + if (p1.sensor_orientation_.coeffs () != p2.sensor_orientation_.coeffs ()) + return ::testing::AssertionFailure () << "Sensor orientations are different"; + return ::testing::AssertionSuccess (); + } + + + template + ::testing::AssertionResult VectorContainsAll(const char* expr1, const char* expr2, const std::vector& elements, const std::vector& v) { + for(const V& item : elements) { + if(std::find(v.cbegin(), v.cend(), item)==v.cend()) { + + std::ostringstream vec_rep; + std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator(vec_rep, ", ")); + vec_rep<(elements_rep, ", ")); + elements_rep << elements.back(); + + return ::testing::AssertionFailure () + << "Actual : " << expr2 << std::endl + << "contains : " << vec_rep.str() << std::endl + << "Target set : " << expr1 << std::endl + << "contains : " << elements_rep.str() << std::endl; + } + } + return ::testing::AssertionSuccess (); + } + + template + ::testing::AssertionResult VectorDoesNotContain(const char* expr1, const char* expr2, const std::vector& elements, const std::vector& v) { + for(const V& item : elements) { + if(std::find(v.cbegin(), v.cend(), item)!=v.cend()) { + std::ostringstream vec_rep; + std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator(vec_rep, ", ")); + vec_rep<(elements_rep, ", ")); + elements_rep << elements.back(); + + return ::testing::AssertionFailure () + << "Actual : " << expr2 << std::endl + << "contains : " << vec_rep.str() << std::endl + << "Forbidden set: " << expr1 << std::endl + << "contains : " << elements_rep.str() << std::endl; + } + } + return ::testing::AssertionSuccess (); + } + + } + + } + +} + +/// Expect that each of x, y, and z fields are equal in +/// two points. +#define EXPECT_XYZ_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::XYZEQ, \ + (expected), (actual)) + +/// Assert that each of x, y, and z fields are equal in +/// two points. +#define ASSERT_XYZ_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::XYZEQ, \ + (expected), (actual)) + +/// Expect that differences between x, y, and z fields in +/// two points are each within abs_error. +#define EXPECT_XYZ_NEAR(expected, actual, abs_error) \ + EXPECT_PRED_FORMAT3(::pcl::test::internal::XYZNear, \ + (expected), (actual), abs_error) + +/// Assert that differences between x, y, and z fields in +/// two points are each within abs_error. +#define ASSERT_XYZ_NEAR(expected, actual, abs_error) \ + ASSERT_PRED_FORMAT3(::pcl::test::internal::XYZNear, \ + (expected), (actual), abs_error) + +/// Expect that each of normal_x, normal_y, and normal_z +/// fields are equal in two points. +#define EXPECT_NORMAL_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::NormalEQ, \ + (expected), (actual)) + +/// Assert that each of normal_x, normal_y, and normal_z +/// fields are equal in two points. +#define ASSERT_NORMAL_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::NormalEQ, \ + (expected), (actual)) + +/// Expect that differences between normal_x, normal_y, +/// and normal_z fields in two points are each within +/// abs_error. +#define EXPECT_NORMAL_NEAR(expected, actual, abs_error) \ + EXPECT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \ + (expected), (actual), abs_error) + +/// Assert that differences between normal_x, normal_y, +/// and normal_z fields in two points are each within +/// abs_error. +#define ASSERT_NORMAL_NEAR(expected, actual, abs_error) \ + ASSERT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \ + (expected), (actual), abs_error) + +/// Expect that each of r, g, and b fields are equal in +/// two points. +#define EXPECT_RGB_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBEQ, \ + (expected), (actual)) + +/// Assert that each of r, g, and b fields are equal in +/// two points. +#define ASSERT_RGB_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBEQ, \ + (expected), (actual)) + +/// Expect that each of r, g, b, and a fields are equal +/// in two points. +#define EXPECT_RGBA_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ, \ + (expected), (actual)) + +/// Assert that each of r, g, b, and a fields are equal +/// in two points. +#define ASSERT_RGBA_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ, \ + (expected), (actual)) + +/// Assert that the metadata (header, width, height, +/// is_dense, sensor origin and orientation) are equal +/// in two point clouds. +#define ASSERT_METADATA_EQ(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::MetaDataEQ, \ + expected, actual) + +/// Expect that the metadata (header, width, height, +/// is_dense, sensor origin and orientation) are equal +/// in two point clouds. +#define EXPECT_METADATA_EQ(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::MetaDataEQ, \ + expected, actual) + + +/// Expect that the vector contains all elements +/// from the expected vector. +#define EXPECT_VECTOR_CONTAINS_ALL(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \ + expected, actual) + + +/// Expect that the vector does not contain any element +/// from the expected vector. +#define EXPECT_VECTOR_DOES_NOT_CONTAIN(expected, actual) \ + EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \ + expected, actual) + + + +/// Assert that the vector contains all elements +/// from the expected vector. +#define ASSERT_VECTOR_CONTAINS_ALL(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \ + expected, actual) + + +/// Assert that the vector does not contain any element +/// from the expected vector. +#define ASSERT_VECTOR_DOES_NOT_CONTAIN(expected, actual) \ + ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \ + expected, actual) + + + diff --git a/deps_install/include/pcl-1.10/pcl/point_cloud.h b/deps_install/include/pcl-1.10/pcl/point_cloud.h new file mode 100755 index 0000000..b6e8266 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/point_cloud.h @@ -0,0 +1,647 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace pcl +{ + namespace detail + { + struct FieldMapping + { + std::size_t serialized_offset; + std::size_t struct_offset; + std::size_t size; + }; + } // namespace detail + + // Forward declarations + template class PointCloud; + using MsgFieldMap = std::vector; + + /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */ + template + struct NdCopyEigenPointFunctor + { + using Pod = typename traits::POD::type; + + /** \brief Constructor + * \param[in] p1 the input Eigen type + * \param[out] p2 the output Point type + */ + NdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointOutT &p2) + : p1_ (p1), + p2_ (reinterpret_cast(p2)), + f_idx_ (0) { } + + /** \brief Operator. Data copy happens here. */ + template inline void + operator() () + { + //boost::fusion::at_key (p2_) = p1_[f_idx_++]; + using T = typename pcl::traits::datatype::type; + std::uint8_t* data_ptr = reinterpret_cast(&p2_) + pcl::traits::offset::value; + *reinterpret_cast(data_ptr) = static_cast (p1_[f_idx_++]); + } + + private: + const Eigen::VectorXf &p1_; + Pod &p2_; + int f_idx_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */ + template + struct NdCopyPointEigenFunctor + { + using Pod = typename traits::POD::type; + + /** \brief Constructor + * \param[in] p1 the input Point type + * \param[out] p2 the output Eigen type + */ + NdCopyPointEigenFunctor (const PointInT &p1, Eigen::VectorXf &p2) + : p1_ (reinterpret_cast(p1)), p2_ (p2), f_idx_ (0) { } + + /** \brief Operator. Data copy happens here. */ + template inline void + operator() () + { + //p2_[f_idx_++] = boost::fusion::at_key (p1_); + using T = typename pcl::traits::datatype::type; + const std::uint8_t* data_ptr = reinterpret_cast(&p1_) + pcl::traits::offset::value; + p2_[f_idx_++] = static_cast (*reinterpret_cast(data_ptr)); + } + + private: + const Pod &p1_; + Eigen::VectorXf &p2_; + int f_idx_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + namespace detail + { + template + PCL_DEPRECATED("use createMapping() instead") + shared_ptr& + getMapping (pcl::PointCloud& p); + } // namespace detail + + /** \brief PointCloud represents the base class in PCL for storing collections of 3D points. + * + * The class is templated, which means you need to specify the type of data + * that it should contain. For example, to create a point cloud that holds 4 + * random XYZ data points, use: + * + * \code + * pcl::PointCloud cloud; + * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); + * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); + * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); + * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); + * \endcode + * + * The PointCloud class contains the following elements: + * - \b width - specifies the width of the point cloud dataset in the number of points. WIDTH has two meanings: + * - it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets; + * - it can specify the width (total number of points in a row) of an organized point cloud dataset. + * \a Mandatory. + * - \b height - specifies the height of the point cloud dataset in the number of points. HEIGHT has two meanings: + * - it can specify the height (total number of rows) of an organized point cloud dataset; + * - it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not). + * \a Mandatory. + * - \b points - the data array where all points of type PointT are stored. \a Mandatory. + * + * - \b is_dense - specifies if all the data in points is finite (true), or whether it might contain Inf/NaN values + * (false). \a Mandatory. + * + * - \b sensor_origin_ - specifies the sensor acquisition pose (origin/translation). \a Optional. + * - \b sensor_orientation_ - specifies the sensor acquisition pose (rotation). \a Optional. + * + * \author Patrick Mihelich, Radu B. Rusu + */ + template + class PCL_EXPORTS PointCloud + { + public: + /** \brief Default constructor. Sets \ref is_dense to true, \ref width + * and \ref height to 0, and the \ref sensor_origin_ and \ref + * sensor_orientation_ to identity. + */ + PointCloud () = default; + + /** \brief Copy constructor from point cloud subset + * \param[in] pc the cloud to copy into this + * \param[in] indices the subset to copy + */ + PointCloud (const PointCloud &pc, + const std::vector &indices) : + header (pc.header), points (indices.size ()), width (indices.size ()), height (1), is_dense (pc.is_dense), + sensor_origin_ (pc.sensor_origin_), sensor_orientation_ (pc.sensor_orientation_) + { + // Copy the obvious + assert (indices.size () <= pc.size ()); + for (std::size_t i = 0; i < indices.size (); i++) + points[i] = pc.points[indices[i]]; + } + + /** \brief Allocate constructor from point cloud subset + * \param[in] width_ the cloud width + * \param[in] height_ the cloud height + * \param[in] value_ default value + */ + PointCloud (std::uint32_t width_, std::uint32_t height_, const PointT& value_ = PointT ()) + : points (width_ * height_, value_) + , width (width_) + , height (height_) + {} + + //TODO: check if copy/move contructors/assignment operators are needed + + /** \brief Add a point cloud to the current cloud. + * \param[in] rhs the cloud to add to the current cloud + * \return the new cloud as a concatenation of the current cloud and the new given cloud + */ + inline PointCloud& + operator += (const PointCloud& rhs) + { + concatenate((*this), rhs); + return (*this); + } + + /** \brief Add a point cloud to another cloud. + * \param[in] rhs the cloud to add to the current cloud + * \return the new cloud as a concatenation of the current cloud and the new given cloud + */ + inline PointCloud + operator + (const PointCloud& rhs) + { + return (PointCloud (*this) += rhs); + } + + inline static bool + concatenate(pcl::PointCloud &cloud1, + const pcl::PointCloud &cloud2) + { + // Make the resultant point cloud take the newest stamp + cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp); + + // libstdc++ (GCC) on calling reserve allocates new memory, copies and deallocates old vector + // This causes a drastic performance hit. Prefer not to use reserve with libstdc++ (default on clang) + cloud1.points.insert (cloud1.points.end (), cloud2.points.begin (), cloud2.points.end ()); + + cloud1.width = static_cast(cloud1.points.size ()); + cloud1.height = 1; + cloud1.is_dense = cloud1.is_dense && cloud2.is_dense; + return true; + } + + inline static bool + concatenate(const pcl::PointCloud &cloud1, + const pcl::PointCloud &cloud2, + pcl::PointCloud &cloud_out) + { + cloud_out = cloud1; + return concatenate(cloud_out, cloud2); + } + + /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized + * datasets (those that have height != 1). + * \param[in] column the column coordinate + * \param[in] row the row coordinate + */ + inline const PointT& + at (int column, int row) const + { + if (this->height > 1) + return (points.at (row * this->width + column)); + else + throw UnorganizedPointCloudException ("Can't use 2D indexing with an unorganized point cloud"); + } + + /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized + * datasets (those that have height != 1). + * \param[in] column the column coordinate + * \param[in] row the row coordinate + */ + inline PointT& + at (int column, int row) + { + if (this->height > 1) + return (points.at (row * this->width + column)); + else + throw UnorganizedPointCloudException ("Can't use 2D indexing with an unorganized point cloud"); + } + + /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized + * datasets (those that have height != 1). + * \param[in] column the column coordinate + * \param[in] row the row coordinate + */ + inline const PointT& + operator () (std::size_t column, std::size_t row) const + { + return (points[row * this->width + column]); + } + + /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized + * datasets (those that have height != 1). + * \param[in] column the column coordinate + * \param[in] row the row coordinate + */ + inline PointT& + operator () (std::size_t column, std::size_t row) + { + return (points[row * this->width + column]); + } + + /** \brief Return whether a dataset is organized (e.g., arranged in a structured grid). + * \note The height value must be different than 1 for a dataset to be organized. + */ + inline bool + isOrganized () const + { + return (height > 1); + } + + /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud. + * \anchor getMatrixXfMap + * \note This method is for advanced users only! Use with care! + * + * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL + * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure, + * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix + * + * \param[in] dim the number of dimensions to consider for each point + * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns) + * \param[in] offset the number of dimensions to skip from the beginning of each point + * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point) + * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment. + * \attention PointT types are most of the time aligned, so the offsets are not continuous! + */ + inline Eigen::Map > + getMatrixXfMap (int dim, int stride, int offset) + { + if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit) + return (Eigen::Map >(reinterpret_cast(&points[0])+offset, points.size (), dim, Eigen::OuterStride<> (stride))); + else + return (Eigen::Map >(reinterpret_cast(&points[0])+offset, dim, points.size (), Eigen::OuterStride<> (stride))); + } + + /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud. + * \anchor getMatrixXfMap + * \note This method is for advanced users only! Use with care! + * + * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL + * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure, + * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix + * + * \param[in] dim the number of dimensions to consider for each point + * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns) + * \param[in] offset the number of dimensions to skip from the beginning of each point + * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point) + * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment. + * \attention PointT types are most of the time aligned, so the offsets are not continuous! + */ + inline const Eigen::Map > + getMatrixXfMap (int dim, int stride, int offset) const + { + if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit) + return (Eigen::Map >(reinterpret_cast(const_cast(&points[0]))+offset, points.size (), dim, Eigen::OuterStride<> (stride))); + else + return (Eigen::Map >(reinterpret_cast(const_cast(&points[0]))+offset, dim, points.size (), Eigen::OuterStride<> (stride))); + } + + /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud. + * \note This method is for advanced users only! Use with care! + * \attention PointT types are most of the time aligned, so the offsets are not continuous! + * See \ref getMatrixXfMap for more information. + */ + inline Eigen::Map > + getMatrixXfMap () + { + return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0)); + } + + /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud. + * \note This method is for advanced users only! Use with care! + * \attention PointT types are most of the time aligned, so the offsets are not continuous! + * See \ref getMatrixXfMap for more information. + */ + inline const Eigen::Map > + getMatrixXfMap () const + { + return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0)); + } + + /** \brief The point cloud header. It contains information about the acquisition time. */ + pcl::PCLHeader header; + + /** \brief The point data. */ + std::vector > points; + + /** \brief The point cloud width (if organized as an image-structure). */ + std::uint32_t width = 0; + /** \brief The point cloud height (if organized as an image-structure). */ + std::uint32_t height = 0; + + /** \brief True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields). */ + bool is_dense = true; + + /** \brief Sensor acquisition pose (origin/translation). */ + Eigen::Vector4f sensor_origin_ = Eigen::Vector4f::Zero (); + /** \brief Sensor acquisition pose (rotation). */ + Eigen::Quaternionf sensor_orientation_ = Eigen::Quaternionf::Identity (); + + using PointType = PointT; // Make the template class available from the outside + using VectorType = std::vector >; + using CloudVectorType = std::vector, Eigen::aligned_allocator > >; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + // std container compatibility typedefs according to + // http://en.cppreference.com/w/cpp/concept/Container + using value_type = PointT; + using reference = PointT&; + using const_reference = const PointT&; + using difference_type = typename VectorType::difference_type; + using size_type = typename VectorType::size_type; + + // iterators + using iterator = typename VectorType::iterator; + using const_iterator = typename VectorType::const_iterator; + inline iterator begin () { return (points.begin ()); } + inline iterator end () { return (points.end ()); } + inline const_iterator begin () const { return (points.begin ()); } + inline const_iterator end () const { return (points.end ()); } + + //capacity + inline std::size_t size () const { return (points.size ()); } + inline void reserve (std::size_t n) { points.reserve (n); } + inline bool empty () const { return points.empty (); } + + /** \brief Resize the cloud + * \param[in] n the new cloud size + */ + inline void resize (std::size_t n) + { + points.resize (n); + if (width * height != n) + { + width = static_cast (n); + height = 1; + } + } + + //element access + inline const PointT& operator[] (std::size_t n) const { return (points[n]); } + inline PointT& operator[] (std::size_t n) { return (points[n]); } + inline const PointT& at (std::size_t n) const { return (points.at (n)); } + inline PointT& at (std::size_t n) { return (points.at (n)); } + inline const PointT& front () const { return (points.front ()); } + inline PointT& front () { return (points.front ()); } + inline const PointT& back () const { return (points.back ()); } + inline PointT& back () { return (points.back ()); } + + /** \brief Insert a new point in the cloud, at the end of the container. + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] pt the point to insert + */ + inline void + push_back (const PointT& pt) + { + points.push_back (pt); + width = static_cast (points.size ()); + height = 1; + } + + /** \brief Emplace a new point in the cloud, at the end of the container. + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] args the parameters to forward to the point to construct + * \return reference to the emplaced point + */ + template inline reference + emplace_back (Args&& ...args) + { + points.emplace_back (std::forward (args)...); + width = static_cast (points.size ()); + height = 1; + return points.back(); + } + + /** \brief Insert a new point in the cloud, given an iterator. + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] position where to insert the point + * \param[in] pt the point to insert + * \return returns the new position iterator + */ + inline iterator + insert (iterator position, const PointT& pt) + { + iterator it = points.insert (position, pt); + width = static_cast (points.size ()); + height = 1; + return (it); + } + + /** \brief Insert a new point in the cloud N times, given an iterator. + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] position where to insert the point + * \param[in] n the number of times to insert the point + * \param[in] pt the point to insert + */ + inline void + insert (iterator position, std::size_t n, const PointT& pt) + { + points.insert (position, n, pt); + width = static_cast (points.size ()); + height = 1; + } + + /** \brief Insert a new range of points in the cloud, at a certain position. + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] position where to insert the data + * \param[in] first where to start inserting the points from + * \param[in] last where to stop inserting the points from + */ + template inline void + insert (iterator position, InputIterator first, InputIterator last) + { + points.insert (position, first, last); + width = static_cast (points.size ()); + height = 1; + } + + /** \brief Emplace a new point in the cloud, given an iterator. + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] position iterator before which the point will be emplaced + * \param[in] args the parameters to forward to the point to construct + * \return returns the new position iterator + */ + template inline iterator + emplace (iterator position, Args&& ...args) + { + iterator it = points.emplace (position, std::forward (args)...); + width = static_cast (points.size ()); + height = 1; + return (it); + } + + /** \brief Erase a point in the cloud. + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] position what data point to erase + * \return returns the new position iterator + */ + inline iterator + erase (iterator position) + { + iterator it = points.erase (position); + width = static_cast (points.size ()); + height = 1; + return (it); + } + + /** \brief Erase a set of points given by a (first, last) iterator pair + * \note This breaks the organized structure of the cloud by setting the height to 1! + * \param[in] first where to start erasing points from + * \param[in] last where to stop erasing points from + * \return returns the new position iterator + */ + inline iterator + erase (iterator first, iterator last) + { + iterator it = points.erase (first, last); + width = static_cast (points.size ()); + height = 1; + return (it); + } + + /** \brief Swap a point cloud with another cloud. + * \param[in,out] rhs point cloud to swap this with + */ + inline void + swap (PointCloud &rhs) + { + std::swap (header, rhs.header); + this->points.swap (rhs.points); + std::swap (width, rhs.width); + std::swap (height, rhs.height); + std::swap (is_dense, rhs.is_dense); + std::swap (sensor_origin_, rhs.sensor_origin_); + std::swap (sensor_orientation_, rhs.sensor_orientation_); + } + + /** \brief Removes all points in a cloud and sets the width and height to 0. */ + inline void + clear () + { + points.clear (); + width = 0; + height = 0; + } + + /** \brief Copy the cloud to the heap and return a smart pointer + * Note that deep copy is performed, so avoid using this function on non-empty clouds. + * The changes of the returned cloud are not mirrored back to this one. + * \return shared pointer to the copy of the cloud + */ + inline Ptr + makeShared () const { return Ptr (new PointCloud (*this)); } + + protected: + // This is motivated by ROS integration. Users should not need to access mapping_. + PCL_DEPRECATED("rewrite your code to avoid using this protected field") shared_ptr mapping_; + + friend shared_ptr& detail::getMapping(pcl::PointCloud &p); + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + namespace detail + { + template shared_ptr& + getMapping (pcl::PointCloud& p) + { + return (p.mapping_); + } + } // namespace detail + + template std::ostream& + operator << (std::ostream& s, const pcl::PointCloud &p) + { + s << "header: " << p.header << std::endl; + s << "points[]: " << p.points.size () << std::endl; + s << "width: " << p.width << std::endl; + s << "height: " << p.height << std::endl; + s << "is_dense: " << p.is_dense << std::endl; + s << "sensor origin (xyz): [" << + p.sensor_origin_.x () << ", " << + p.sensor_origin_.y () << ", " << + p.sensor_origin_.z () << "] / orientation (xyzw): [" << + p.sensor_orientation_.x () << ", " << + p.sensor_orientation_.y () << ", " << + p.sensor_orientation_.z () << ", " << + p.sensor_orientation_.w () << "]" << + std::endl; + return (s); + } +} + +#define PCL_INSTANTIATE_PointCloud(T) template class PCL_EXPORTS pcl::PointCloud; diff --git a/deps_install/include/pcl-1.10/pcl/point_representation.h b/deps_install/include/pcl-1.10/pcl/point_representation.h new file mode 100755 index 0000000..0a24ad8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/point_representation.h @@ -0,0 +1,579 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace pcl +{ + /** \brief @b PointRepresentation provides a set of methods for converting a point structs/object into an + * n-dimensional vector. + * \note This is an abstract class. Subclasses must set nr_dimensions_ to the appropriate value in the constructor + * and provide an implementation of the pure virtual copyToFloatArray method. + * \author Michael Dixon + */ + template + class PointRepresentation + { + protected: + /** \brief The number of dimensions in this point's vector (i.e. the "k" in "k-D") */ + int nr_dimensions_ = 0; + /** \brief A vector containing the rescale factor to apply to each dimension. */ + std::vector alpha_; + /** \brief Indicates whether this point representation is trivial. It is trivial if and only if the following + * conditions hold: + * - the relevant data consists only of float values + * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array + * - sizeof(PointT) is a multiple of sizeof(float) + * In short, a trivial point representation converts the input point to a float array that is the same as if + * the point was reinterpret_casted to a float array of length nr_dimensions_ . This value says that this + * representation can be trivial; it is only trivial if setRescaleValues() has not been set. + */ + bool trivial_ = false; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty destructor */ + virtual ~PointRepresentation () = default; + //TODO: check if copy and move constructors / assignment operators are needed + + /** \brief Copy point data from input point to a float array. This method must be overridden in all subclasses. + * \param[in] p The input point + * \param[out] out A pointer to a float array. + */ + virtual void copyToFloatArray (const PointT &p, float *out) const = 0; + + /** \brief Returns whether this point representation is trivial. It is trivial if and only if the following + * conditions hold: + * - the relevant data consists only of float values + * - the vectorize operation directly copies the first nr_dimensions_ elements of PointT to the out array + * - sizeof(PointT) is a multiple of sizeof(float) + * In short, a trivial point representation converts the input point to a float array that is the same as if + * the point was reinterpret_casted to a float array of length nr_dimensions_ . */ + inline bool isTrivial() const { return trivial_ && alpha_.empty (); } + + /** \brief Verify that the input point is valid. + * \param p The point to validate + */ + virtual bool + isValid (const PointT &p) const + { + bool is_valid = true; + + if (trivial_) + { + const float* temp = reinterpret_cast(&p); + + for (int i = 0; i < nr_dimensions_; ++i) + { + if (!std::isfinite (temp[i])) + { + is_valid = false; + break; + } + } + } + else + { + float *temp = new float[nr_dimensions_]; + copyToFloatArray (p, temp); + + for (int i = 0; i < nr_dimensions_; ++i) + { + if (!std::isfinite (temp[i])) + { + is_valid = false; + break; + } + } + delete [] temp; + } + return (is_valid); + } + + /** \brief Convert input point into a vector representation, rescaling by \a alpha. + * \param[in] p the input point + * \param[out] out The output vector. Can be of any type that implements the [] operator. + */ + template void + vectorize (const PointT &p, OutputType &out) const + { + float *temp = new float[nr_dimensions_]; + copyToFloatArray (p, temp); + if (alpha_.empty ()) + { + for (int i = 0; i < nr_dimensions_; ++i) + out[i] = temp[i]; + } + else + { + for (int i = 0; i < nr_dimensions_; ++i) + out[i] = temp[i] * alpha_[i]; + } + delete [] temp; + } + + /** \brief Set the rescale values to use when vectorizing points + * \param[in] rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator. + */ + void + setRescaleValues (const float *rescale_array) + { + alpha_.resize (nr_dimensions_); + std::copy_n(rescale_array, nr_dimensions_, alpha_.begin()); + } + + /** \brief Return the number of dimensions in the point's vector representation. */ + inline int getNumberOfDimensions () const { return (nr_dimensions_); } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b DefaultPointRepresentation extends PointRepresentation to define default behavior for common point types. + */ + template + class DefaultPointRepresentation : public PointRepresentation + { + using PointRepresentation ::nr_dimensions_; + using PointRepresentation ::trivial_; + + public: + // Boost shared pointers + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + DefaultPointRepresentation () + { + // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions + nr_dimensions_ = sizeof (PointDefault) / sizeof (float); + // Limit the default representation to the first 3 elements + if (nr_dimensions_ > 3) nr_dimensions_ = 3; + + trivial_ = true; + } + + ~DefaultPointRepresentation () {} + + inline Ptr + makeShared () const + { + return (Ptr (new DefaultPointRepresentation (*this))); + } + + void + copyToFloatArray (const PointDefault &p, float * out) const override + { + // If point type is unknown, treat it as a struct/array of floats + const float* ptr = reinterpret_cast (&p); + std::copy_n(ptr, nr_dimensions_, out); + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the + * default behavior for feature descriptor types (i.e., copy each element of each field into a float array). + */ + template + class DefaultFeatureRepresentation : public PointRepresentation + { + protected: + using PointRepresentation ::nr_dimensions_; + + private: + struct IncrementFunctor + { + IncrementFunctor (int &n) : n_ (n) + { + n_ = 0; + } + + template inline void operator () () + { + n_ += pcl::traits::datatype::size; + } + + private: + int &n_; + }; + + struct NdCopyPointFunctor + { + using Pod = typename traits::POD::type; + + NdCopyPointFunctor (const PointDefault &p1, float * p2) + : p1_ (reinterpret_cast(p1)), p2_ (p2), f_idx_ (0) { } + + template inline void operator() () + { + using FieldT = typename pcl::traits::datatype::type; + const int NrDims = pcl::traits::datatype::size; + Helper::copyPoint (p1_, p2_, f_idx_); + } + + // Copy helper for scalar fields + template + struct Helper + { + static void copyPoint (const Pod &p1, float * p2, int &f_idx) + { + const std::uint8_t * data_ptr = reinterpret_cast (&p1) + + pcl::traits::offset::value; + p2[f_idx++] = *reinterpret_cast (data_ptr); + } + }; + // Copy helper for array fields + template + struct Helper + { + static void copyPoint (const Pod &p1, float * p2, int &f_idx) + { + const std::uint8_t * data_ptr = reinterpret_cast (&p1) + + pcl::traits::offset::value; + int nr_dims = NrDims; + const FieldT * array = reinterpret_cast (data_ptr); + for (int i = 0; i < nr_dims; ++i) + { + p2[f_idx++] = array[i]; + } + } + }; + + private: + const Pod &p1_; + float * p2_; + int f_idx_; + }; + + public: + // Boost shared pointers + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + using FieldList = typename pcl::traits::fieldList::type; + + DefaultFeatureRepresentation () + { + nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented + pcl::for_each_type (IncrementFunctor (nr_dimensions_)); + } + + inline Ptr + makeShared () const + { + return (Ptr (new DefaultFeatureRepresentation (*this))); + } + + void + copyToFloatArray (const PointDefault &p, float * out) const override + { + pcl::for_each_type (NdCopyPointFunctor (p, out)); + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 3; + trivial_ = true; + } + + void + copyToFloatArray (const PointXYZ &p, float * out) const override + { + out[0] = p.x; + out[1] = p.y; + out[2] = p.z; + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 3; + trivial_ = true; + } + + void + copyToFloatArray (const PointXYZI &p, float * out) const override + { + out[0] = p.x; + out[1] = p.y; + out[2] = p.z; + // By default, p.intensity is not part of the PointXYZI vectorization + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 3; + trivial_ = true; + } + + void + copyToFloatArray (const PointNormal &p, float * out) const override + { + out[0] = p.x; + out[1] = p.y; + out[2] = p.z; + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 4; + trivial_ = true; + } + + void + copyToFloatArray (const PPFSignature &p, float * out) const override + { + out[0] = p.f1; + out[1] = p.f2; + out[2] = p.f3; + out[3] = p.f4; + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 36; + trivial_=false; + } + + void + copyToFloatArray (const Narf36 &p, float * out) const override + { + for (int i = 0; i < nr_dimensions_; ++i) + out[i] = p.descriptor[i]; + } + }; + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public DefaultFeatureRepresentation + {}; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 1980; + } + + void + copyToFloatArray (const ShapeContext1980 &p, float * out) const override + { + for (int i = 0; i < nr_dimensions_; ++i) + out[i] = p.descriptor[i]; + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 1960; + } + + void + copyToFloatArray (const UniqueShapeContext1960 &p, float * out) const override + { + for (int i = 0; i < nr_dimensions_; ++i) + out[i] = p.descriptor[i]; + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 352; + } + + void + copyToFloatArray (const SHOT352 &p, float * out) const override + { + for (int i = 0; i < nr_dimensions_; ++i) + out[i] = p.descriptor[i]; + } + }; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template <> + class DefaultPointRepresentation : public PointRepresentation + { + public: + DefaultPointRepresentation () + { + nr_dimensions_ = 1344; + } + + void + copyToFloatArray (const SHOT1344 &p, float * out) const override + { + for (int i = 0; i < nr_dimensions_; ++i) + out[i] = p.descriptor[i]; + } + }; + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point. + */ + template + class CustomPointRepresentation : public PointRepresentation + { + using PointRepresentation ::nr_dimensions_; + + public: + // Boost shared pointers + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor + * \param[in] max_dim the maximum number of dimensions to use + * \param[in] start_dim the starting dimension + */ + CustomPointRepresentation (const int max_dim = 3, const int start_dim = 0) + : max_dim_(max_dim), start_dim_(start_dim) + { + // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions + nr_dimensions_ = static_cast (sizeof (PointDefault) / sizeof (float)) - start_dim_; + // Limit the default representation to the first 3 elements + if (nr_dimensions_ > max_dim_) + nr_dimensions_ = max_dim_; + } + + inline Ptr + makeShared () const + { + return Ptr (new CustomPointRepresentation (*this)); + } + + /** \brief Copy the point data into a float array + * \param[in] p the input point + * \param[out] out the resultant output array + */ + virtual void + copyToFloatArray (const PointDefault &p, float *out) const + { + // If point type is unknown, treat it as a struct/array of floats + const float *ptr = (reinterpret_cast (&p)) + start_dim_; + std::copy_n(ptr, nr_dimensions_, out); + } + + protected: + /** \brief Use at most this many dimensions (i.e. the "k" in "k-D" is at most max_dim_) -- \note float fields are assumed */ + int max_dim_; + /** \brief Use dimensions only starting with this one (i.e. the "k" in "k-D" is = dim - start_dim_) -- \note float fields are assumed */ + int start_dim_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/point_traits.h b/deps_install/include/pcl-1.10/pcl/point_traits.h new file mode 100755 index 0000000..1ff6235 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/point_traits.h @@ -0,0 +1,359 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#include "pcl/pcl_macros.h" + +#include +#include + +// This is required for the workaround at line 109 +#ifdef _MSC_VER +#include +#include +#endif + +#include + +namespace pcl +{ + namespace deprecated + { + /** \class DeprecatedType + * \brief A dummy type to aid in template parameter deprecation + */ + struct T {}; + } + + namespace fields + { + // Tag types get put in this namespace + } + + namespace traits + { + // Metafunction to return enum value representing a type + template struct asEnum {}; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::INT8; }; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::UINT8; }; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::INT16; }; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::UINT16; }; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::INT32; }; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::UINT32; }; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::FLOAT32; }; + template<> struct asEnum { static const std::uint8_t value = pcl::PCLPointField::FLOAT64; }; + + // Metafunction to return type of enum value + template struct asType {}; + template<> struct asType { using type = std::int8_t; }; + template<> struct asType { using type = std::uint8_t; }; + template<> struct asType { using type = std::int16_t; }; + template<> struct asType { using type = std::uint16_t; }; + template<> struct asType { using type = std::int32_t; }; + template<> struct asType { using type = std::uint32_t; }; + template<> struct asType { using type = float; }; + template<> struct asType { using type = double; }; + + // Metafunction to decompose a type (possibly of array of any number of dimensions) into + // its scalar type and total number of elements. + template struct decomposeArray + { + using type = std::remove_all_extents_t; + static const std::uint32_t value = sizeof (T) / sizeof (type); + }; + + // For non-POD point types, this is specialized to return the corresponding POD type. + template + struct POD + { + using type = PointT; + }; + +#ifdef _MSC_VER + + /* Sometimes when calling functions like `copyPoint()` or `copyPointCloud` + * without explicitly specifying point types, MSVC deduces them to be e.g. + * `Eigen::internal::workaround_msvc_stl_support` instead of + * plain `pcl::PointXYZ`. Subsequently these types are passed to meta- + * functions like `has_field` or `fieldList` and make them choke. This hack + * makes use of the fact that internally `fieldList` always applies `POD` to + * its argument type. This specialization therefore allows to unwrap the + * contained point type. */ + template + struct POD > + { + using type = PointT; + }; + +#endif + + // name + /* This really only depends on Tag, but we go through some gymnastics to avoid ODR violations. + We template it on the point type PointT to avoid ODR violations when registering multiple + point types with shared tags. + The dummy parameter is so we can partially specialize name on PointT and Tag but leave it + templated on dummy. Each specialization declares a static char array containing the tag + name. The definition of the static member would conflict when linking multiple translation + units that include the point type registration. But when the static member definition is + templated (on dummy), we sidestep the ODR issue. + */ + template + struct name : name::type, Tag, dummy> + { + // Contents of specialization: + // static const char value[]; + + // Avoid infinite compile-time recursion + BOOST_MPL_ASSERT_MSG((!std::is_same::type>::value), + POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&)); + }; + + // offset + template + struct offset : offset::type, Tag> + { + // Contents of specialization: + // static const std::size_t value; + + // Avoid infinite compile-time recursion + BOOST_MPL_ASSERT_MSG((!std::is_same::type>::value), + POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&)); + }; + + // datatype + template + struct datatype : datatype::type, Tag> + { + // Contents of specialization: + // using type = ...; + // static const std::uint8_t value; + // static const std::uint32_t size; + + // Avoid infinite compile-time recursion + BOOST_MPL_ASSERT_MSG((!std::is_same::type>::value), + POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&)); + }; + + // fields + template + struct fieldList : fieldList::type> + { + // Contents of specialization: + // using type = boost::mpl::vector<...>; + + // Avoid infinite compile-time recursion + BOOST_MPL_ASSERT_MSG((!std::is_same::type>::value), + POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&)); + }; + } //namespace traits + + // Return true if the PCLPointField matches the expected name and data type. + // Written as a struct to allow partially specializing on Tag. + template + struct FieldMatches + { + bool operator() (const pcl::PCLPointField& field) + { + return (field.name == traits::name::value && + field.datatype == traits::datatype::value && + (field.count == traits::datatype::size || + ((field.count == 0) && (traits::datatype::size == 1 /* see bug #821 */)))); + } + }; + + /** \brief A helper functor that can copy a specific value if the given field exists. + * + * \note In order to actually copy the value an instance of this functor should be passed + * to a pcl::for_each_type loop. See the example below. + * + * \code + * PointInT p; + * bool exists; + * float value; + * using FieldList = typename pcl::traits::fieldList::type; + * pcl::for_each_type (pcl::CopyIfFieldExists (p, "intensity", exists, value)); + * \endcode + */ + template + struct CopyIfFieldExists + { + using Pod = typename traits::POD::type; + + /** \brief Constructor. + * \param[in] pt the input point + * \param[in] field the name of the field + * \param[out] exists set to true if the field exists, false otherwise + * \param[out] value the copied field value + */ + CopyIfFieldExists (const PointInT &pt, + const std::string &field, + bool &exists, + OutT &value) + : pt_ (reinterpret_cast(pt)), name_ (field), exists_ (exists), value_ (value) + { + exists_ = false; + } + + /** \brief Constructor. + * \param[in] pt the input point + * \param[in] field the name of the field + * \param[out] value the copied field value + */ + CopyIfFieldExists (const PointInT &pt, + const std::string &field, + OutT &value) + : pt_ (reinterpret_cast(pt)), name_ (field), exists_ (exists_tmp_), value_ (value) + { + } + + /** \brief Operator. Data copy happens here. */ + template inline void + operator() () + { + if (name_ == pcl::traits::name::value) + { + exists_ = true; + using T = typename pcl::traits::datatype::type; + const std::uint8_t* data_ptr = reinterpret_cast(&pt_) + pcl::traits::offset::value; + value_ = static_cast (*reinterpret_cast(data_ptr)); + } + } + + private: + const Pod &pt_; + const std::string &name_; + bool &exists_; + // Bogus entry + bool exists_tmp_; + OutT &value_; + }; + + /** \brief A helper functor that can set a specific value in a field if the field exists. + * + * \note In order to actually set the value an instance of this functor should be passed + * to a pcl::for_each_type loop. See the example below. + * + * \code + * PointT p; + * using FieldList = typename pcl::traits::fieldList::type; + * pcl::for_each_type (pcl::SetIfFieldExists (p, "intensity", 42.0f)); + * \endcode + */ + template + struct SetIfFieldExists + { + using Pod = typename traits::POD::type; + + /** \brief Constructor. + * \param[in] pt the input point + * \param[in] field the name of the field + * \param[out] value the value to set + */ + SetIfFieldExists (PointOutT &pt, + const std::string &field, + const InT &value) + : pt_ (reinterpret_cast(pt)), name_ (field), value_ (value) + { + } + + /** \brief Operator. Data copy happens here. */ + template inline void + operator() () + { + if (name_ == pcl::traits::name::value) + { + using T = typename pcl::traits::datatype::type; + std::uint8_t* data_ptr = reinterpret_cast(&pt_) + pcl::traits::offset::value; + *reinterpret_cast(data_ptr) = static_cast (value_); + } + } + + private: + Pod &pt_; + const std::string &name_; + const InT &value_; + }; + + /** \brief Set the value at a specified field in a point + * \param[out] pt the point to set the value to + * \param[in] field_offset the offset of the field + * \param[in] value the value to set + */ + template inline void + setFieldValue (PointT &pt, std::size_t field_offset, const ValT &value) + { + std::uint8_t* data_ptr = reinterpret_cast(&pt) + field_offset; + *reinterpret_cast(data_ptr) = value; + } + + /** \brief Get the value at a specified field in a point + * \param[in] pt the point to get the value from + * \param[in] field_offset the offset of the field + * \param[out] value the value to retrieve + */ + template inline void + getFieldValue (const PointT &pt, std::size_t field_offset, ValT &value) + { + const std::uint8_t* data_ptr = reinterpret_cast(&pt) + field_offset; + value = *reinterpret_cast(data_ptr); + } + + template using void_t = void; // part of std in c++17 + +#ifdef DOXYGEN_ONLY + + /** + * \brief Tests at compile time if type T has a custom allocator + * + * \see pcl::make_shared, PCL_MAKE_ALIGNED_OPERATOR_NEW + * \tparam T Type of the object to test + */ + template struct has_custom_allocator; + +#else + + template > struct has_custom_allocator : std::false_type {}; + template struct has_custom_allocator> : std::true_type {}; + +#endif +} diff --git a/deps_install/include/pcl-1.10/pcl/point_types.h b/deps_install/include/pcl-1.10/pcl/point_types.h new file mode 100755 index 0000000..02c4d2b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/point_types.h @@ -0,0 +1,914 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +/** + * \file pcl/point_types.h + * Defines all the PCL implemented PointT point type structures + * \ingroup common + */ + +// We're doing a lot of black magic with Boost here, so disable warnings in Maintainer mode, as we will never +// be able to fix them anyway +#if defined _MSC_VER + #pragma warning(disable: 4201) +#endif +//#pragma warning(push, 1) +#if defined __GNUC__ +# pragma GCC system_header +#endif + +/** @{*/ +namespace pcl +{ + /** \brief Members: float x, y, z + * \ingroup common + */ + struct PointXYZ; + + /** \brief Members: rgba + * \ingroup common + */ + struct RGB; + + /** \brief Members: intensity (float) + * \ingroup common + */ + struct Intensity; + + /** \brief Members: intensity (std::uint8_t) + * \ingroup common + */ + struct Intensity8u; + + /** \brief Members: intensity (std::uint32_t) + * \ingroup common + */ + struct Intensity32u; + + /** \brief Members: float x, y, z, intensity + * \ingroup common + */ + struct PointXYZI; + + /** \brief Members: float x, y, z, uin32_t label + * \ingroup common + */ + struct PointXYZL; + + /** \brief Members: std::uint32_t label + * \ingroup common + */ + struct Label; + + /** \brief Members: float x, y, z; std::uint32_t rgba + * \ingroup common + */ + struct PointXYZRGBA; + + /** \brief Members: float x, y, z, rgb + * \ingroup common + */ + struct PointXYZRGB; + + /** \brief Members: float x, y, z, rgb, std::uint32_t label + * \ingroup common + */ + struct PointXYZRGBL; + + /** \brief Members: float x, y, z, h, s, v + * \ingroup common + */ + struct PointXYZHSV; + + /** \brief Members: float x, y + * \ingroup common + */ + struct PointXY; + + /** \brief Members: float u, v + * \ingroup common + */ + struct PointUV; + + /** \brief Members: float x, y, z, strength + * \ingroup common + */ + struct InterestPoint; + + /** \brief Members: float normal[3], curvature + * \ingroup common + */ + struct Normal; + + /** \brief Members: float normal[3] + * \ingroup common + */ + struct Axis; + + /** \brief Members: float x, y, z; float normal[3], curvature + * \ingroup common + */ + struct PointNormal; + + /** \brief Members: float x, y, z, rgb, normal[3], curvature + * \ingroup common + */ + struct PointXYZRGBNormal; + + /** \brief Members: float x, y, z, intensity, normal[3], curvature + * \ingroup common + */ + struct PointXYZINormal; + + /** \brief Members: float x, y, z, label, normal[3], curvature + * \ingroup common + */ + struct PointXYZLNormal; + + /** \brief Members: float x, y, z (union with float point[4]), range + * \ingroup common + */ + struct PointWithRange; + + /** \brief Members: float x, y, z, vp_x, vp_y, vp_z + * \ingroup common + */ + struct PointWithViewpoint; + + /** \brief Members: float j1, j2, j3 + * \ingroup common + */ + struct MomentInvariants; + + /** \brief Members: float r_min, r_max + * \ingroup common + */ + struct PrincipalRadiiRSD; + + /** \brief Members: std::uint8_t boundary_point + * \ingroup common + */ + struct Boundary; + + /** \brief Members: float principal_curvature[3], pc1, pc2 + * \ingroup common + */ + struct PrincipalCurvatures; + + /** \brief Members: float descriptor[352], rf[9] + * \ingroup common + */ + struct SHOT352; + + /** \brief Members: float descriptor[1344], rf[9] + * \ingroup common + */ + struct SHOT1344; + + /** \brief Members: Axis x_axis, y_axis, z_axis + * \ingroup common + */ + struct ReferenceFrame; + + /** \brief Members: float descriptor[1980], rf[9] + * \ingroup common + */ + struct ShapeContext1980; + + /** \brief Members: float descriptor[1960], rf[9] + * \ingroup common + */ + struct UniqueShapeContext1960; + + /** \brief Members: float pfh[125] + * \ingroup common + */ + struct PFHSignature125; + + /** \brief Members: float pfhrgb[250] + * \ingroup common + */ + struct PFHRGBSignature250; + + /** \brief Members: float f1, f2, f3, f4, alpha_m + * \ingroup common + */ + struct PPFSignature; + + /** \brief Members: float f1, f2, f3, f4, f5, f6, f7, f8, f9, f10, alpha_m + * \ingroup common + */ + struct CPPFSignature; + + /** \brief Members: float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio, alpha_m + * \ingroup common + */ + struct PPFRGBSignature; + + /** \brief Members: float values[12] + * \ingroup common + */ + struct NormalBasedSignature12; + + /** \brief Members: float fpfh[33] + * \ingroup common + */ + struct FPFHSignature33; + + /** \brief Members: float vfh[308] + * \ingroup common + */ + struct VFHSignature308; + + /** \brief Members: float grsd[21] + * \ingroup common + */ + struct GRSDSignature21; + + /** \brief Members: float esf[640] + * \ingroup common + */ + struct ESFSignature640; + + /** \brief Members: float gasd[512] + * \ingroup common + */ + struct GASDSignature512; + + /** \brief Members: float gasd[984] + * \ingroup common + */ + struct GASDSignature984; + + /** \brief Members: float gasd[7992] + * \ingroup common + */ + struct GASDSignature7992; + + /** \brief Members: float histogram[16] + * \ingroup common + */ + struct GFPFHSignature16; + + /** \brief Members: float scale; float orientation; std::uint8_t descriptor[64] + * \ingroup common + */ + struct BRISKSignature512; + + /** \brief Members: float x, y, z, roll, pitch, yaw; float descriptor[36] + * \ingroup common + */ + struct Narf36; + + /** \brief Data type to store extended information about a transition from foreground to backgroundSpecification of the fields for BorderDescription::traits. + * \ingroup common + */ + using BorderTraits = std::bitset<32>; + + /** \brief Specification of the fields for BorderDescription::traits. + * \ingroup common + */ + enum BorderTrait + { + BORDER_TRAIT__OBSTACLE_BORDER, BORDER_TRAIT__SHADOW_BORDER, BORDER_TRAIT__VEIL_POINT, + BORDER_TRAIT__SHADOW_BORDER_TOP, BORDER_TRAIT__SHADOW_BORDER_RIGHT, BORDER_TRAIT__SHADOW_BORDER_BOTTOM, + BORDER_TRAIT__SHADOW_BORDER_LEFT, BORDER_TRAIT__OBSTACLE_BORDER_TOP, BORDER_TRAIT__OBSTACLE_BORDER_RIGHT, + BORDER_TRAIT__OBSTACLE_BORDER_BOTTOM, BORDER_TRAIT__OBSTACLE_BORDER_LEFT, BORDER_TRAIT__VEIL_POINT_TOP, + BORDER_TRAIT__VEIL_POINT_RIGHT, BORDER_TRAIT__VEIL_POINT_BOTTOM, BORDER_TRAIT__VEIL_POINT_LEFT + }; + + /** \brief Members: int x, y; BorderTraits traits + * \ingroup common + */ + struct BorderDescription; + + /** \brief Members: float gradient[3] + * \ingroup common + */ + struct IntensityGradient; + + /** \brief Members: float histogram[N] + * \ingroup common + */ + template + struct Histogram; + + /** \brief Members: float x, y, z, scale, angle, response, octave + * \ingroup common + */ + struct PointWithScale; + + /** \brief Members: float x, y, z, normal[3], rgba, radius, confidence, curvature + * \ingroup common + */ + struct PointSurfel; + + /** \brief Members: float x, y, z, intensity, intensity_variance, height_variance + * \ingroup common + */ + struct PointDEM; +} + +/** @} */ + +#include // Include struct definitions + +// ============================== +// =====POINT_CLOUD_REGISTER===== +// ============================== + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_RGB, + (std::uint32_t, rgba, rgba) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::RGB, pcl::_RGB) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity, + (float, intensity, intensity) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity, pcl::_Intensity) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity8u, + (std::uint8_t, intensity, intensity) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity8u, pcl::_Intensity8u) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Intensity32u, + (std::uint32_t, intensity, intensity) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Intensity32u, pcl::_Intensity32u) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZ, + (float, x, x) + (float, y, y) + (float, z, z) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZ, pcl::_PointXYZ) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBA, + (float, x, x) + (float, y, y) + (float, z, z) + (std::uint32_t, rgba, rgba) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBA, pcl::_PointXYZRGBA) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGB, + (float, x, x) + (float, y, y) + (float, z, z) + (float, rgb, rgb) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGB, pcl::_PointXYZRGB) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBL, + (float, x, x) + (float, y, y) + (float, z, z) + (std::uint32_t, rgba, rgba) + (std::uint32_t, label, label) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBL, pcl::_PointXYZRGBL) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZHSV, + (float, x, x) + (float, y, y) + (float, z, z) + (float, h, h) + (float, s, s) + (float, v, v) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZHSV, pcl::_PointXYZHSV) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXY, + (float, x, x) + (float, y, y) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointUV, + (float, u, u) + (float, v, v) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::InterestPoint, + (float, x, x) + (float, y, y) + (float, z, z) + (float, strength, strength) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZI, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZI, pcl::_PointXYZI) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZL, + (float, x, x) + (float, y, y) + (float, z, z) + (std::uint32_t, label, label) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Label, + (std::uint32_t, label, label) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Normal, + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (float, curvature, curvature) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Normal, pcl::_Normal) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_Axis, + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::Axis, pcl::_Axis) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointNormal, + (float, x, x) + (float, y, y) + (float, z, z) + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (float, curvature, curvature) +) +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBNormal, + (float, x, x) + (float, y, y) + (float, z, z) + (float, rgb, rgb) + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (float, curvature, curvature) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBNormal, pcl::_PointXYZRGBNormal) +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZINormal, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (float, curvature, curvature) +) +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZLNormal, + (float, x, x) + (float, y, y) + (float, z, z) + (std::uint32_t, label, label) + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (float, curvature, curvature) +) +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithRange, + (float, x, x) + (float, y, y) + (float, z, z) + (float, range, range) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointWithViewpoint, + (float, x, x) + (float, y, y) + (float, z, z) + (float, vp_x, vp_x) + (float, vp_y, vp_y) + (float, vp_z, vp_z) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointWithViewpoint, pcl::_PointWithViewpoint) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::MomentInvariants, + (float, j1, j1) + (float, j2, j2) + (float, j3, j3) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PrincipalRadiiRSD, + (float, r_min, r_min) + (float, r_max, r_max) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Boundary, + (std::uint8_t, boundary_point, boundary_point) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PrincipalCurvatures, + (float, principal_curvature_x, principal_curvature_x) + (float, principal_curvature_y, principal_curvature_y) + (float, principal_curvature_z, principal_curvature_z) + (float, pc1, pc1) + (float, pc2, pc2) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PFHSignature125, + (float[125], histogram, pfh) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PFHRGBSignature250, + (float[250], histogram, pfhrgb) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFSignature, + (float, f1, f1) + (float, f2, f2) + (float, f3, f3) + (float, f4, f4) + (float, alpha_m, alpha_m) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::CPPFSignature, + (float, f1, f1) + (float, f2, f2) + (float, f3, f3) + (float, f4, f4) + (float, f5, f5) + (float, f6, f6) + (float, f7, f7) + (float, f8, f8) + (float, f9, f9) + (float, f10, f10) + (float, alpha_m, alpha_m) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PPFRGBSignature, + (float, f1, f1) + (float, f2, f2) + (float, f3, f3) + (float, f4, f4) + (float, r_ratio, r_ratio) + (float, g_ratio, g_ratio) + (float, b_ratio, b_ratio) + (float, alpha_m, alpha_m) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::NormalBasedSignature12, + (float[12], values, values) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ShapeContext1980, + (float[1980], descriptor, shape_context) + (float[9], rf, rf) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::UniqueShapeContext1960, + (float[1960], descriptor, shape_context) + (float[9], rf, rf) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::SHOT352, + (float[352], descriptor, shot) + (float[9], rf, rf) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::SHOT1344, + (float[1344], descriptor, shot) + (float[9], rf, rf) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::FPFHSignature33, + (float[33], histogram, fpfh) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::BRISKSignature512, + (float, scale, brisk_scale) + (float, orientation, brisk_orientation) + (unsigned char[64], descriptor, brisk_descriptor512) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::VFHSignature308, + (float[308], histogram, vfh) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::GRSDSignature21, + (float[21], histogram, grsd) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ESFSignature640, + (float[640], histogram, esf) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature512, + (float[512], histogram, gasd) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature984, + (float[984], histogram, gasd) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::GASDSignature7992, + (float[7992], histogram, gasd) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Narf36, + (float[36], descriptor, descriptor) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::GFPFHSignature16, + (float[16], histogram, gfpfh) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::IntensityGradient, + (float, gradient_x, gradient_x) + (float, gradient_y, gradient_y) + (float, gradient_z, gradient_z) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithScale, + (float, x, x) + (float, y, y) + (float, z, z) + (float, scale, scale) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointSurfel, + (float, x, x) + (float, y, y) + (float, z, z) + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (std::uint32_t, rgba, rgba) + (float, radius, radius) + (float, confidence, confidence) + (float, curvature, curvature) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_ReferenceFrame, + (float[3], x_axis, x_axis) + (float[3], y_axis, y_axis) + (float[3], z_axis, z_axis) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::ReferenceFrame, pcl::_ReferenceFrame) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointDEM, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (float, intensity_variance, intensity_variance) + (float, height_variance, height_variance) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointDEM, pcl::_PointDEM) + +namespace pcl +{ + // Allow float 'rgb' data to match to the newer uint32 'rgba' tag. This is so + // you can load old 'rgb' PCD files into e.g. a PointCloud. + template + struct FieldMatches + { + bool operator() (const pcl::PCLPointField& field) + { + if (field.name == "rgb") + { + // For fixing the alpha value bug #1141, the rgb field can also match + // uint32. + return ((field.datatype == pcl::PCLPointField::FLOAT32 || + field.datatype == pcl::PCLPointField::UINT32) && + field.count == 1); + } + else + { + return (field.name == traits::name::value && + field.datatype == traits::datatype::value && + field.count == traits::datatype::size); + } + } + }; + template + struct FieldMatches + { + bool operator() (const pcl::PCLPointField& field) + { + if (field.name == "rgba") + { + return (field.datatype == pcl::PCLPointField::UINT32 && + field.count == 1); + } + else + { + // For fixing the alpha value bug #1141, rgb can also match uint32 + return (field.name == traits::name::value && + (field.datatype == traits::datatype::value || + field.datatype == pcl::PCLPointField::UINT32) && + field.count == traits::datatype::size); + } + } + }; + + namespace traits + { + + /** \brief Metafunction to check if a given point type has a given field. + * + * Example usage at run-time: + * + * \code + * bool curvature_available = pcl::traits::has_field::value; + * \endcode + * + * Example usage at compile-time: + * + * \code + * BOOST_MPL_ASSERT_MSG ((pcl::traits::has_field::value), + * POINT_TYPE_SHOULD_HAVE_LABEL_FIELD, + * (PointT)); + * \endcode + */ + template + struct has_field : boost::mpl::contains::type, Field>::type + { }; + + /** Metafunction to check if a given point type has all given fields. */ + template + struct has_all_fields : boost::mpl::fold, + boost::mpl::and_ > >::type + { }; + + /** Metafunction to check if a given point type has any of the given fields. */ + template + struct has_any_field : boost::mpl::fold, + boost::mpl::or_ > >::type + { }; + + /** \brief Traits defined for ease of use with fields already registered before + * + * has_: struct with `value` datamember defined at compiletime + * has__v: constexpr boolean + * Has: concept modelling name alias for `enable_if` + */ + + /** Metafunction to check if a given point type has x and y fields. */ + template + struct has_xy : has_all_fields > + { }; + + template + constexpr auto has_xy_v = has_xy::value; + + template + using HasXY = std::enable_if_t, bool>; + + template + using HasNoXY = std::enable_if_t, bool>; + + /** Metafunction to check if a given point type has x, y, and z fields. */ + template + struct has_xyz : has_all_fields > + { }; + + template + constexpr auto has_xyz_v = has_xyz::value; + + template + using HasXYZ = std::enable_if_t, bool>; + + template + using HasNoXYZ = std::enable_if_t, bool>; + + /** Metafunction to check if a given point type has normal_x, normal_y, and + * normal_z fields. */ + template + struct has_normal : has_all_fields > + { }; + + template + constexpr auto has_normal_v = has_normal::value; + + template + using HasNormal = std::enable_if_t, bool>; + + template + using HasNoNormal = std::enable_if_t, bool>; + + /** Metafunction to check if a given point type has curvature field. */ + template + struct has_curvature : has_field + { }; + + template + constexpr auto has_curvature_v = has_curvature::value; + + template + using HasCurvature = std::enable_if_t, bool>; + + template + using HasNoCurvature = std::enable_if_t, bool>; + + /** Metafunction to check if a given point type has intensity field. */ + template + struct has_intensity : has_field + { }; + + template + constexpr auto has_intensity_v = has_intensity::value; + + template + using HasIntensity = std::enable_if_t, bool>; + + template + using HasNoIntensity = std::enable_if_t, bool>; + + /** Metafunction to check if a given point type has either rgb or rgba field. */ + template + struct has_color : has_any_field > + { }; + + template + constexpr auto has_color_v = has_color::value; + + template + using HasColor = std::enable_if_t, bool>; + + template + using HasNoColor = std::enable_if_t, bool>; + + /** Metafunction to check if a given point type has label field. */ + template + struct has_label : has_field + { }; + + template + constexpr auto has_label_v = has_label::value; + + template + using HasLabel = std::enable_if_t, bool>; + + template + using HasNoLabel = std::enable_if_t, bool>; + } + +} // namespace pcl + +// Not strictly required, merely to preserve API for PCL users < 1.4 +#include + +#if defined _MSC_VER + #pragma warning(default: 4201) +#endif +//#pragma warning(pop) diff --git a/deps_install/include/pcl-1.10/pcl/point_types_conversion.h b/deps_install/include/pcl-1.10/pcl/point_types_conversion.h new file mode 100755 index 0000000..28c8c48 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/point_types_conversion.h @@ -0,0 +1,395 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include + +#include +#include + +namespace pcl +{ + // r,g,b, i values are from 0 to 255 + // h = [0,360] + // s, v values are from 0 to 1 + // if s = 0 => h = 0 + + /** \brief Convert a XYZRGB point type to a XYZI + * \param[in] in the input XYZRGB point + * \param[out] out the output XYZI point + */ + inline void + PointXYZRGBtoXYZI (const PointXYZRGB& in, + PointXYZI& out) + { + out.x = in.x; out.y = in.y; out.z = in.z; + out.intensity = 0.299f * static_cast (in.r) + 0.587f * static_cast (in.g) + 0.114f * static_cast (in.b); + } + + /** \brief Convert a RGB point type to a I + * \param[in] in the input RGB point + * \param[out] out the output Intensity point + */ + inline void + PointRGBtoI (const RGB& in, + Intensity& out) + { + out.intensity = 0.299f * static_cast (in.r) + 0.587f * static_cast (in.g) + 0.114f * static_cast (in.b); + } + + /** \brief Convert a RGB point type to a I + * \param[in] in the input RGB point + * \param[out] out the output Intensity point + */ + inline void + PointRGBtoI (const RGB& in, + Intensity8u& out) + { + out.intensity = static_cast(0.299f * static_cast (in.r) + + 0.587f * static_cast (in.g) + 0.114f * static_cast (in.b)); + } + + /** \brief Convert a RGB point type to a I + * \param[in] in the input RGB point + * \param[out] out the output Intensity point + */ + inline void + PointRGBtoI (const RGB& in, + Intensity32u& out) + { + out.intensity = static_cast(0.299f * static_cast (in.r) + + 0.587f * static_cast (in.g) + 0.114f * static_cast (in.b)); + } + + /** \brief Convert a XYZRGB point type to a XYZHSV + * \param[in] in the input XYZRGB point + * \param[out] out the output XYZHSV point + */ + inline void + PointXYZRGBtoXYZHSV (const PointXYZRGB& in, + PointXYZHSV& out) + { + const unsigned char max = std::max (in.r, std::max (in.g, in.b)); + const unsigned char min = std::min (in.r, std::min (in.g, in.b)); + + out.x = in.x; out.y = in.y; out.z = in.z; + out.v = static_cast (max) / 255.f; + + if (max == 0) // division by zero + { + out.s = 0.f; + out.h = 0.f; // h = -1.f; + return; + } + + const float diff = static_cast (max - min); + out.s = diff / static_cast (max); + + if (min == max) // diff == 0 -> division by zero + { + out.h = 0; + return; + } + + if (max == in.r) out.h = 60.f * ( static_cast (in.g - in.b) / diff); + else if (max == in.g) out.h = 60.f * (2.f + static_cast (in.b - in.r) / diff); + else out.h = 60.f * (4.f + static_cast (in.r - in.g) / diff); // max == b + + if (out.h < 0.f) out.h += 360.f; + } + + /** \brief Convert a XYZRGBA point type to a XYZHSV + * \param[in] in the input XYZRGBA point + * \param[out] out the output XYZHSV point + * \todo include the A parameter but how? + */ + inline void + PointXYZRGBAtoXYZHSV (const PointXYZRGBA& in, + PointXYZHSV& out) + { + const unsigned char max = std::max (in.r, std::max (in.g, in.b)); + const unsigned char min = std::min (in.r, std::min (in.g, in.b)); + + out.x = in.x; out.y = in.y; out.z = in.z; + out.v = static_cast (max) / 255.f; + + if (max == 0) // division by zero + { + out.s = 0.f; + out.h = 0.f; + return; + } + + const float diff = static_cast (max - min); + out.s = diff / static_cast (max); + + if (min == max) // diff == 0 -> division by zero + { + out.h = 0; + return; + } + + if (max == in.r) out.h = 60.f * ( static_cast (in.g - in.b) / diff); + else if (max == in.g) out.h = 60.f * (2.f + static_cast (in.b - in.r) / diff); + else out.h = 60.f * (4.f + static_cast (in.r - in.g) / diff); // max == b + + if (out.h < 0.f) out.h += 360.f; + } + + /* \brief Convert a XYZHSV point type to a XYZRGB + * \param[in] in the input XYZHSV point + * \param[out] out the output XYZRGB point + */ + inline void + PointXYZHSVtoXYZRGB (const PointXYZHSV& in, + PointXYZRGB& out) + { + out.x = in.x; out.y = in.y; out.z = in.z; + if (in.s == 0) + { + out.r = out.g = out.b = static_cast (255 * in.v); + return; + } + float a = in.h / 60; + int i = static_cast (std::floor (a)); + float f = a - static_cast (i); + float p = in.v * (1 - in.s); + float q = in.v * (1 - in.s * f); + float t = in.v * (1 - in.s * (1 - f)); + + switch (i) + { + case 0: + { + out.r = static_cast (255 * in.v); + out.g = static_cast (255 * t); + out.b = static_cast (255 * p); + break; + } + case 1: + { + out.r = static_cast (255 * q); + out.g = static_cast (255 * in.v); + out.b = static_cast (255 * p); + break; + } + case 2: + { + out.r = static_cast (255 * p); + out.g = static_cast (255 * in.v); + out.b = static_cast (255 * t); + break; + } + case 3: + { + out.r = static_cast (255 * p); + out.g = static_cast (255 * q); + out.b = static_cast (255 * in.v); + break; + } + case 4: + { + out.r = static_cast (255 * t); + out.g = static_cast (255 * p); + out.b = static_cast (255 * in.v); + break; + } + default: + { + out.r = static_cast (255 * in.v); + out.g = static_cast (255 * p); + out.b = static_cast (255 * q); + break; + } + } + } + + /** \brief Convert a RGB point cloud to an Intensity + * \param[in] in the input RGB point cloud + * \param[out] out the output Intensity point cloud + */ + inline void + PointCloudRGBtoI (const PointCloud& in, + PointCloud& out) + { + out.width = in.width; + out.height = in.height; + for (const auto &point : in.points) + { + Intensity p; + PointRGBtoI (point, p); + out.points.push_back (p); + } + } + + /** \brief Convert a RGB point cloud to an Intensity + * \param[in] in the input RGB point cloud + * \param[out] out the output Intensity point cloud + */ + inline void + PointCloudRGBtoI (const PointCloud& in, + PointCloud& out) + { + out.width = in.width; + out.height = in.height; + for (const auto &point : in.points) + { + Intensity8u p; + PointRGBtoI (point, p); + out.points.push_back (p); + } + } + + /** \brief Convert a RGB point cloud to an Intensity + * \param[in] in the input RGB point cloud + * \param[out] out the output Intensity point cloud + */ + inline void + PointCloudRGBtoI (const PointCloud& in, + PointCloud& out) + { + out.width = in.width; + out.height = in.height; + for (const auto &point : in.points) + { + Intensity32u p; + PointRGBtoI (point, p); + out.points.push_back (p); + } + } + + /** \brief Convert a XYZRGB point cloud to a XYZHSV + * \param[in] in the input XYZRGB point cloud + * \param[out] out the output XYZHSV point cloud + */ + inline void + PointCloudXYZRGBtoXYZHSV (const PointCloud& in, + PointCloud& out) + { + out.width = in.width; + out.height = in.height; + for (const auto &point : in.points) + { + PointXYZHSV p; + PointXYZRGBtoXYZHSV (point, p); + out.points.push_back (p); + } + } + + /** \brief Convert a XYZRGB point cloud to a XYZHSV + * \param[in] in the input XYZRGB point cloud + * \param[out] out the output XYZHSV point cloud + */ + inline void + PointCloudXYZRGBAtoXYZHSV (const PointCloud& in, + PointCloud& out) + { + out.width = in.width; + out.height = in.height; + for (const auto &point : in.points) + { + PointXYZHSV p; + PointXYZRGBAtoXYZHSV (point, p); + out.points.push_back (p); + } + } + + /** \brief Convert a XYZRGB point cloud to a XYZI + * \param[in] in the input XYZRGB point cloud + * \param[out] out the output XYZI point cloud + */ + inline void + PointCloudXYZRGBtoXYZI (const PointCloud& in, + PointCloud& out) + { + out.width = in.width; + out.height = in.height; + for (const auto &point : in.points) + { + PointXYZI p; + PointXYZRGBtoXYZI (point, p); + out.points.push_back (p); + } + } + + /** \brief Convert registered Depth image and RGB image to PointCloudXYZRGBA + * \param[in] depth the input depth image as intensity points in float + * \param[in] image the input RGB image + * \param[in] focal the focal length + * \param[out] out the output pointcloud + * **/ + inline void + PointCloudDepthAndRGBtoXYZRGBA (const PointCloud& depth, + const PointCloud& image, + const float& focal, + PointCloud& out) + { + float bad_point = std::numeric_limits::quiet_NaN(); + std::size_t width_ = depth.width; + std::size_t height_ = depth.height; + float constant_ = 1.0f / focal; + + for (std::size_t v = 0; v < height_; v++) + { + for (std::size_t u = 0; u < width_; u++) + { + PointXYZRGBA pt; + float depth_ = depth.at (u, v).intensity; + + if (depth_ == 0) + { + pt.x = pt.y = pt.z = bad_point; + } + else + { + pt.z = depth_ * 0.001f; + pt.x = static_cast (u) * pt.z * constant_; + pt.y = static_cast (v) * pt.z * constant_; + } + pt.r = image.at (u, v).r; + pt.g = image.at (u, v).g; + pt.b = image.at (u, v).b; + + out.points.push_back (pt); + } + } + out.width = width_; + out.height = height_; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/range_image/bearing_angle_image.h b/deps_install/include/pcl-1.10/pcl/range_image/bearing_angle_image.h new file mode 100755 index 0000000..cea6249 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/range_image/bearing_angle_image.h @@ -0,0 +1,84 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Intelligent Robotics Lab, DLUT. + * Author: Qinghua Li, Yan Zhuang, Fei Yan + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Intelligent Robotics Lab, DLUT. nor the names + * of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * \file bearing_angle_image.h + * \Created on: July 07, 2012 + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief class BearingAngleImage is used as an interface to generate Bearing Angle(BA) image. + * \author: Qinghua Li (qinghua__li@163.com) + */ + class PCL_EXPORTS BearingAngleImage : public pcl::PointCloud + { + public: + // ===== TYPEDEFS ===== + using BaseClass = pcl::PointCloud; + + // =====CONSTRUCTOR & DESTRUCTOR===== + /** Constructor */ + BearingAngleImage (); + + public: + /** \brief Reset all values to an empty Bearing Angle image */ + void + reset (); + + /** \brief Calculate the angle between the laser beam and the segment joining two consecutive + * measurement points. + * \param point1 + * \param point2 + */ + double + getAngle (const PointXYZ &point1, const PointXYZ &point2); + + /** \brief Transform 3D point cloud into a 2D Bearing Angle(BA) image */ + void + generateBAImage (PointCloud& point_cloud); + + protected: + /**< This point is used to be able to return a reference to a unknown gray point */ + PointXYZRGBA unobserved_point_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image.hpp b/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image.hpp new file mode 100755 index 0000000..03f2ee8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image.hpp @@ -0,0 +1,1255 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_RANGE_IMAGE_IMPL_HPP_ +#define PCL_RANGE_IMAGE_IMPL_HPP_ + +#include +#include + +namespace pcl +{ + +///////////////////////////////////////////////////////////////////////// +inline float +RangeImage::asinLookUp (float value) +{ + return (asin_lookup_table[ + static_cast ( + static_cast (pcl_lrintf ( (static_cast (lookup_table_size-1) / 2.0f) * value)) + + static_cast (lookup_table_size-1) / 2.0f)]); +} + +///////////////////////////////////////////////////////////////////////// +inline float +RangeImage::atan2LookUp (float y, float x) +{ + if (x==0 && y==0) + return 0; + float ret; + if (std::abs (x) < std::abs (y)) + { + ret = atan_lookup_table[ + static_cast ( + static_cast (pcl_lrintf ( (static_cast (lookup_table_size-1) / 2.0f) * (x / y))) + + static_cast (lookup_table_size-1) / 2.0f)]; + ret = static_cast (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret); + } + else + ret = atan_lookup_table[ + static_cast ( + static_cast (pcl_lrintf ( (static_cast (lookup_table_size-1) / 2.0f) * (y / x))) + + static_cast (lookup_table_size-1)/2.0f)]; + if (x < 0) + ret = static_cast (y < 0 ? ret-M_PI : ret+M_PI); + + return (ret); +} + +///////////////////////////////////////////////////////////////////////// +inline float +RangeImage::cosLookUp (float value) +{ + int cell_idx = static_cast (pcl_lrintf ( (static_cast (lookup_table_size-1)) * std::abs (value) / (2.0f * static_cast (M_PI)))); + return (cos_lookup_table[cell_idx]); +} + +///////////////////////////////////////////////////////////////////////// +template void +RangeImage::createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution, + float max_angle_width, float max_angle_height, + const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, + float noise_level, float min_range, int border_size) +{ + createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height, + sensor_pose, coordinate_frame, noise_level, min_range, border_size); +} + +///////////////////////////////////////////////////////////////////////// +template void +RangeImage::createFromPointCloud (const PointCloudType& point_cloud, + float angular_resolution_x, float angular_resolution_y, + float max_angle_width, float max_angle_height, + const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, + float noise_level, float min_range, int border_size) +{ + setAngularResolution (angular_resolution_x, angular_resolution_y); + + width = static_cast (pcl_lrint (std::floor (max_angle_width*angular_resolution_x_reciprocal_))); + height = static_cast (pcl_lrint (std::floor (max_angle_height*angular_resolution_y_reciprocal_))); + + int full_width = static_cast (pcl_lrint (std::floor (pcl::deg2rad (360.0f)*angular_resolution_x_reciprocal_))), + full_height = static_cast (pcl_lrint (std::floor (pcl::deg2rad (180.0f)*angular_resolution_y_reciprocal_))); + image_offset_x_ = (full_width -static_cast (width) )/2; + image_offset_y_ = (full_height-static_cast (height))/2; + is_dense = false; + + getCoordinateFrameTransformation (coordinate_frame, to_world_system_); + to_world_system_ = sensor_pose * to_world_system_; + + to_range_image_system_ = to_world_system_.inverse (Eigen::Isometry); + //std::cout << "to_world_system_ is\n"< void +RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution, + const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, + const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, + float noise_level, float min_range, int border_size) +{ + createFromPointCloudWithKnownSize (point_cloud, angular_resolution, angular_resolution, point_cloud_center, point_cloud_radius, + sensor_pose, coordinate_frame, noise_level, min_range, border_size); +} + +///////////////////////////////////////////////////////////////////////// +template void +RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, + float angular_resolution_x, float angular_resolution_y, + const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, + const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, + float noise_level, float min_range, int border_size) +{ + //MEASURE_FUNCTION_TIME; + + //std::cout << "Starting to create range image from "< void +RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, + float angular_resolution, + float max_angle_width, float max_angle_height, + RangeImage::CoordinateFrame coordinate_frame, + float noise_level, float min_range, int border_size) +{ + createFromPointCloudWithViewpoints (point_cloud, angular_resolution, angular_resolution, + max_angle_width, max_angle_height, coordinate_frame, + noise_level, min_range, border_size); +} + +///////////////////////////////////////////////////////////////////////// +template void +RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, + float angular_resolution_x, float angular_resolution_y, + float max_angle_width, float max_angle_height, + RangeImage::CoordinateFrame coordinate_frame, + float noise_level, float min_range, int border_size) +{ + Eigen::Vector3f average_viewpoint = getAverageViewPoint (point_cloud); + Eigen::Affine3f sensor_pose = static_cast (Eigen::Translation3f (average_viewpoint)); + createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height, + sensor_pose, coordinate_frame, noise_level, min_range, border_size); +} + +///////////////////////////////////////////////////////////////////////// +template void +RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, float min_range, int& top, int& right, int& bottom, int& left) +{ + using PointType2 = typename PointCloudType::PointType; + const typename pcl::PointCloud::VectorType &points2 = point_cloud.points; + + unsigned int size = width*height; + int* counters = new int[size]; + ERASE_ARRAY (counters, size); + + top=height; right=-1; bottom=-1; left=width; + + float x_real, y_real, range_of_current_point; + int x, y; + for (const auto& point: points2) + { + if (!isFinite (point)) // Check for NAN etc + continue; + Vector3fMapConst current_point = point.getVector3fMap (); + + this->getImagePoint (current_point, x_real, y_real, range_of_current_point); + this->real2DToInt2D (x_real, y_real, x, y); + + if (range_of_current_point < min_range|| !isInImage (x, y)) + continue; + //std::cout << " ("< ("< "< "<::infinity (); + float image_point_range = getPoint (image_x, image_y).range; + if (std::isinf (image_point_range)) + { + if (image_point_range > 0.0f) + return std::numeric_limits::infinity (); + return -std::numeric_limits::infinity (); + } + return image_point_range - range; +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const +{ + image_x = (angle_x*cosLookUp (angle_y) + static_cast (M_PI))*angular_resolution_x_reciprocal_ - static_cast (image_offset_x_); + image_y = (angle_y + 0.5f*static_cast (M_PI))*angular_resolution_y_reciprocal_ - static_cast (image_offset_y_); +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::real2DToInt2D (float x, float y, int& xInt, int& yInt) const +{ + xInt = static_cast (pcl_lrintf (x)); + yInt = static_cast (pcl_lrintf (y)); +} + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::isInImage (int x, int y) const +{ + return (x >= 0 && x < static_cast (width) && y >= 0 && y < static_cast (height)); +} + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::isValid (int x, int y) const +{ + return isInImage (x,y) && std::isfinite (getPoint (x,y).range); +} + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::isValid (int index) const +{ + return std::isfinite (getPoint (index).range); +} + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::isObserved (int x, int y) const +{ + return !(!isInImage (x,y) || (std::isinf (getPoint (x,y).range) && getPoint (x,y).range < 0.0f)); +} + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::isMaxRange (int x, int y) const +{ + float range = getPoint (x,y).range; + return std::isinf (range) && range>0.0f; +} + +///////////////////////////////////////////////////////////////////////// +const PointWithRange& +RangeImage::getPoint (int image_x, int image_y) const +{ + if (!isInImage (image_x, image_y)) + return unobserved_point; + return points[image_y*width + image_x]; +} + +///////////////////////////////////////////////////////////////////////// +const PointWithRange& +RangeImage::getPointNoCheck (int image_x, int image_y) const +{ + return points[image_y*width + image_x]; +} + +///////////////////////////////////////////////////////////////////////// +PointWithRange& +RangeImage::getPointNoCheck (int image_x, int image_y) +{ + return points[image_y*width + image_x]; +} + +///////////////////////////////////////////////////////////////////////// +PointWithRange& +RangeImage::getPoint (int image_x, int image_y) +{ + return points[image_y*width + image_x]; +} + + +///////////////////////////////////////////////////////////////////////// +const PointWithRange& +RangeImage::getPoint (int index) const +{ + return points[index]; +} + +///////////////////////////////////////////////////////////////////////// +const PointWithRange& +RangeImage::getPoint (float image_x, float image_y) const +{ + int x, y; + real2DToInt2D (image_x, image_y, x, y); + return getPoint (x, y); +} + +///////////////////////////////////////////////////////////////////////// +PointWithRange& +RangeImage::getPoint (float image_x, float image_y) +{ + int x, y; + real2DToInt2D (image_x, image_y, x, y); + return getPoint (x, y); +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::getPoint (int image_x, int image_y, Eigen::Vector3f& point) const +{ + //std::cout << getPoint (image_x, image_y)<< " - " << getPoint (image_x, image_y).getVector3fMap ()<<"\n"; + point = getPoint (image_x, image_y).getVector3fMap (); +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::getPoint (int index, Eigen::Vector3f& point) const +{ + point = getPoint (index).getVector3fMap (); +} + +///////////////////////////////////////////////////////////////////////// +const Eigen::Map +RangeImage::getEigenVector3f (int x, int y) const +{ + return getPoint (x, y).getVector3fMap (); +} + +///////////////////////////////////////////////////////////////////////// +const Eigen::Map +RangeImage::getEigenVector3f (int index) const +{ + return getPoint (index).getVector3fMap (); +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const +{ + float angle_x, angle_y; + //std::cout << image_x<<","< (image_offset_y_))*angular_resolution_y_ - 0.5f*static_cast (M_PI); + float cos_angle_y = std::cos (angle_y); + angle_x = (cos_angle_y==0.0f ? 0.0f : ( (image_x+ static_cast (image_offset_x_))*angular_resolution_x_ - static_cast (M_PI))/cos_angle_y); +} + +///////////////////////////////////////////////////////////////////////// +float +RangeImage::getImpactAngle (int x1, int y1, int x2, int y2) const +{ + if (!isInImage (x1, y1) || !isInImage (x2,y2)) + return -std::numeric_limits::infinity (); + return getImpactAngle (getPoint (x1,y1),getPoint (x2,y2)); +} + +///////////////////////////////////////////////////////////////////////// +float +RangeImage::getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const { + if ( (std::isinf (point1.range)&&point1.range<0) || (std::isinf (point2.range)&&point2.range<0)) + return -std::numeric_limits::infinity (); + + float r1 = (std::min) (point1.range, point2.range), + r2 = (std::max) (point1.range, point2.range); + float impact_angle = static_cast (0.5f * M_PI); + + if (std::isinf (r2)) + { + if (r2 > 0.0f && !std::isinf (r1)) + impact_angle = 0.0f; + } + else if (!std::isinf (r1)) + { + float r1Sqr = r1*r1, + r2Sqr = r2*r2, + dSqr = squaredEuclideanDistance (point1, point2), + d = std::sqrt (dSqr); + float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d); + cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle)); + impact_angle = std::acos (cos_impact_angle); // Using the cosine rule + } + + if (point1.range > point2.range) + impact_angle = -impact_angle; + + return impact_angle; +} + +///////////////////////////////////////////////////////////////////////// +float +RangeImage::getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const +{ + float impact_angle = getImpactAngle (point1, point2); + if (std::isinf (impact_angle)) + return -std::numeric_limits::infinity (); + float ret = 1.0f - float (std::fabs (impact_angle)/ (0.5f*M_PI)); + if (impact_angle < 0.0f) + ret = -ret; + //if (std::abs (ret)>1) + //std::cout << PVARAC (impact_angle)<::infinity (); + return getAcutenessValue (getPoint (x1,y1), getPoint (x2,y2)); +} + +///////////////////////////////////////////////////////////////////////// +const Eigen::Vector3f +RangeImage::getSensorPos () const +{ + return Eigen::Vector3f (to_world_system_ (0,3), to_world_system_ (1,3), to_world_system_ (2,3)); +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const +{ + angle_change_x = angle_change_y = -std::numeric_limits::infinity (); + if (!isValid (x,y)) + return; + Eigen::Vector3f point; + getPoint (x, y, point); + Eigen::Affine3f transformation = getTransformationToViewerCoordinateFrame (point); + + if (isObserved (x-radius, y) && isObserved (x+radius, y)) + { + Eigen::Vector3f transformed_left; + if (isMaxRange (x-radius, y)) + transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f); + else + { + Eigen::Vector3f left; + getPoint (x-radius, y, left); + transformed_left = - (transformation * left); + //std::cout << PVARN (transformed_left[1]); + transformed_left[1] = 0.0f; + transformed_left.normalize (); + } + + Eigen::Vector3f transformed_right; + if (isMaxRange (x+radius, y)) + transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f); + else + { + Eigen::Vector3f right; + getPoint (x+radius, y, right); + transformed_right = transformation * right; + //std::cout << PVARN (transformed_right[1]); + transformed_right[1] = 0.0f; + transformed_right.normalize (); + } + angle_change_x = transformed_left.dot (transformed_right); + angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x)); + angle_change_x = std::acos (angle_change_x); + } + + if (isObserved (x, y-radius) && isObserved (x, y+radius)) + { + Eigen::Vector3f transformed_top; + if (isMaxRange (x, y-radius)) + transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f); + else + { + Eigen::Vector3f top; + getPoint (x, y-radius, top); + transformed_top = - (transformation * top); + //std::cout << PVARN (transformed_top[0]); + transformed_top[0] = 0.0f; + transformed_top.normalize (); + } + + Eigen::Vector3f transformed_bottom; + if (isMaxRange (x, y+radius)) + transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f); + else + { + Eigen::Vector3f bottom; + getPoint (x, y+radius, bottom); + transformed_bottom = transformation * bottom; + //std::cout << PVARN (transformed_bottom[0]); + transformed_bottom[0] = 0.0f; + transformed_bottom.normalize (); + } + angle_change_y = transformed_top.dot (transformed_bottom); + angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y)); + angle_change_y = std::acos (angle_change_y); + } +} + + +//inline float RangeImage::getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, const PointWithRange& neighbor2) const +//{ + //if (!std::isfinite (point.range) || (!std::isfinite (neighbor1.range)&&neighbor1.range<0) || (!std::isfinite (neighbor2.range)&&neighbor2.range<0)) + //return -std::numeric_limits::infinity (); + //if (std::isinf (neighbor1.range)) + //{ + //if (std::isinf (neighbor2.range)) + //return 0.0f; + //else + //return std::acos ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor2.x, neighbor2.y, neighbor2.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ())); + //} + //if (std::isinf (neighbor2.range)) + //return std::acos ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor1.x, neighbor1.y, neighbor1.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ())); + + //float d1_squared = squaredEuclideanDistance (point, neighbor1), + //d1 = std::sqrt (d1_squared), + //d2_squared = squaredEuclideanDistance (point, neighbor2), + //d2 = std::sqrt (d2_squared), + //d3_squared = squaredEuclideanDistance (neighbor1, neighbor2); + //float cos_surface_change = (d1_squared + d2_squared - d3_squared)/ (2.0f*d1*d2), + //surface_change = std::acos (cos_surface_change); + //if (std::isnan (surface_change)) + //surface_change = static_cast (M_PI); + ////std::cout << PVARN (point)<0.0f) // The first point is max range -> return a max range point + return; + weight_sum = 0.0f; + average_point.x = average_point.y = average_point.z = average_point.range = 0.0f; + } + + int x2=x, y2=y; + Vector4fMap average_point_eigen = average_point.getVector4fMap (); + //std::cout << PVARN (no_of_points); + for (int step=1; step::infinity (); + const PointWithRange& point1 = getPoint (x1,y1), + & point2 = getPoint (x2,y2); + if (std::isinf (point1.range) && std::isinf (point2.range)) + return 0.0f; + if (std::isinf (point1.range) || std::isinf (point2.range)) + return std::numeric_limits::infinity (); + return squaredEuclideanDistance (point1, point2); +} + +///////////////////////////////////////////////////////////////////////// +float +RangeImage::getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const +{ + float average_pixel_distance = 0.0f; + float weight=0.0f; + for (int i=0; i"<"< "<::infinity (); + const PointWithRange& point = getPoint (x, y); + int no_of_nearest_neighbors = static_cast (pow (static_cast ( (radius + 1.0)), 2.0)); + Eigen::Vector3f normal; + if (!getNormalForClosestNeighbors (x, y, radius, point, no_of_nearest_neighbors, normal, 1)) + return -std::numeric_limits::infinity (); + return deg2rad (90.0f) - std::acos (normal.dot ( (getSensorPos ()-getEigenVector3f (point)).normalized ())); +} + + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size) const +{ + VectorAverage3f vector_average; + for (int y2=y-radius; y2<=y+radius; y2+=step_size) + { + for (int x2=x-radius; x2<=x+radius; x2+=step_size) + { + if (!isInImage (x2, y2)) + continue; + const PointWithRange& point = getPoint (x2, y2); + if (!std::isfinite (point.range)) + continue; + vector_average.add (Eigen::Vector3f (point.x, point.y, point.z)); + } + } + if (vector_average.getNoOfSamples () < 3) + return false; + Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3; + vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3); + if (normal.dot ( (getSensorPos ()-vector_average.getMean ()).normalized ()) < 0.0f) + normal *= -1.0f; + return true; +} + +///////////////////////////////////////////////////////////////////////// +float +RangeImage::getNormalBasedAcutenessValue (int x, int y, int radius) const +{ + float impact_angle = getImpactAngleBasedOnLocalNormal (x, y, radius); + if (std::isinf (impact_angle)) + return -std::numeric_limits::infinity (); + float ret = 1.0f - static_cast ( (impact_angle / (0.5f * M_PI))); + //std::cout << PVARAC (impact_angle)< (pow (static_cast (radius + 1.0), 2.0)); + return getNormalForClosestNeighbors (x, y, radius, getPoint (x,y).getVector3fMap (), no_of_nearest_neighbors, normal); +} + +namespace +{ // Anonymous namespace, so that this is only accessible in this file + struct NeighborWithDistance + { // local struct to help us with sorting + float distance; + const PointWithRange* neighbor; + bool operator < (const NeighborWithDistance& other) const { return distancesetZero (); + if (mean_all_neighbors!=nullptr) + mean_all_neighbors->setZero (); + if (eigen_values_all_neighbors!=nullptr) + eigen_values_all_neighbors->setZero (); + + const auto sqrt_blocksize = 2 * radius + 1; + const auto blocksize = sqrt_blocksize * sqrt_blocksize; + + PointWithRange given_point; + given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2]; + + std::vector ordered_neighbors (blocksize); + int neighbor_counter = 0; + for (int y2=y-radius; y2<=y+radius; y2+=step_size) + { + for (int x2=x-radius; x2<=x+radius; x2+=step_size) + { + if (!isValid (x2, y2)) + continue; + NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter]; + neighbor_with_distance.neighbor = &getPoint (x2, y2); + neighbor_with_distance.distance = squaredEuclideanDistance (given_point, *neighbor_with_distance.neighbor); + ++neighbor_counter; + } + } + no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors); + + std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter); // Normal sort seems to be the fastest method (faster than partial_sort) + //std::stable_sort (ordered_neighbors, ordered_neighbors+neighbor_counter); + //std::partial_sort (ordered_neighbors, ordered_neighbors+no_of_closest_neighbors, ordered_neighbors+neighbor_counter); + + max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance; + //float max_distance_squared = max_closest_neighbor_distance_squared; + float max_distance_squared = max_closest_neighbor_distance_squared*4.0f; // Double the allowed distance value + //max_closest_neighbor_distance_squared = max_distance_squared; + + VectorAverage3f vector_average; + //for (int neighbor_idx=0; neighbor_idx max_distance_squared) + break; + //std::cout << ordered_neighbors[neighbor_idx].distance<<"\n"; + vector_average.add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ()); + } + + if (vector_average.getNoOfSamples () < 3) + return false; + //std::cout << PVARN (vector_average.getNoOfSamples ()); + Eigen::Vector3f eigen_vector2, eigen_vector3; + vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3); + Eigen::Vector3f viewing_direction = (getSensorPos ()-point).normalized (); + if (normal.dot (viewing_direction) < 0.0f) + normal *= -1.0f; + mean = vector_average.getMean (); + + if (normal_all_neighbors==nullptr) + return true; + + // Add remaining neighbors + for (int neighbor_idx2=neighbor_idx; neighbor_idx2getVector3fMap ()); + + vector_average.doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3); + //std::cout << PVARN (vector_average.getNoOfSamples ())<<".\n"; + if (normal_all_neighbors->dot (viewing_direction) < 0.0f) + *normal_all_neighbors *= -1.0f; + *mean_all_neighbors = vector_average.getMean (); + + //std::cout << viewing_direction[0]<<","<::infinity (); + + const auto sqrt_blocksize = 2 * radius + 1; + const auto blocksize = sqrt_blocksize * sqrt_blocksize; + std::vector neighbor_distances (blocksize); + int neighbor_counter = 0; + for (int y2=y-radius; y2<=y+radius; y2+=step_size) + { + for (int x2=x-radius; x2<=x+radius; x2+=step_size) + { + if (!isValid (x2, y2) || (x2==x&&y2==y)) + continue; + const PointWithRange& neighbor = getPointNoCheck (x2,y2); + float& neighbor_distance = neighbor_distances[neighbor_counter++]; + neighbor_distance = squaredEuclideanDistance (point, neighbor); + } + } + std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter); // Normal sort seems to be + // the fastest method (faster than partial_sort) + n = (std::min) (neighbor_counter, n); + return neighbor_distances[n-1]; +} + + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_nearest_neighbors, + Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane, int step_size) const +{ + Eigen::Vector3f mean, eigen_values; + float used_squared_max_distance; + bool ret = getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance, + normal, mean, eigen_values); + + if (ret) + { + if (point_on_plane != nullptr) + *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point; + } + return ret; +} + + +///////////////////////////////////////////////////////////////////////// +float +RangeImage::getCurvature (int x, int y, int radius, int step_size) const +{ + VectorAverage3f vector_average; + for (int y2=y-radius; y2<=y+radius; y2+=step_size) + { + for (int x2=x-radius; x2<=x+radius; x2+=step_size) + { + if (!isInImage (x2, y2)) + continue; + const PointWithRange& point = getPoint (x2, y2); + if (!std::isfinite (point.range)) + continue; + vector_average.add (Eigen::Vector3f (point.x, point.y, point.z)); + } + } + if (vector_average.getNoOfSamples () < 3) + return false; + Eigen::Vector3f eigen_values; + vector_average.doPCA (eigen_values); + return eigen_values[0]/eigen_values.sum (); +} + + +///////////////////////////////////////////////////////////////////////// +template Eigen::Vector3f +RangeImage::getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud) +{ + Eigen::Vector3f average_viewpoint (0,0,0); + int point_counter = 0; + for (const auto& point: point_cloud.points) + { + if (!std::isfinite (point.vp_x) || !std::isfinite (point.vp_y) || !std::isfinite (point.vp_z)) + continue; + average_viewpoint[0] += point.vp_x; + average_viewpoint[1] += point.vp_y; + average_viewpoint[2] += point.vp_z; + ++point_counter; + } + average_viewpoint /= point_counter; + + return average_viewpoint; +} + +///////////////////////////////////////////////////////////////////////// +bool +RangeImage::getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const +{ + if (!isValid (x, y)) + return false; + viewing_direction = (getPoint (x,y).getVector3fMap ()-getSensorPos ()).normalized (); + return true; +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const +{ + viewing_direction = (point-getSensorPos ()).normalized (); +} + +///////////////////////////////////////////////////////////////////////// +Eigen::Affine3f +RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const +{ + Eigen::Affine3f transformation; + getTransformationToViewerCoordinateFrame (point, transformation); + return transformation; +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const +{ + Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized (); + getTransformationFromTwoUnitVectorsAndOrigin (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, point, transformation); +} + +///////////////////////////////////////////////////////////////////////// +void +RangeImage::getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const +{ + Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized (); + getTransformationFromTwoUnitVectors (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, transformation); +} + +///////////////////////////////////////////////////////////////////////// +inline void +RangeImage::setAngularResolution (float angular_resolution) +{ + angular_resolution_x_ = angular_resolution_y_ = angular_resolution; + angular_resolution_x_reciprocal_ = angular_resolution_y_reciprocal_ = 1.0f / angular_resolution; +} + +///////////////////////////////////////////////////////////////////////// +inline void +RangeImage::setAngularResolution (float angular_resolution_x, float angular_resolution_y) +{ + angular_resolution_x_ = angular_resolution_x; + angular_resolution_x_reciprocal_ = 1.0f / angular_resolution_x_; + angular_resolution_y_ = angular_resolution_y; + angular_resolution_y_reciprocal_ = 1.0f / angular_resolution_y_; +} + +///////////////////////////////////////////////////////////////////////// +inline void +RangeImage::setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system) +{ + to_range_image_system_ = to_range_image_system; + to_world_system_ = to_range_image_system_.inverse (); +} + +///////////////////////////////////////////////////////////////////////// +inline void +RangeImage::getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const +{ + angular_resolution_x = angular_resolution_x_; + angular_resolution_y = angular_resolution_y_; +} + +///////////////////////////////////////////////////////////////////////// +template void +RangeImage::integrateFarRanges (const PointCloudType& far_ranges) +{ + float x_real, y_real, range_of_current_point; + for (const auto& point: far_ranges.points) + { + //if (!isFinite (point)) // Check for NAN etc + //continue; + Vector3fMapConst current_point = point.getVector3fMap (); + + this->getImagePoint (current_point, x_real, y_real, range_of_current_point); + + int floor_x = static_cast (pcl_lrint (std::floor (x_real))), + floor_y = static_cast (pcl_lrint (std::floor (y_real))), + ceil_x = static_cast (pcl_lrint (std::ceil (x_real))), + ceil_y = static_cast (pcl_lrint (std::ceil (y_real))); + + int neighbor_x[4], neighbor_y[4]; + neighbor_x[0]=floor_x; neighbor_y[0]=floor_y; + neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y; + neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y; + neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y; + + for (int i=0; i<4; ++i) + { + int x=neighbor_x[i], y=neighbor_y[i]; + if (!isInImage (x, y)) + continue; + PointWithRange& image_point = getPoint (x, y); + if (!std::isfinite (image_point.range)) + image_point.range = std::numeric_limits::infinity (); + } + } +} + +} // namespace end +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image_planar.hpp b/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image_planar.hpp new file mode 100755 index 0000000..30dda5d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image_planar.hpp @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_RANGE_IMAGE_PLANAR_IMPL_HPP_ +#define PCL_RANGE_IMAGE_PLANAR_IMPL_HPP_ + +#include +#include + +namespace pcl +{ + +///////////////////////////////////////////////////////////////////////// +template void +RangeImagePlanar::createFromPointCloudWithFixedSize (const PointCloudType& point_cloud, + int di_width, int di_height, + float di_center_x, float di_center_y, + float di_focal_length_x, float di_focal_length_y, + const Eigen::Affine3f& sensor_pose, + CoordinateFrame coordinate_frame, float noise_level, + float min_range) +{ + //std::cout << "Starting to create range image from "< (image_offset_x_)-center_x_)*focal_length_x_reciprocal_, + delta_y = (image_y+static_cast (image_offset_y_)-center_y_)*focal_length_y_reciprocal_; + point[2] = range / (std::sqrt (delta_x*delta_x + delta_y*delta_y + 1)); + point[0] = delta_x*point[2]; + point[1] = delta_y*point[2]; + point = to_world_system_ * point; +} + +///////////////////////////////////////////////////////////////////////// +inline void +RangeImagePlanar::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const +{ + Eigen::Vector3f transformedPoint = to_range_image_system_ * point; + if (transformedPoint[2]<=0) // Behind the observer? + { + image_x = image_y = range = -1.0f; + return; + } + range = transformedPoint.norm (); + + image_x = center_x_ + focal_length_x_*transformedPoint[0]/transformedPoint[2] - static_cast (image_offset_x_); + image_y = center_y_ + focal_length_y_*transformedPoint[1]/transformedPoint[2] - static_cast (image_offset_y_); +} + +} // namespace end + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image_spherical.hpp b/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image_spherical.hpp new file mode 100755 index 0000000..2c78575 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/range_image/impl/range_image_spherical.hpp @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012-, Open Perception, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include + +namespace pcl +{ + +///////////////////////////////////////////////////////////////////////// +void +RangeImageSpherical::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const +{ + float angle_x, angle_y; + getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y); + + float cosY = std::cos (angle_y); + point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * std::cos (angle_x)*cosY); + point = to_world_system_ * point; +} + +///////////////////////////////////////////////////////////////////////// +inline void +RangeImageSpherical::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const +{ + Eigen::Vector3f transformedPoint = to_range_image_system_ * point; + range = transformedPoint.norm (); + float angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]), + angle_y = asinLookUp (transformedPoint[1]/range); + getImagePointFromAngles (angle_x, angle_y, image_x, image_y); +} +///////////////////////////////////////////////////////////////////////// +void +RangeImageSpherical::getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const +{ + angle_y = (image_y+static_cast (image_offset_y_))*angular_resolution_y_ - 0.5f*static_cast (M_PI); + angle_x = ((image_x+ static_cast (image_offset_x_))*angular_resolution_x_ - static_cast (M_PI)); +} +///////////////////////////////////////////////////////////////////////// +void +RangeImageSpherical::getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const +{ + image_x = (angle_x + static_cast (M_PI))*angular_resolution_x_reciprocal_ - static_cast (image_offset_x_); + image_y = (angle_y + 0.5f*static_cast (M_PI))*angular_resolution_y_reciprocal_ - static_cast (image_offset_y_); +} +} // namespace end diff --git a/deps_install/include/pcl-1.10/pcl/range_image/range_image.h b/deps_install/include/pcl-1.10/pcl/range_image/range_image.h new file mode 100755 index 0000000..106188b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/range_image/range_image.h @@ -0,0 +1,841 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where + * a 3D scene was captured from a specific view point. + * \author Bastian Steder + * \ingroup range_image + */ + class RangeImage : public pcl::PointCloud + { + public: + // =====TYPEDEFS===== + using BaseClass = pcl::PointCloud; + using VectorOfEigenVector3f = std::vector >; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + enum CoordinateFrame + { + CAMERA_FRAME = 0, + LASER_FRAME = 1 + }; + + + // =====CONSTRUCTOR & DESTRUCTOR===== + /** Constructor */ + PCL_EXPORTS RangeImage (); + /** Destructor */ + PCL_EXPORTS virtual ~RangeImage () = default; + + // =====STATIC VARIABLES===== + /** The maximum number of openmp threads that can be used in this class */ + static int max_no_of_threads; + + // =====STATIC METHODS===== + /** \brief Get the size of a certain area when seen from the given pose + * \param viewer_pose an affine matrix defining the pose of the viewer + * \param center the center of the area + * \param radius the radius of the area + * \return the size of the area as viewed according to \a viewer_pose + */ + static inline float + getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, + float radius); + + /** \brief Get Eigen::Vector3f from PointWithRange + * \param point the input point + * \return an Eigen::Vector3f representation of the input point + */ + static inline Eigen::Vector3f + getEigenVector3f (const PointWithRange& point); + + /** \brief Get the transformation that transforms the given coordinate frame into CAMERA_FRAME + * \param coordinate_frame the input coordinate frame + * \param transformation the resulting transformation that warps \a coordinate_frame into CAMERA_FRAME + */ + PCL_EXPORTS static void + getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame, + Eigen::Affine3f& transformation); + + /** \brief Get the average viewpoint of a point cloud where each point carries viewpoint information as + * vp_x, vp_y, vp_z + * \param point_cloud the input point cloud + * \return the average viewpoint (as an Eigen::Vector3f) + */ + template static Eigen::Vector3f + getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud); + + /** \brief Check if the provided data includes far ranges and add them to far_ranges + * \param point_cloud_data a PCLPointCloud2 message containing the input cloud + * \param far_ranges the resulting cloud containing those points with far ranges + */ + PCL_EXPORTS static void + extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data, PointCloud& far_ranges); + + // =====METHODS===== + /** \brief Get a boost shared pointer of a copy of this */ + inline Ptr + makeShared () { return Ptr (new RangeImage (*this)); } + + /** \brief Reset all values to an empty range image */ + PCL_EXPORTS void + reset (); + + /** \brief Create the depth image from a point cloud + * \param point_cloud the input point cloud + * \param angular_resolution the angular difference (in radians) between the individual pixels in the image + * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor + * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor + * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to + * Eigen::Affine3f::Identity () ) + * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, + * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and + * will always take the minimum per cell. + * \param min_range the minimum visible range (defaults to 0) + * \param border_size the border size (defaults to 0) + */ + template void + createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution=pcl::deg2rad (0.5f), + float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f), + const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), + CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, + float min_range=0.0f, int border_size=0); + + /** \brief Create the depth image from a point cloud + * \param point_cloud the input point cloud + * \param angular_resolution_x the angular difference (in radians) between the + * individual pixels in the image in the x-direction + * \param angular_resolution_y the angular difference (in radians) between the + * individual pixels in the image in the y-direction + * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor + * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor + * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to + * Eigen::Affine3f::Identity () ) + * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, + * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and + * will always take the minimum per cell. + * \param min_range the minimum visible range (defaults to 0) + * \param border_size the border size (defaults to 0) + */ + template void + createFromPointCloud (const PointCloudType& point_cloud, + float angular_resolution_x=pcl::deg2rad (0.5f), float angular_resolution_y=pcl::deg2rad (0.5f), + float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f), + const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), + CoordinateFrame coordinate_frame=CAMERA_FRAME, + float noise_level=0.0f, float min_range=0.0f, int border_size=0); + + /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for + * faster calculation. + * \param point_cloud the input point cloud + * \param angular_resolution the angle (in radians) between each sample in the depth image + * \param point_cloud_center the center of bounding sphere + * \param point_cloud_radius the radius of the bounding sphere + * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to + * Eigen::Affine3f::Identity () ) + * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, + * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and + * will always take the minimum per cell. + * \param min_range the minimum visible range (defaults to 0) + * \param border_size the border size (defaults to 0) + */ + template void + createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution, + const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, + const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), + CoordinateFrame coordinate_frame=CAMERA_FRAME, + float noise_level=0.0f, float min_range=0.0f, int border_size=0); + + /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for + * faster calculation. + * \param point_cloud the input point cloud + * \param angular_resolution_x the angular difference (in radians) between the + * individual pixels in the image in the x-direction + * \param angular_resolution_y the angular difference (in radians) between the + * individual pixels in the image in the y-direction + * \param point_cloud_center the center of bounding sphere + * \param point_cloud_radius the radius of the bounding sphere + * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to + * Eigen::Affine3f::Identity () ) + * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, + * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and + * will always take the minimum per cell. + * \param min_range the minimum visible range (defaults to 0) + * \param border_size the border size (defaults to 0) + */ + template void + createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, + float angular_resolution_x, float angular_resolution_y, + const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, + const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (), + CoordinateFrame coordinate_frame=CAMERA_FRAME, + float noise_level=0.0f, float min_range=0.0f, int border_size=0); + + /** \brief Create the depth image from a point cloud, using the average viewpoint of the points + * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)). + * \param point_cloud the input point cloud + * \param angular_resolution the angle (in radians) between each sample in the depth image + * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor + * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor + * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, + * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and + * will always take the minimum per cell. + * \param min_range the minimum visible range (defaults to 0) + * \param border_size the border size (defaults to 0) + * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame + * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y + * to the bottom and z to the front) */ + template void + createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution, + float max_angle_width, float max_angle_height, + CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, + float min_range=0.0f, int border_size=0); + + /** \brief Create the depth image from a point cloud, using the average viewpoint of the points + * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)). + * \param point_cloud the input point cloud + * \param angular_resolution_x the angular difference (in radians) between the + * individual pixels in the image in the x-direction + * \param angular_resolution_y the angular difference (in radians) between the + * individual pixels in the image in the y-direction + * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor + * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor + * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, + * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and + * will always take the minimum per cell. + * \param min_range the minimum visible range (defaults to 0) + * \param border_size the border size (defaults to 0) + * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame + * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y + * to the bottom and z to the front) */ + template void + createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, + float angular_resolution_x, float angular_resolution_y, + float max_angle_width, float max_angle_height, + CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, + float min_range=0.0f, int border_size=0); + + /** \brief Create an empty depth image (filled with unobserved points) + * \param[in] angular_resolution the angle (in radians) between each sample in the depth image + * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () ) + * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg)) + * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg)) + */ + void + createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (), + RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f), + float angle_height=pcl::deg2rad (180.0f)); + + /** \brief Create an empty depth image (filled with unobserved points) + * \param angular_resolution_x the angular difference (in radians) between the + * individual pixels in the image in the x-direction + * \param angular_resolution_y the angular difference (in radians) between the + * individual pixels in the image in the y-direction + * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to Eigen::Affine3f::Identity () ) + * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) + * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg)) + * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg)) + */ + void + createEmpty (float angular_resolution_x, float angular_resolution_y, + const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (), + RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f), + float angle_height=pcl::deg2rad (180.0f)); + + /** \brief Integrate the given point cloud into the current range image using a z-buffer + * \param point_cloud the input point cloud + * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, + * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and + * will always take the minimum per cell. + * \param min_range the minimum visible range + * \param top returns the minimum y pixel position in the image where a point was added + * \param right returns the maximum x pixel position in the image where a point was added + * \param bottom returns the maximum y pixel position in the image where a point was added + * \param top returns the minimum y position in the image where a point was added + * \param left returns the minimum x pixel position in the image where a point was added + */ + template void + doZBuffer (const PointCloudType& point_cloud, float noise_level, + float min_range, int& top, int& right, int& bottom, int& left); + + /** \brief Integrates the given far range measurements into the range image */ + template void + integrateFarRanges (const PointCloudType& far_ranges); + + /** \brief Cut the range image to the minimal size so that it still contains all actual range readings. + * \param border_size allows increase from the minimal size by the specified number of pixels (defaults to 0) + * \param top if positive, this value overrides the position of the top edge (defaults to -1) + * \param right if positive, this value overrides the position of the right edge (defaults to -1) + * \param bottom if positive, this value overrides the position of the bottom edge (defaults to -1) + * \param left if positive, this value overrides the position of the left edge (defaults to -1) + */ + PCL_EXPORTS void + cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1); + + /** \brief Get all the range values in one float array of size width*height + * \return a pointer to a new float array containing the range values + * \note This method allocates a new float array; the caller is responsible for freeing this memory. + */ + PCL_EXPORTS float* + getRangesArray () const; + + /** Getter for the transformation from the world system into the range image system + * (the sensor coordinate frame) */ + inline const Eigen::Affine3f& + getTransformationToRangeImageSystem () const { return (to_range_image_system_); } + + /** Setter for the transformation from the range image system + * (the sensor coordinate frame) into the world system */ + inline void + setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system); + + /** Getter for the transformation from the range image system + * (the sensor coordinate frame) into the world system */ + inline const Eigen::Affine3f& + getTransformationToWorldSystem () const { return to_world_system_;} + + /** Getter for the angular resolution of the range image in x direction in radians per pixel. + * Provided for downwards compatibility */ + inline float + getAngularResolution () const { return angular_resolution_x_;} + + /** Getter for the angular resolution of the range image in x direction in radians per pixel. */ + inline float + getAngularResolutionX () const { return angular_resolution_x_;} + + /** Getter for the angular resolution of the range image in y direction in radians per pixel. */ + inline float + getAngularResolutionY () const { return angular_resolution_y_;} + + /** Getter for the angular resolution of the range image in x and y direction (in radians). */ + inline void + getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const; + + /** \brief Set the angular resolution of the range image + * \param angular_resolution the new angular resolution in x and y direction (in radians per pixel) + */ + inline void + setAngularResolution (float angular_resolution); + + /** \brief Set the angular resolution of the range image + * \param angular_resolution_x the new angular resolution in x direction (in radians per pixel) + * \param angular_resolution_y the new angular resolution in y direction (in radians per pixel) + */ + inline void + setAngularResolution (float angular_resolution_x, float angular_resolution_y); + + + /** \brief Return the 3D point with range at the given image position + * \param image_x the x coordinate + * \param image_y the y coordinate + * \return the point at the specified location (returns unobserved_point if outside of the image bounds) + */ + inline const PointWithRange& + getPoint (int image_x, int image_y) const; + + /** \brief Non-const-version of getPoint */ + inline PointWithRange& + getPoint (int image_x, int image_y); + + /** Return the 3d point with range at the given image position */ + inline const PointWithRange& + getPoint (float image_x, float image_y) const; + + /** Non-const-version of the above */ + inline PointWithRange& + getPoint (float image_x, float image_y); + + /** \brief Return the 3D point with range at the given image position. This methd performs no error checking + * to make sure the specified image position is inside of the image! + * \param image_x the x coordinate + * \param image_y the y coordinate + * \return the point at the specified location (program may fail if the location is outside of the image bounds) + */ + inline const PointWithRange& + getPointNoCheck (int image_x, int image_y) const; + + /** Non-const-version of getPointNoCheck */ + inline PointWithRange& + getPointNoCheck (int image_x, int image_y); + + /** Same as above */ + inline void + getPoint (int image_x, int image_y, Eigen::Vector3f& point) const; + + /** Same as above */ + inline void + getPoint (int index, Eigen::Vector3f& point) const; + + /** Same as above */ + inline const Eigen::Map + getEigenVector3f (int x, int y) const; + + /** Same as above */ + inline const Eigen::Map + getEigenVector3f (int index) const; + + /** Return the 3d point with range at the given index (whereas index=y*width+x) */ + inline const PointWithRange& + getPoint (int index) const; + + /** Calculate the 3D point according to the given image point and range */ + inline void + calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const; + /** Calculate the 3D point according to the given image point and the range value at the closest pixel */ + inline void + calculate3DPoint (float image_x, float image_y, PointWithRange& point) const; + + /** Calculate the 3D point according to the given image point and range */ + virtual inline void + calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const; + /** Calculate the 3D point according to the given image point and the range value at the closest pixel */ + inline void + calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const; + + /** Recalculate all 3D point positions according to their pixel position and range */ + PCL_EXPORTS void + recalculate3DPointPositions (); + + /** Get imagePoint from 3D point in world coordinates */ + inline virtual void + getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const; + + /** Same as above */ + inline void + getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const; + + /** Same as above */ + inline void + getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const; + + /** Same as above */ + inline void + getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const; + + /** Same as above */ + inline void + getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const; + + /** Same as above */ + inline void + getImagePoint (float x, float y, float z, float& image_x, float& image_y) const; + + /** Same as above */ + inline void + getImagePoint (float x, float y, float z, int& image_x, int& image_y) const; + + /** point_in_image will be the point in the image at the position the given point would be. Returns + * the range of the given point. */ + inline float + checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const; + + /** Returns the difference in range between the given point and the range of the point in the image + * at the position the given point would be. + * (Return value is point_in_image.range-given_point.range) */ + inline float + getRangeDifference (const Eigen::Vector3f& point) const; + + /** Get the image point corresponding to the given angles */ + inline void + getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const; + + /** Get the angles corresponding to the given image point */ + inline void + getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const; + + /** Transforms an image point in float values to an image point in int values */ + inline void + real2DToInt2D (float x, float y, int& xInt, int& yInt) const; + + /** Check if a point is inside of the image */ + inline bool + isInImage (int x, int y) const; + + /** Check if a point is inside of the image and has a finite range */ + inline bool + isValid (int x, int y) const; + + /** Check if a point has a finite range */ + inline bool + isValid (int index) const; + + /** Check if a point is inside of the image and has either a finite range or a max reading (range=INFINITY) */ + inline bool + isObserved (int x, int y) const; + + /** Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! */ + inline bool + isMaxRange (int x, int y) const; + + /** Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius. + * step_size determines how many pixels are used. 1 means all, 2 only every second, etc.. + * Returns false if it was unable to calculate a normal.*/ + inline bool + getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const; + + /** Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered. */ + inline bool + getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point, + int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const; + + /** Same as above */ + inline bool + getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, + int no_of_nearest_neighbors, Eigen::Vector3f& normal, + Eigen::Vector3f* point_on_plane=nullptr, int step_size=1) const; + + /** Same as above, using default values */ + inline bool + getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const; + + /** Same as above but extracts some more data and can also return the extracted + * information for all neighbors in radius if normal_all_neighbors is not NULL */ + inline bool + getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, + int no_of_closest_neighbors, int step_size, + float& max_closest_neighbor_distance_squared, + Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values, + Eigen::Vector3f* normal_all_neighbors=nullptr, + Eigen::Vector3f* mean_all_neighbors=nullptr, + Eigen::Vector3f* eigen_values_all_neighbors=nullptr) const; + + // Return the squared distance to the n-th neighbors of the point at x,y + inline float + getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const; + + /** Calculate the impact angle based on the sensor position and the two given points - will return + * -INFINITY if one of the points is unobserved */ + inline float + getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const; + //! Same as above + inline float + getImpactAngle (int x1, int y1, int x2, int y2) const; + + /** Extract a local normal (with a heuristic not to include background points) and calculate the impact + * angle based on this */ + inline float + getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const; + /** Uses the above function for every point in the image */ + PCL_EXPORTS float* + getImpactAngleImageBasedOnLocalNormals (int radius) const; + + /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) + * This uses getImpactAngleBasedOnLocalNormal + * Will return -INFINITY if no normal could be calculated */ + inline float + getNormalBasedAcutenessValue (int x, int y, int radius) const; + + /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg) + * will return -INFINITY if one of the points is unobserved */ + inline float + getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const; + //! Same as above + inline float + getAcutenessValue (int x1, int y1, int x2, int y2) const; + + /** Calculate getAcutenessValue for every point */ + PCL_EXPORTS void + getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x, + float*& acuteness_value_image_y) const; + + /** Calculates, how much the surface changes at a point. Pi meaning a flat suface and 0.0f + * would be a needle point */ + //inline float + // getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, + // const PointWithRange& neighbor2) const; + + /** Calculates, how much the surface changes at a point. 1 meaning a 90deg angle and 0 a flat suface */ + PCL_EXPORTS float + getSurfaceChange (int x, int y, int radius) const; + + /** Uses the above function for every point in the image */ + PCL_EXPORTS float* + getSurfaceChangeImage (int radius) const; + + /** Calculates, how much the surface changes at a point. Returns an angle [0.0f, PI] for x and y direction. + * A return value of -INFINITY means that a point was unobserved. */ + inline void + getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const; + + /** Uses the above function for every point in the image */ + PCL_EXPORTS void + getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const; + + /** Calculates the curvature in a point using pca */ + inline float + getCurvature (int x, int y, int radius, int step_size) const; + + //! Get the sensor position + inline const Eigen::Vector3f + getSensorPos () const; + + /** Sets all -INFINITY values to INFINITY */ + PCL_EXPORTS void + setUnseenToMaxRange (); + + //! Getter for image_offset_x_ + inline int + getImageOffsetX () const { return image_offset_x_;} + //! Getter for image_offset_y_ + inline int + getImageOffsetY () const { return image_offset_y_;} + + //! Setter for image offsets + inline void + setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;} + + + + /** Get a sub part of the complete image as a new range image. + * \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image. + * This is always according to absolute 0,0 meaning -180°,-90° + * and it is already in the system of the new image, so the + * actual pixel used in the original image is + * combine_pixels* (image_offset_x-image_offset_x_) + * \param sub_image_image_offset_y - Same as image_offset_x for the y coordinate + * \param sub_image_width - width of the new image + * \param sub_image_height - height of the new image + * \param combine_pixels - shrinking factor, meaning the new angular resolution + * is combine_pixels times the old one + * \param sub_image - the output image + */ + virtual void + getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width, + int sub_image_height, int combine_pixels, RangeImage& sub_image) const; + + //! Get a range image with half the resolution + virtual void + getHalfImage (RangeImage& half_image) const; + + //! Find the minimum and maximum range in the image + PCL_EXPORTS void + getMinMaxRanges (float& min_range, float& max_range) const; + + //! This function sets the sensor pose to 0 and transforms all point positions to this local coordinate frame + PCL_EXPORTS void + change3dPointsToLocalCoordinateFrame (); + + /** Calculate a range patch as the z values of the coordinate frame given by pose. + * The patch will have size pixel_size x pixel_size and each pixel + * covers world_size/pixel_size meters in the world + * You are responsible for deleting the structure afterwards! */ + PCL_EXPORTS float* + getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const; + + //! Same as above, but using the local coordinate frame defined by point and the viewing direction + PCL_EXPORTS float* + getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const; + + //! Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction + inline Eigen::Affine3f + getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const; + //! Same as above, using a reference for the retrurn value + inline void + getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, + Eigen::Affine3f& transformation) const; + //! Same as above, but only returning the rotation + inline void + getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const; + + /** Get a local coordinate frame at the given point based on the normal. */ + PCL_EXPORTS bool + getNormalBasedUprightTransformation (const Eigen::Vector3f& point, + float max_dist, Eigen::Affine3f& transformation) const; + + /** Get the integral image of the range values (used for fast blur operations). + * You are responsible for deleting it after usage! */ + PCL_EXPORTS void + getIntegralImage (float*& integral_image, int*& valid_points_num_image) const; + + /** Get a blurred version of the range image using box filters on the provided integral image*/ + PCL_EXPORTS void // Template necessary so that this function also works in derived classes + getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image, + RangeImage& range_image) const; + + /** Get a blurred version of the range image using box filters */ + PCL_EXPORTS virtual void // Template necessary so that this function also works in derived classes + getBlurredImage (int blur_radius, RangeImage& range_image) const; + + /** Get the squared euclidean distance between the two image points. + * Returns -INFINITY if one of the points was not observed */ + inline float + getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const; + //! Doing the above for some steps in the given direction and averaging + inline float + getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const; + + //! Project all points on the local plane approximation, thereby smoothing the surface of the scan + PCL_EXPORTS void + getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const; + //void getLocalNormals (int radius) const; + + /** Calculates the average 3D position of the no_of_points points described by the start + * point x,y in the direction delta. + * Returns a max range point (range=INFINITY) if the first point is max range and an + * unobserved point (range=-INFINITY) if non of the points is observed. */ + inline void + get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, + PointWithRange& average_point) const; + + /** Calculates the overlap of two range images given the relative transformation + * (from the given image to *this) */ + PCL_EXPORTS float + getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation, + int search_radius, float max_distance, int pixel_step=1) const; + + /** Get the viewing direction for the given point */ + inline bool + getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const; + + /** Get the viewing direction for the given point */ + inline void + getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const; + + /** Return a newly created Range image. + * Can be reimplemented in derived classes like RangeImagePlanar to return an image of the same type. */ + PCL_EXPORTS virtual RangeImage* + getNew () const { return new RangeImage; } + + /** Copy other to *this. Necessary for use in virtual functions that need to copy derived RangeImage classes (like RangeImagePlanar) */ + PCL_EXPORTS virtual void + copyTo (RangeImage& other) const; + + + // =====MEMBER VARIABLES===== + // BaseClass: + // roslib::Header header; + // std::vector points; + // std::uint32_t width; + // std::uint32_t height; + // bool is_dense; + + static bool debug; /**< Just for... well... debugging purposes. :-) */ + + protected: + // =====PROTECTED MEMBER VARIABLES===== + Eigen::Affine3f to_range_image_system_; /**< Inverse of to_world_system_ */ + Eigen::Affine3f to_world_system_; /**< Inverse of to_range_image_system_ */ + float angular_resolution_x_; /**< Angular resolution of the range image in x direction in radians per pixel */ + float angular_resolution_y_; /**< Angular resolution of the range image in y direction in radians per pixel */ + float angular_resolution_x_reciprocal_; /**< 1.0/angular_resolution_x_ - provided for better performance of + * multiplication compared to division */ + float angular_resolution_y_reciprocal_; /**< 1.0/angular_resolution_y_ - provided for better performance of + * multiplication compared to division */ + int image_offset_x_, image_offset_y_; /**< Position of the top left corner of the range image compared to + * an image of full size (360x180 degrees) */ + PointWithRange unobserved_point; /**< This point is used to be able to return + * a reference to a non-existing point */ + + // =====PROTECTED METHODS===== + + + // =====STATIC PROTECTED===== + static const int lookup_table_size; + static std::vector asin_lookup_table; + static std::vector atan_lookup_table; + static std::vector cos_lookup_table; + /** Create lookup tables for trigonometric functions */ + static void + createLookupTables (); + + /** Query the asin lookup table */ + static inline float + asinLookUp (float value); + + /** Query the std::atan2 lookup table */ + static inline float + atan2LookUp (float y, float x); + + /** Query the cos lookup table */ + static inline float + cosLookUp (float value); + + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** + * /ingroup range_image + */ + inline std::ostream& + operator<< (std::ostream& os, const RangeImage& r) + { + os << "header: " << std::endl; + os << r.header; + os << "points[]: " << r.points.size () << std::endl; + os << "width: " << r.width << std::endl; + os << "height: " << r.height << std::endl; + os << "sensor_origin_: " + << r.sensor_origin_[0] << ' ' + << r.sensor_origin_[1] << ' ' + << r.sensor_origin_[2] << std::endl; + os << "sensor_orientation_: " + << r.sensor_orientation_.x () << ' ' + << r.sensor_orientation_.y () << ' ' + << r.sensor_orientation_.z () << ' ' + << r.sensor_orientation_.w () << std::endl; + os << "is_dense: " << r.is_dense << std::endl; + os << "angular resolution: " + << RAD2DEG (r.getAngularResolutionX ()) << "deg/pixel in x and " + << RAD2DEG (r.getAngularResolutionY ()) << "deg/pixel in y.\n" << std::endl; + return (os); + } + +} // namespace end + + +#include // Definitions of templated and inline functions diff --git a/deps_install/include/pcl-1.10/pcl/range_image/range_image_planar.h b/deps_install/include/pcl-1.10/pcl/range_image/range_image_planar.h new file mode 100755 index 0000000..a095118 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/range_image/range_image_planar.h @@ -0,0 +1,216 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief @b RangeImagePlanar is derived from the original range image and differs from it because it's not a + * spherical projection, but using a projection plane (as normal cameras do), therefore being better applicable + * for range sensors that already provide a range image by themselves (stereo cameras, ToF-cameras), so that + * a conversion to point cloud and then to a spherical range image becomes unnecessary. + * \author Bastian Steder + * \ingroup range_image + */ + class RangeImagePlanar : public RangeImage + { + public: + // =====TYPEDEFS===== + using BaseClass = RangeImage; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + // =====CONSTRUCTOR & DESTRUCTOR===== + /** Constructor */ + PCL_EXPORTS RangeImagePlanar (); + /** Destructor */ + PCL_EXPORTS ~RangeImagePlanar (); + + /** Return a newly created RangeImagePlanar. + * Reimplementation to return an image of the same type. */ + RangeImage* + getNew () const override { return new RangeImagePlanar; } + + /** Copy *this to other. Derived version - also copying additional RangeImagePlanar members */ + PCL_EXPORTS void + copyTo (RangeImage& other) const override; + + // =====PUBLIC METHODS===== + /** \brief Get a boost shared pointer of a copy of this */ + inline Ptr + makeShared () { return Ptr (new RangeImagePlanar (*this)); } + + /** \brief Create the image from an existing disparity image. + * \param disparity_image the input disparity image data + * \param di_width the disparity image width + * \param di_height the disparity image height + * \param focal_length the focal length of the primary camera that generated the disparity image + * \param base_line the baseline of the stereo pair that generated the disparity image + * \param desired_angular_resolution If this is set, the system will skip as many pixels as necessary to get as + * close to this angular resolution as possible while not going over this value (the density will not be + * lower than this value). The value is in radians per pixel. + */ + PCL_EXPORTS void + setDisparityImage (const float* disparity_image, int di_width, int di_height, + float focal_length, float base_line, float desired_angular_resolution=-1); + + /** Create the image from an existing depth image. + * \param depth_image the input depth image data as float values + * \param di_width the disparity image width + * \param di_height the disparity image height + * \param di_center_x the x-coordinate of the camera's center of projection + * \param di_center_y the y-coordinate of the camera's center of projection + * \param di_focal_length_x the camera's focal length in the horizontal direction + * \param di_focal_length_y the camera's focal length in the vertical direction + * \param desired_angular_resolution If this is set, the system will skip as many pixels as necessary to get as + * close to this angular resolution as possible while not going over this value (the density will not be + * lower than this value). The value is in radians per pixel. + */ + PCL_EXPORTS void + setDepthImage (const float* depth_image, int di_width, int di_height, float di_center_x, float di_center_y, + float di_focal_length_x, float di_focal_length_y, float desired_angular_resolution=-1); + + /** Create the image from an existing depth image. + * \param depth_image the input disparity image data as short values describing millimeters + * \param di_width the disparity image width + * \param di_height the disparity image height + * \param di_center_x the x-coordinate of the camera's center of projection + * \param di_center_y the y-coordinate of the camera's center of projection + * \param di_focal_length_x the camera's focal length in the horizontal direction + * \param di_focal_length_y the camera's focal length in the vertical direction + * \param desired_angular_resolution If this is set, the system will skip as many pixels as necessary to get as + * close to this angular resolution as possible while not going over this value (the density will not be + * lower than this value). The value is in radians per pixel. + */ + PCL_EXPORTS void + setDepthImage (const unsigned short* depth_image, int di_width, int di_height, float di_center_x, float di_center_y, + float di_focal_length_x, float di_focal_length_y, float desired_angular_resolution=-1); + + /** Create the image from an existing point cloud. + * \param point_cloud the source point cloud + * \param di_width the disparity image width + * \param di_height the disparity image height + * \param di_center_x the x-coordinate of the camera's center of projection + * \param di_center_y the y-coordinate of the camera's center of projection + * \param di_focal_length_x the camera's focal length in the horizontal direction + * \param di_focal_length_y the camera's focal length in the vertical direction + * \param sensor_pose the pose of the virtual depth camera + * \param coordinate_frame the used coordinate frame of the point cloud + * \param noise_level what is the typical noise of the sensor - is used for averaging in the z-buffer + * \param min_range minimum range to consifder points + */ + template void + createFromPointCloudWithFixedSize (const PointCloudType& point_cloud, + int di_width, int di_height, float di_center_x, float di_center_y, + float di_focal_length_x, float di_focal_length_y, + const Eigen::Affine3f& sensor_pose, + CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f, + float min_range=0.0f); + + // Since we reimplement some of these overloaded functions, we have to do the following: + using RangeImage::calculate3DPoint; + using RangeImage::getImagePoint; + + /** \brief Calculate the 3D point according to the given image point and range + * \param image_x the x image position + * \param image_y the y image position + * \param range the range + * \param point the resulting 3D point + * \note Implementation according to planar range images (compared to spherical as in the original) + */ + inline void + calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const override; + + /** \brief Calculate the image point and range from the given 3D point + * \param point the resulting 3D point + * \param image_x the resulting x image position + * \param image_y the resulting y image position + * \param range the resulting range + * \note Implementation according to planar range images (compared to spherical as in the original) + */ + inline void + getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const override; + + /** Get a sub part of the complete image as a new range image. + * \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image. + * This is always according to absolute 0,0 meaning -180°,-90° + * and it is already in the system of the new image, so the + * actual pixel used in the original image is + * combine_pixels* (image_offset_x-image_offset_x_) + * \param sub_image_image_offset_y - Same as image_offset_x for the y coordinate + * \param sub_image_width - width of the new image + * \param sub_image_height - height of the new image + * \param combine_pixels - shrinking factor, meaning the new angular resolution + * is combine_pixels times the old one + * \param sub_image - the output image + */ + PCL_EXPORTS void + getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width, + int sub_image_height, int combine_pixels, RangeImage& sub_image) const override; + + //! Get a range image with half the resolution + PCL_EXPORTS void + getHalfImage (RangeImage& half_image) const override; + + //! Getter for the focal length in X + inline float + getFocalLengthX () const { return focal_length_x_; } + + //! Getter for the focal length in Y + inline float + getFocalLengthY () const { return focal_length_y_; } + + //! Getter for the principal point in X + inline float + getCenterX () const { return center_x_; } + + //! Getter for the principal point in Y + inline float + getCenterY () const { return center_y_; } + + + protected: + float focal_length_x_, focal_length_y_; //!< The focal length of the image in pixels + float focal_length_x_reciprocal_, focal_length_y_reciprocal_; //!< 1/focal_length -> for internal use + float center_x_, center_y_; //!< The principle point of the image + }; +} // namespace end + + +#include // Definitions of templated and inline functions diff --git a/deps_install/include/pcl-1.10/pcl/range_image/range_image_spherical.h b/deps_install/include/pcl-1.10/pcl/range_image/range_image_spherical.h new file mode 100755 index 0000000..c2ffc60 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/range_image/range_image_spherical.h @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012-, Open Perception, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief @b RangeImageSpherical is derived from the original range image and uses a slightly different + * spherical projection. In the original range image, the image will appear more and more + * "scaled down" along the y axis, the further away from the mean line of the image a point is. + * This class removes this scaling, which makes it especially suitable for spinning LIDAR sensors + * that capure a 360° view, since a rotation of the sensor will now simply correspond to a shift of the + * range image. (This class is similar to RangeImagePlanar, but changes less of the behaviour of the base class.) + * \author Andreas Muetzel + * \ingroup range_image + */ + class RangeImageSpherical : public RangeImage + { + public: + // =====TYPEDEFS===== + using BaseClass = RangeImage; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + // =====CONSTRUCTOR & DESTRUCTOR===== + /** Constructor */ + PCL_EXPORTS RangeImageSpherical () {} + /** Destructor */ + PCL_EXPORTS virtual ~RangeImageSpherical () {} + + /** Return a newly created RangeImagePlanar. + * Reimplementation to return an image of the same type. */ + virtual RangeImage* + getNew () const { return new RangeImageSpherical; } + + // =====PUBLIC METHODS===== + /** \brief Get a boost shared pointer of a copy of this */ + inline Ptr + makeShared () { return Ptr (new RangeImageSpherical (*this)); } + + + // Since we reimplement some of these overloaded functions, we have to do the following: + using RangeImage::calculate3DPoint; + using RangeImage::getImagePoint; + + /** \brief Calculate the 3D point according to the given image point and range + * \param image_x the x image position + * \param image_y the y image position + * \param range the range + * \param point the resulting 3D point + * \note Implementation according to planar range images (compared to spherical as in the original) + */ + virtual inline void + calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const; + + /** \brief Calculate the image point and range from the given 3D point + * \param point the resulting 3D point + * \param image_x the resulting x image position + * \param image_y the resulting y image position + * \param range the resulting range + * \note Implementation according to planar range images (compared to spherical as in the original) + */ + virtual inline void + getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const; + + /** Get the angles corresponding to the given image point */ + inline void + getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const; + + /** Get the image point corresponding to the given ranges */ + inline void + getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const; + + }; +} // namespace end + + +#include // Definitions of templated and inline functions diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/abstract-search.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/abstract-search.hh new file mode 100755 index 0000000..43e80f4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/abstract-search.hh @@ -0,0 +1,353 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#ifndef METS_ABSTRACT_SEARCH_HH_ +#define METS_ABSTRACT_SEARCH_HH_ + +namespace mets { + + /// @defgroup common Common components + /// @{ + + /// @brief The solution recorder is used by search algorithm, at the + /// end of each iteration, to record the best seen solution. + /// + /// The concept of best is externalized so that you can record the + /// best ever solution met or the best solution that matches some + /// other criteria (e.g. feasibility constraints relaxed in the + /// feasible_solution implementation of the cost function). + /// + class solution_recorder { + public: + /// @brief Default ctor. + solution_recorder() {} + /// @brief Unimplemented copy ctor. + solution_recorder(const solution_recorder&); + /// @brief Unimplemented assignment operator. + solution_recorder& operator=(const solution_recorder&); + + /// @brief A virtual dtor. + virtual + ~solution_recorder(); + + /// @brief Accept is called at the end of each iteration for an + /// opportunity to record the best move ever. + /// + /// (this is a chain of responsibility) + /// + virtual bool + accept(const feasible_solution& sol) = 0; + + virtual gol_type + best_cost() const = 0; + }; + + /// @brief An abstract search. + /// + /// @see mets::tabu_search, mets::simulated_annealing, mets::local_search + template + class abstract_search : public subject< abstract_search > + { + public: + /// @brief Set some common values needed for neighborhood based + /// metaheuristics. + /// + /// @param working The starting point solution (this will be modified + /// during search as the working solution) + /// + /// @param recorder A solution recorder instance used to record + /// the best solution found + /// + /// @param moveman A problem specific implementation of the + /// move_manager_type used to generate the neighborhood. + /// + abstract_search(feasible_solution& working, + solution_recorder& recorder, + move_manager_type& moveman) + : subject >(), + solution_recorder_m(recorder), + working_solution_m(working), + moves_m(moveman), + current_move_m(), + step_m() + { } + + /// purposely not implemented (see Effective C++) + abstract_search(const abstract_search&); + /// purposely not implemented (see Effective C++) + abstract_search& operator==(const abstract_search&); + + /// @brief Virtual destructor. + virtual + ~abstract_search() + { }; + + enum { + /// @brief We just made a move. + MOVE_MADE = 0, + /// @brief Our solution_recorder_chain object reported an improvement + IMPROVEMENT_MADE, + /// @brief We are about to start a new iteration + ITERATION_BEGIN, + /// @brief We have done the iteration + ITERATION_END, + /// @brief Placeholer for next values + LAST + }; + + /// @brief This method starts the search. + /// + /// Remember that this is a minimization. + /// + /// An exception mets::no_moves_error can be risen when no move is + /// possible. + virtual void + search() = 0; + + /// @brief The solution recorder instance. + const solution_recorder& + recorder() const + { return solution_recorder_m; }; + + /// @brief The current working solution. + const feasible_solution& + working() const + { return working_solution_m; } + + feasible_solution& + working() + { return working_solution_m; } + + /// @brief The last move made + const move& + current_move() const + { return **current_move_m; } + + /// @brief The last move made + move& + current_move() + { return **current_move_m; } + + /// @brief The move manager used by this search + const move_manager_type& + move_manager() const + { return moves_m; } + + /// @brief The move manager used by this search + move_manager_type& + move_manager() + { return moves_m; } + + /// @brief The current step of the algorithm (to be used by the + /// observers). + /// + /// When you implement a new type of search you should set step_m + /// protected variable to the status of the algorithm + /// (0 = "MOVE_MADE", 1 = "IMPROVEMENT_MADE", etc.). + int + step() const + { return step_m; } + + protected: + solution_recorder& solution_recorder_m; + feasible_solution& working_solution_m; + move_manager_type& moves_m; + typename move_manager_type::iterator current_move_m; + int step_m; + }; + + /// @} + + /// @defgroup common Common components + /// @{ + + /// @brief The best ever solution recorder can be used as a simple + /// solution recorder that just records the best copyable solution + /// found during its lifetime. + /// + class best_ever_solution : public solution_recorder + { + public: + /// @brief The mets::evaluable_solution will be stored as a + /// reference: please provide an instance that is not + /// modified/needed elsewhere. + /// + /// @param best The instance used to store the best solution found + /// (will be modified). + best_ever_solution(evaluable_solution& best) : + solution_recorder(), + best_ever_m(best) + { } + + /// @brief Unimplemented default ctor. + best_ever_solution(); + /// @brief Unimplemented copy ctor. + best_ever_solution(const best_ever_solution&); + /// @brief Unimplemented assignment operator. + best_ever_solution& operator=(const best_ever_solution&); + + /// @brief Accept is called at the end of each iteration for an + /// opportunity to record the best solution found during the + /// search. + bool accept(const feasible_solution& sol); + + /// @brief Returns the best solution found since the beginning. + const evaluable_solution& best_seen() const + { return best_ever_m; } + + /// @brief Best cost seen. + gol_type best_cost() const + { return best_ever_m.cost_function(); } + protected: + /// @brief Records the best solution + evaluable_solution& best_ever_m; + }; + + /// @brief An object that is called back during the search progress. + template + class search_listener : public observer > + { + public: + using search_type = abstract_search; + /// @brief A new observer (listener) of a search process, remember + /// to attach the created object to the search process to be + /// observed (mets::search_type::attach()) + explicit + search_listener() : observer() + { } + + /// purposely not implemented (see Effective C++) + search_listener(const search_listener& other); + search_listener& + operator=(const search_listener& other); + + /// @brief Virtual destructor + virtual + ~search_listener() + { } + + /// @brief This is the callback method called by searches + /// when a move, an improvement or something else happens + virtual void + update(search_type* algorithm) = 0; + }; + + + template + struct iteration_logger : public mets::search_listener + { + explicit + iteration_logger(std::ostream& o) + : mets::search_listener(), + iteration(0), + os(o) + { } + + void + update(mets::abstract_search* as) + { + const mets::feasible_solution& p = as->working(); + if(as->step() == mets::abstract_search::MOVE_MADE) + { + os << iteration++ << "\t" + << static_cast(p).cost_function() + << "\n"; + } + } + + protected: + int iteration; + std::ostream& os; + }; + + template + struct improvement_logger : public mets::search_listener + { + explicit + improvement_logger(std::ostream& o, gol_type epsilon = 1e-7) + : mets::search_listener(), + iteration_m(0), + best_m(std::numeric_limits::max()), + os_m(o), + epsilon_m(epsilon) + { } + + void + update(mets::abstract_search* as) + { + const mets::feasible_solution& p = as->working(); + + if(as->step() == mets::abstract_search::MOVE_MADE) + { + iteration_m++; + double val = static_cast(p) + .cost_function(); + if(val < best_m - epsilon_m) + { + best_m = val; + os_m << iteration_m << "\t" + << best_m + << " (*)\n"; + } + } + } + + protected: + int iteration_m; + double best_m; + std::ostream& os_m; + gol_type epsilon_m; + }; + + /// @} + + +} + +inline mets::solution_recorder::~solution_recorder() +{ } + +inline bool +mets::best_ever_solution::accept(const mets::feasible_solution& sol) +{ + const evaluable_solution& s = dynamic_cast(sol); + if(s.cost_function() < best_ever_m.cost_function()) + { + best_ever_m.copy_from(s); + return true; + } + return false; +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/local-search.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/local-search.hh new file mode 100755 index 0000000..b2c4139 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/local-search.hh @@ -0,0 +1,146 @@ +// METS lib source file - local-search.hh -*- C++ -*- +// +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef LOCAL_SEARCH_HH_ +#define LOCAL_SEARCH_HH_ + +namespace mets { + /// @defgroup local_search Local Search + /// @{ + + /// @brief Local search algorithm. + /// + /// With customary phase alternation + /// and move managers generated neighborhood this can be used to do + /// also a Random Restart Local Search, a Greedy Search, + /// an Iterated Local Search and a Variable Neighborhood Search. + template + class local_search : public mets::abstract_search + { + public: + /// @brief Creates a local search instance + /// + /// @param working The working solution (this will be modified + /// during search) + /// + /// @param best_so_far A different solution + /// instance used to store the best solution found + /// + /// @param moveman A problem specific implementation of the + /// move_manager_type concept used to generate the neighborhood. + /// + /// @param short_circuit Wether the search should stop on + /// the first improving move or not. + local_search(evaluable_solution& starting_point, + solution_recorder& recorder, + move_manager_type& moveman, + gol_type epsilon = 1e-7, + bool short_circuit = false); + + /// purposely not implemented (see Effective C++) + local_search(const local_search&); + local_search& operator=(const local_search&); + + /// @brief This method starts the local search process. + /// + /// To have a real local search you should provide an + /// move_manager_type than enumerates all feasible + /// moves. + /// + virtual void + search(); + + protected: + bool short_circuit_m; + gol_type epsilon_m; + }; + + /// @} + +} + +template +mets::local_search::local_search(evaluable_solution& working, + solution_recorder& recorder, + move_manager_t& moveman, + gol_type epsilon, + bool short_circuit) + : abstract_search(working, recorder, moveman), + short_circuit_m(short_circuit), epsilon_m(epsilon) +{ + using base_t = abstract_search; + base_t::step_m = 0; +} + +template +void +mets::local_search::search() +{ + using base_t = abstract_search; + typename move_manager_t::iterator best_movit; + + base_t::solution_recorder_m.accept(base_t::working_solution_m); + + gol_type best_cost = + static_cast(base_t::working_solution_m) + .cost_function(); + + do + { + base_t::moves_m.refresh(base_t::working_solution_m); + best_movit = base_t::moves_m.end(); + for(typename move_manager_t::iterator movit = base_t::moves_m.begin(); + movit != base_t::moves_m.end(); ++movit) + { + // evaluate the cost after the move + gol_type cost = (*movit)->evaluate(base_t::working_solution_m); + if(cost < best_cost - epsilon_m) + { + best_cost = cost; + best_movit = movit; + if(short_circuit_m) break; + } + } // end for each move + + if(best_movit != base_t::moves_m.end()) + { + (*best_movit)->apply(base_t::working_solution_m); + base_t::solution_recorder_m.accept(base_t::working_solution_m); + base_t::current_move_m = best_movit; + this->notify(); + } + + } while(best_movit != base_t::moves_m.end()); +} +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/mets.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/mets.hh new file mode 100755 index 0000000..50b27cc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/mets.hh @@ -0,0 +1,162 @@ +// METS lib source file - mets.h -*- C++ -*- +// +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +/// @mainpage METSlib +/// +/// \image html http://www.coin-or.org/images/logo/COIN-OR_150.png +/// +/// @section Introduction +/// +/// This is a library implementing some neighborhood based +/// metaheuristics with or without memory. +/// +/// The solution instance must implement mets::feasible_solution and +/// the moves must be derived from mets::move classes. +/// +/// The neighborhood can be specified implementing a +/// mets::move_manager subclass or providing another class with the +/// same concept (using the move_manager can ease things, but you can +/// also provide your own custom neighorhood iterator). +/// +/// All the mentioned classes must be implemented to model the problem +/// at hand. See as an example the "tutorial" and "qap" programs. +/// +/// You are also responsible of configuring and running the algorithm +/// of choice. +/// +/// Once your model is set up you are free of experimenting different +/// metaheuristics without changing it, but simply configuring one +/// algorithm or another. +/// +/// Each algorithm can be customized implementing your own decorating +/// classes, although a bunch of predefined and commonly used +/// decorators are provided with the library, some problems may need +/// customary tabu lists, termination criterias, aspiration criteria, +/// or cooling schedules. +/// +/// The framework you must implement your model into is made of: +/// +/// - mets::feasible_solution +/// - mets::evaluable_solution (use this if you also use mets::best_ever_solution) +/// - mets::permutation_problem +/// - mets::move +/// - mets::mana_move (use this if you also use by mets::simple_tabu_list) +/// - mets::swap_elements +/// +/// The toolkit of implemented algorithms is made of: +/// +/// - mets::move_manager (or a class implementing the same concept) +/// - mets::swap_neighborhood +/// - mets::local_search +/// - mets::simulated_annealing +/// - mets::abstract_cooling_schedule +/// - mets::solution_recorder +/// - mets::best_ever_solution +/// - mets::termination_criteria_chain +/// - mets::iteration_termination_criteria +/// - mets::noimprove_termination_criteria +/// - mets::threshold_termination_criteria +/// - mets::tabu_search +/// - mets::tabu_list_chain +/// - mets::simple_tabu_list +/// - mets::aspiration_criteria_chain +/// - mets::best_ever_criteria +/// - mets::solution_recorder +/// - mets::best_ever_solution +/// - mets::termination_criteria_chain +/// - mets::iteration_termination_criteria +/// - mets::noimprove_termination_criteria +/// - mets::threshold_termination_criteria +/// +/// To use the mets::simple_tabu_list you need to derive your moves +/// from the mets::mana_move base class and implement the pure virtual +/// methods. +/// +#ifndef METS_METS_HH_ +#define METS_METS_HH_ + +#include "metslib_config.hh" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined (METSLIB_TR1_BOOST) +# include +# include +# include +#elif defined (METSLIB_HAVE_UNORDERED_MAP) +# include +# include +#elif defined (METSLIB_HAVE_TR1_UNORDERED_MAP) +# include +# include +#else +# error "Unable to find unordered_map header file. Please use a recent C++ compiler supporting TR1 extension." +#endif + + +/// +/// @brief METSlib Metaheuristic framework namespace. +/// +/// Framework for neighborhood based metaheuristics (Tabu Search, +/// Simulated Annealing, Iterated Local Search, Random Restart Local +/// Search). +/// + +#include "observer.hh" +#include "model.hh" +#include "termination-criteria.hh" +#include "abstract-search.hh" +#include "local-search.hh" +#include "tabu-search.hh" +#include "simulated-annealing.hh" + + +//________________________________________________________________________ +inline std::ostream& +operator<<(std::ostream& os, const mets::printable& p) +{ + p.print(os); + return os; +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/metslib_config.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/metslib_config.hh new file mode 100755 index 0000000..97eef3b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/metslib_config.hh @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef METSLIB_CONFIG_HH_ +#define METSLIB_CONFIG_HH_ +# if defined (_WIN32) +# define METSLIB_HAVE_UNORDERED_MAP 1 +# define METSLIB_TR1_MIXED_NAMESPACE 1 +# elif defined (__GXX_EXPERIMENTAL_CXX0X__) +# define METSLIB_HAVE_UNORDERED_MAP 1 +# else +# define METSLIB_HAVE_TR1_UNORDERED_MAP 1 +# endif + +//Within PCL, force always to use boost unordered_map and random generators +//Other options give problems with Clang... needs further investigation... +#define METSLIB_TR1_BOOST 1 + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/model.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/model.hh new file mode 100755 index 0000000..6d9a35e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/model.hh @@ -0,0 +1,793 @@ +// METSlib source file - model.hh -*- C++ -*- +// +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef METS_MODEL_HH_ +#define METS_MODEL_HH_ + +namespace mets { + + /// @brief Type of the objective/cost function. + /// + /// You should be able to change this to "int" for your uses + /// to improve performance if it suffice, no guarantee. + /// + using gol_type = double; + + /// @brief Exception risen when some algorithm has no more moves to + /// make. + class no_moves_error + : public std::runtime_error + { + public: + no_moves_error() + : std::runtime_error("There are no more available moves.") {} + no_moves_error(const std::string message) + : std::runtime_error(message) {} + }; + + /// @brief A sequence function object useful as an STL generator. + /// + /// Returns start, start+1, ... + /// + class sequence + { + public: + /// @brief A sequence class starting at start. + sequence(int start = 0) + : value_m(start) + { } + /// @brief Subscript operator. Each call increments the value of + /// the sequence. + int operator()() + { return value_m++; } + protected: + int value_m; + }; + + /// @brief An interface for prototype objects. + class clonable { + public: + virtual + ~clonable() {}; + virtual clonable* + clone() const = 0; + }; + + /// @brief An interface for hashable objects. + class hashable { + public: + virtual + ~hashable() {}; + virtual std::size_t + hash() const = 0; + }; + + /// @brief An interface for copyable objects. + class copyable { + public: + virtual + ~copyable() {}; + virtual void + copy_from(const copyable&) = 0; + }; + + /// @brief An interface for printable objects. + class printable { + public: + virtual + ~printable() {} + virtual void + print(std::ostream& /*os*/) const { }; + }; + + /// @defgroup model Model + /// @{ + + /// @brief interface of a feasible solution space to be searched + /// with tabu search. + /// + /// Note that "feasible" is not intended w.r.t. the constraint of + /// the problem but only regarding the space we want the local + /// search to explore. From time to time allowing solutions to + /// explore unfeasible regions is non only allowed, but encouraged + /// to improve tabu search performances. In those cases the + /// objective function should probably account for unfeasibility + /// with a penalty term. + /// + /// This is the most generic solution type and is useful only if you + /// implement your own solution recorder and + /// max-noimprove. Otherwise you might want to derive from an + /// evaluable_solution or from a permutation_problem class, + /// depending on your problem type. + /// + class feasible_solution + { + public: + /// @brief Virtual dtor. + virtual + ~feasible_solution() + { } + + }; + + + /// @brief A copyable and evaluable solution implementation, + /// + /// All you need, if you implement your own mets::solution_recorder, + /// is to derive from the almost empty + /// mets::feasible_solution. However, if you want to use the + /// provided mets::best_ever_recorder you need to derive from this + /// class (that also defines an interface to copy and evaluate a + /// solution). + /// + /// @see mets::best_ever_recorder + class evaluable_solution : public feasible_solution, + public copyable + { + public: + /// @brief Cost function to be minimized. + /// + /// The cost function is the target that the search algorithm + /// tries to minimize. + /// + /// You must implement this for your problem. + /// + virtual gol_type + cost_function() const = 0; + }; + + /// @brief An abstract permutation problem. + /// + /// The permutation problem provides a skeleton to rapidly prototype + /// permutation problems (such as Assignment problem, Quadratic AP, + /// TSP, and so on). The skeleton holds a pi_m variable that, at + /// each step, contains a permutation of the numbers in [0..n-1]. + /// + /// A few operators are provided to randomly choose a solution, to + /// perturbate a solution with some random swaps and to simply swap + /// two items in the list. + /// + /// @see mets::swap_elements + class permutation_problem: public evaluable_solution + { + public: + + /// @brief Unimplemented. + permutation_problem(); + + /// @brief Inizialize pi_m = {0, 1, 2, ..., n-1}. + permutation_problem(int n) : pi_m(n), cost_m(0.0) + { std::generate(pi_m.begin(), pi_m.end(), sequence(0)); } + + /// @brief Copy from another permutation problem, if you introduce + /// new member variables remember to override this and to call + /// permutation_problem::copy_from in the overriding code. + /// + /// @param other the problem to copy from + void copy_from(const copyable& other); + + /// @brief: Compute cost of the whole solution. + /// + /// You will need to override this one. + virtual gol_type + compute_cost() const = 0; + + /// @brief: Evaluate a swap. + /// + /// Implement this method to evaluate the change in the cost + /// function after the swap (without actually modifying the + /// solution). The method should return the difference in cost + /// between the current position and the position after the swap + /// (negative if decreasing and positive otherwise). + /// + /// To obtain maximal performance from the algorithm it is + /// essential, whenever possible, to only compute the cost update + /// and not the whole cost function. + virtual gol_type + evaluate_swap(int i, int j) const = 0; + + /// @brief The size of the problem. + /// Do not override unless you know what you are doing. + std::size_t + size() const + { return pi_m.size(); } + + /// @brief Returns the cost of the current solution. The default + /// implementation provided returns the protected + /// mets::permutation_problem::cost_m member variable. Do not + /// override unless you know what you are doing. + gol_type cost_function() const + { return cost_m; } + + /// @brief Updates the cost with the one computed by the subclass. + /// Do not override unless you know what you are doing. + void + update_cost() + { cost_m = compute_cost(); } + + /// @brief: Apply a swap and update the cost. + /// Do not override unless you know what you are doing. + void + apply_swap(int i, int j) + { cost_m += evaluate_swap(i,j); std::swap(pi_m[i], pi_m[j]); } + + + protected: + std::vector pi_m; + gol_type cost_m; + template + friend void random_shuffle(permutation_problem& p, random_generator& rng); + }; + + + /// @brief Shuffle a permutation problem (generates a random starting point). + /// + /// @see mets::permutation_problem + template + void random_shuffle(permutation_problem& p, random_generator& rng) + { +#if defined (METSLIB_TR1_BOOST) + boost::uniform_int unigen; + boost::variate_generator >gen(rng, unigen); +#elif defined (METSLIB_HAVE_UNORDERED_MAP) && !defined (METSLIB_TR1_MIXED_NAMESPACE) + std::uniform_int unigen; + std::variate_generator >gen(rng, unigen); +#else + std::tr1::uniform_int unigen; + std::tr1::variate_generator >gen(rng, unigen); +#endif + std::random_shuffle(p.pi_m.begin(), p.pi_m.end(), gen); + p.update_cost(); + } + + /// @brief Perturbate a problem with n swap moves. + /// + /// @see mets::permutation_problem + template + void perturbate(permutation_problem& p, unsigned int n, random_generator& rng) + { +#if defined (METSLIB_TR1_BOOST) + boost::uniform_int<> int_range; +#elif defined (METSLIB_HAVE_UNORDERED_MAP) && !defined (METSLIB_TR1_MIXED_NAMESPACE) + std::uniform_int<> int_range; +#else + std::tr1::uniform_int<> int_range; +#endif + for(unsigned int ii=0; ii!=n;++ii) + { + int p1 = int_range(rng, p.size()); + int p2 = int_range(rng, p.size()); + while(p1 == p2) + p2 = int_range(rng, p.size()); + p.apply_swap(p1, p2); + } + } + + /// @brief Move to be operated on a feasible solution. + /// + /// You must implement this (one or more types are allowed) for your + /// problem. + /// + /// You must provide an apply as well as an evaluate method. + /// + /// NOTE: this interface changed from 0.4.x to 0.5.x. The change was + /// needed to provide a more general interface. + class move + { + public: + + virtual + ~move() + { }; + + /// + /// @brief Evaluate the cost after the move. + /// + /// What if we make this move? Local searches can be speed up by a + /// substantial amount if we are able to efficiently evaluate the + /// cost of the neighboring solutions without actually changing + /// the solution. + virtual gol_type + evaluate(const feasible_solution& sol) const = 0; + + /// + /// @brief Operates this move on sol. + /// + /// This should actually change the solution. + virtual void + apply(feasible_solution& sol) const = 0; + + + }; + + /// @brief A Mana Move is a move that can be automatically made tabu + /// by the mets::simple_tabu_list. + /// + /// If you implement this class you can use the + /// mets::simple_tabu_list as a ready to use tabu list. + /// + /// You must implement a clone() method, provide an hash funciton + /// and provide a operator==() method that is responsible to find if + /// a move is equal to another. + /// + /// NOTE: If the desired behaviour is to declare tabu the *opposite* + /// of the last made move you can achieve that behavioud override + /// the opposite_of() method as well. + /// + class mana_move : + public move, + public clonable, + public hashable + { + public: + + /// @brief Create and return a new move that is the reverse of + /// this one + /// + /// By default this just calls "clone". If this method is not + /// overridden the mets::simple_tabu_list declares tabu the last + /// made move. Reimplementing this method it is possibile to + /// actually declare as tabu the opposite of the last made move + /// (if we moved a to b we can declare tabu moving b to a). + virtual mana_move* + opposite_of() const + { return static_cast(clone()); } + + /// @brief Tell if this move equals another w.r.t. the tabu list + /// management (for mets::simple_tabu_list) + virtual bool + operator==(const mana_move& other) const = 0; + + }; + + template class swap_neighborhood; // fw decl + + /// @brief A mets::mana_move that swaps two elements in a + /// mets::permutation_problem. + /// + /// Each instance swaps two specific objects. + /// + /// @see mets::permutation_problem, mets::mana_move + /// + class swap_elements : public mets::mana_move + { + public: + + /// @brief A move that swaps from and to. + swap_elements(int from, int to) + : p1(std::min(from,to)), p2(std::max(from,to)) + { } + + /// @brief Virtual method that applies the move on a point + gol_type + evaluate(const mets::feasible_solution& s) const + { const permutation_problem& sol = + static_cast(s); + return sol.cost_function() + sol.evaluate_swap(p1, p2); } + + /// @brief Virtual method that applies the move on a point + void + apply(mets::feasible_solution& s) const + { permutation_problem& sol = static_cast(s); + sol.apply_swap(p1, p2); } + + /// @brief Clones this move (so that the tabu list can store it) + clonable* + clone() const + { return new swap_elements(p1, p2); } + + /// @brief An hash function used by the tabu list (the hash value is + /// used to insert the move in an hash set). + std::size_t + hash() const + { return (p1)<<16^(p2); } + + /// @brief Comparison operator used to tell if this move is equal to + /// a move in the simple tabu list move set. + bool + operator==(const mets::mana_move& o) const; + + /// @brief Modify this swap move. + void change(int from, int to) + { p1 = std::min(from,to); p2 = std::max(from,to); } + + protected: + int p1; ///< the first element to swap + int p2; ///< the second element to swap + + template + friend class swap_neighborhood; + }; + + /// @brief A mets::mana_move that swaps a subsequence of elements in + /// a mets::permutation_problem. + /// + /// @see mets::permutation_problem, mets::mana_move + /// + class invert_subsequence : public mets::mana_move + { + public: + + /// @brief A move that swaps from and to. + invert_subsequence(int from, int to) + : p1(from), p2(to) + { } + + /// @brief Virtual method that applies the move on a point + gol_type + evaluate(const mets::feasible_solution& s) const; + + /// @brief Virtual method that applies the move on a point + void + apply(mets::feasible_solution& s) const; + + clonable* + clone() const + { return new invert_subsequence(p1, p2); } + + /// @brief An hash function used by the tabu list (the hash value is + /// used to insert the move in an hash set). + std::size_t + hash() const + { return (p1)<<16^(p2); } + + /// @brief Comparison operator used to tell if this move is equal to + /// a move in the tabu list. + bool + operator==(const mets::mana_move& o) const; + + void change(int from, int to) + { p1 = from; p2 = to; } + + protected: + int p1; ///< the first element to swap + int p2; ///< the second element to swap + + // template + // friend class invert_full_neighborhood; + }; + + /// @brief A neighborhood generator. + /// + /// This is a sample implementation of the neighborhood exploration + /// concept. You can still derive from this class and implement the + /// refresh method, but, since version 0.5.x you don't need to. + /// + /// To implement your own move manager you should simply adhere to + /// the following concept: + /// + /// provide an iterator, and size_type types, a begin() and end() + /// method returning iterators to a move collection. The switch to a + /// template based move_manager was made so that you can use any + /// iterator type that you want. This allows, between other things, + /// to implement intelligent iterators that dynamically return + /// moves. + /// + /// The move manager can represent both Variable and Constant + /// Neighborhoods. + /// + /// To make a constant neighborhood put moves in the moves_m queue + /// in the constructor and implement an empty void + /// refresh(feasible_solution&) method. + /// + class move_manager + { + public: + /// + /// @brief Initialize the move manager with an empty list of moves + move_manager() + : moves_m() + { } + + /// @brief Virtual destructor + virtual ~move_manager() + { } + + /// @brief Selects a different set of moves at each iteration. + virtual void + refresh(mets::feasible_solution& s) = 0; + + /// @brief Iterator type to iterate over moves of the neighborhood + using iterator = std::deque::iterator; + + /// @brief Size type + using size_type = std::deque::size_type; + + /// @brief Begin iterator of available moves queue. + iterator begin() + { return moves_m.begin(); } + + /// @brief End iterator of available moves queue. + iterator end() + { return moves_m.end(); } + + /// @brief Size of the neighborhood. + size_type size() const + { return moves_m.size(); } + + protected: + std::deque moves_m; ///< The moves queue + move_manager(const move_manager&); + }; + + + /// @brief Generates a stochastic subset of the neighborhood. +#if defined (METSLIB_TR1_BOOST) + template +#elif defined (METSLIB_HAVE_UNORDERED_MAP) && !defined (METSLIB_TR1_MIXED_NAMESPACE) + template +#else + template +#endif + class swap_neighborhood : public mets::move_manager + { + public: + /// @brief A neighborhood exploration strategy for mets::swap_elements. + /// + /// This strategy selects *moves* random swaps + /// + /// @param r a random number generator (e.g. an instance of + /// std::tr1::minstd_rand0 or std::tr1::mt19936) + /// + /// @param moves the number of swaps to add to the exploration + /// + swap_neighborhood(random_generator& r, + unsigned int moves); + + /// @brief Dtor. + ~swap_neighborhood(); + + /// @brief Selects a different set of moves at each iteration. + void refresh(mets::feasible_solution& s); + + protected: + random_generator& rng; + +#if defined (METSLIB_TR1_BOOST) + boost::uniform_int<> int_range; +#elif defined (METSLIB_HAVE_UNORDERED_MAP) && !defined (METSLIB_TR1_MIXED_NAMESPACE) + std::uniform_int<> int_range; +#else + std::tr1::uniform_int<> int_range; +#endif + unsigned int n; + + void randomize_move(swap_elements& m, unsigned int size); + }; + + //________________________________________________________________________ + template + mets::swap_neighborhood< random_generator + >::swap_neighborhood(random_generator& r, + unsigned int moves) + : mets::move_manager(), rng(r), int_range(0), n(moves) + { + // n simple moves + for(unsigned int ii = 0; ii != n; ++ii) + moves_m.push_back(new swap_elements(0,0)); + } + + template + mets::swap_neighborhood::~swap_neighborhood() + { + // delete all moves + for(iterator ii = begin(); ii != end(); ++ii) + delete (*ii); + } + + template + void + mets::swap_neighborhood::refresh(mets::feasible_solution& s) + { + permutation_problem& sol = dynamic_cast(s); + iterator ii = begin(); + + // the first n are simple qap_moveS + for(unsigned int cnt = 0; cnt != n; ++cnt) + { + swap_elements* m = static_cast(*ii); + randomize_move(*m, sol.size()); + ++ii; + } + + } + + template + void + mets::swap_neighborhood::randomize_move(swap_elements& m, unsigned int size) + { + int p1 = int_range(rng, size); + int p2 = int_range(rng, size); + while(p1 == p2) + p2 = int_range(rng, size); + // we are friend, so we know how to handle the nuts&bolts of + // swap_elements + m.p1 = std::min(p1,p2); + m.p2 = std::max(p1,p2); + } + + /// @brief Generates a the full swap neighborhood. + class swap_full_neighborhood : public mets::move_manager + { + public: + /// @brief A neighborhood exploration strategy for mets::swap_elements. + /// + /// This strategy selects *moves* random swaps. + /// + /// @param size the size of the problem + swap_full_neighborhood(int size) : move_manager() + { + for(int ii(0); ii!=size-1; ++ii) + for(int jj(ii+1); jj!=size; ++jj) + moves_m.push_back(new swap_elements(ii,jj)); + } + + /// @brief Dtor. + ~swap_full_neighborhood() { + for(move_manager::iterator it = moves_m.begin(); + it != moves_m.end(); ++it) + delete *it; + } + + /// @brief Use the same set set of moves at each iteration. + void refresh(mets::feasible_solution& /*s*/) { } + + }; + + + /// @brief Generates a the full subsequence inversion neighborhood. + class invert_full_neighborhood : public mets::move_manager + { + public: + invert_full_neighborhood(int size) : move_manager() + { + for(int ii(0); ii!=size; ++ii) + for(int jj(0); jj!=size; ++jj) + if(ii != jj) + moves_m.push_back(new invert_subsequence(ii,jj)); + } + + /// @brief Dtor. + ~invert_full_neighborhood() { + for(std::deque::iterator it = moves_m.begin(); + it != moves_m.end(); ++it) + delete *it; + } + + /// @brief This is a static neighborhood + void + refresh(mets::feasible_solution& /*s*/) + { } + + }; + + /// @} + + /// @brief Functor class to allow hash_set of moves (used by tabu list) + class mana_move_hash + { + public: + std::size_t operator()(mana_move const* mov) const + {return mov->hash();} + }; + + /// @brief Functor class to allow hash_set of moves (used by tabu list) + template + struct dereferenced_equal_to + { + bool operator()(const Tp l, + const Tp r) const + { return l->operator==(*r); } + }; + +} + +//________________________________________________________________________ +inline void +mets::permutation_problem::copy_from(const mets::copyable& other) +{ + const mets::permutation_problem& o = + dynamic_cast(other); + pi_m = o.pi_m; + cost_m = o.cost_m; +} + +//________________________________________________________________________ +inline bool +mets::swap_elements::operator==(const mets::mana_move& o) const +{ + try { + const mets::swap_elements& other = + dynamic_cast(o); + return (this->p1 == other.p1 && this->p2 == other.p2); + } catch (std::bad_cast& e) { + return false; + } +} + +//________________________________________________________________________ + +inline void +mets::invert_subsequence::apply(mets::feasible_solution& s) const +{ + mets::permutation_problem& sol = + static_cast(s); + int size = static_cast(sol.size()); + int top = p1 < p2 ? (p2-p1+1) : (size+p2-p1+1); + for(int ii(0); ii!=top/2; ++ii) + { + int from = (p1+ii)%size; + int to = (size+p2-ii)%size; + assert(from >= 0 && from < size); + assert(to >= 0 && to < size); + sol.apply_swap(from, to); + } +} + +inline mets::gol_type +mets::invert_subsequence::evaluate(const mets::feasible_solution& s) const +{ + const mets::permutation_problem& sol = + static_cast(s); + int size = static_cast(sol.size()); + int top = p1 < p2 ? (p2-p1+1) : (size+p2-p1+1); + mets::gol_type eval = 0.0; + for(int ii(0); ii!=top/2; ++ii) + { + int from = (p1+ii)%size; + int to = (size+p2-ii)%size; + assert(from >= 0 && from < size); + assert(to >= 0 && to < size); + eval += sol.evaluate_swap(from, to); + } + return eval; +} + +inline bool +mets::invert_subsequence::operator==(const mets::mana_move& o) const +{ + try { + const mets::invert_subsequence& other = + dynamic_cast(o); + return (this->p1 == other.p1 && this->p2 == other.p2); + } catch (std::bad_cast& e) { + return false; + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/observer.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/observer.hh new file mode 100755 index 0000000..0c6f3c2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/observer.hh @@ -0,0 +1,171 @@ +// -*- C++ -*- +// +// METS lib source file - observer.h +// +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef METS_OBSERVER_HH_ +#define METS_OBSERVER_HH_ + +#include +#include + +namespace mets { + + template + class observer; // forward declaration + + template + class subject; // forward declaration + + /// + /// @brief Functor class to update observers with a for_each, + /// only intended for internal use. + /// + template + class update_observer + { + public: + /// @brief Ctor. + update_observer(observed_subject* who) : who_m(who) {} + /// @brief Subscript operator to update an observer. + void + operator()(observer* o) { o->update(who_m); } + private: + update_observer(); + observed_subject* who_m; + }; + + /// + /// @brief template class for subjects (cfr. Observer Design Pattern). + /// + /// You must declare the subject of the observations with: + /// + /// class my_observed_sbj : public subject + /// + /// Than you should call notify() manually or automatically + /// from every method that changes the subject status. + /// + /// Only attached observers (cfr. attach() and detach() methods) + /// will be notified. + /// + template + class subject + { + public: + virtual + ~subject() {}; + /// @brief Attach a new observer to this subject. + /// + /// @param o: a new observer for this subject. + /// if the observer was already present + /// nothing happens. + virtual void + attach(observer& o); + /// @brief Detach a new observer to this subject. + /// + /// @param o: observer to detach from this subject. + /// if the observer "o" was not present + /// nothing happens. + virtual void + detach(observer& o); + /// @brief Notify all attached observers. + /// + /// When this method is called every + /// observed_subject#update method is called + /// and "this" subject is passed as a param. + /// + virtual void + notify(); + protected: + subject(); + std::set*> observers_m; + }; + + /// + /// @brief Template base class for the observers of some observed_subject + /// + /// You should declare a new observer type of some my_subject this way: + /// + /// class my_observer : public observer + /// + /// Every time notify() is called on the subject every attached + /// observer is updated. + /// + template + class observer + { + public: + virtual + ~observer() {}; + /// @brief This method is automatically called when this + /// observer is attached to a "notified" subject. + /// + /// @param subject: The subject that was notified and that + /// called our update method. + virtual void + update(observed_subject*) = 0; + protected: + observer() {}; + }; + + + // Implementation of the template methods in STL + + template + subject::subject() + : observers_m() { } + + template + void + subject::attach(observer& o) + { observers_m.insert(&o); } + + template + void + subject::detach(observer& o) + { observers_m.erase(&o); } + + template + void + subject::notify() + { + // upcast the object to the real observer_subject type + observed_subject* real_subject = static_cast(this); + std::for_each(observers_m.begin(), observers_m.end(), + update_observer(real_subject)); + } + +} + +#endif // METS_OBSERVER_HH_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/simulated-annealing.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/simulated-annealing.hh new file mode 100755 index 0000000..11c1830 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/simulated-annealing.hh @@ -0,0 +1,277 @@ +// METSlib source file - simulated-annealing.hh -*- C++ -*- +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef METS_SIMULATED_ANNEALING_HH_ +#define METS_SIMULATED_ANNEALING_HH_ + +namespace mets { + + /// @defgroup simulated_annealing Simulated Annealing + /// @{ + + /// @brief Cooling criteria (for Simulated Annealing). + /// + /// @see mets::simulated_annealing + /// + /// An abstract annealing schedule. Implementations + /// should decide the new temperature every time the + /// subscript operator is called (every search iteration) + class abstract_cooling_schedule + { + public: + /// @brief Constructor + abstract_cooling_schedule() + { } + + /// @brief Virtual destructor + virtual + ~abstract_cooling_schedule() + { } + + /// @brief The function that updates the SA temperature. + /// + /// @param temp The actual annealing temperature. + /// @param fs The current working solution. + /// @return The new scheduled temperature. + virtual double + operator()(double temp, feasible_solution& fs) = 0; + }; + + /// @brief Search by Simulated Annealing. + template + class simulated_annealing : public mets::abstract_search + { + public: + using search_type = simulated_annealing; + /// @brief Creates a search by simulated annealing instance. + /// + /// @param working The working solution (this will be modified + /// during search). + /// + /// @param recorder A solution recorder (possibly holding a + /// different solution instance) used to record the best solution + /// found. + /// + /// @param moveman A problem specific implementation of the + /// move_manager_type used to generate the neighborhood (the + /// choice of the neighbourhood and its exploration greatly + /// influences the algorithm quality and speed). + /// + /// @param tc The termination criteria used to terminate the + /// search process, this is an extension to the standard Simulated + /// Annealing: the algorithm terminates either when the + /// termination criterion is met or when the temperature is <= 0. + /// + /// @param cs The annealing schedule that will be used by the + /// algorithm to regulate the temperature at each iteration (many + /// have been proposed in literature and influence the quality and + /// speed of the algorithm). + /// + /// @param starting_temp The starting SA temperature (this is an + /// important parameter that depends on the problem and will + /// influence the search quality and duration). + /// + /// @param K The "Boltzmann" constant that we want ot use (default is 1). + simulated_annealing(evaluable_solution& starting_point, + solution_recorder& recorder, + move_manager_type& moveman, + termination_criteria_chain& tc, + abstract_cooling_schedule& cs, + double starting_temp, + double stop_temp = 1e-7, + double K = 1.0); + + /// purposely not implemented (see Effective C++) + simulated_annealing(const simulated_annealing&); + simulated_annealing& operator=(const simulated_annealing&); + + /// @brief This method starts the simulated annealing search + /// process. + /// + /// Remember that this is a minimization process. + /// + virtual void + search(); + + void setApplyAndEvaluate(bool b) { + apply_and_evaluate = b; + } + + /// @brief The current annealing temperature. + /// + /// @return The current temperature of the algorithm. + double + current_temp() const + { return current_temp_m; } + + /// @brief The annealing schedule instance. + /// + /// @return The cooling schedule used by this search process. + const abstract_cooling_schedule& + cooling_schedule() const + { return cooling_schedule_m; } + + protected: + termination_criteria_chain& termination_criteria_m; + abstract_cooling_schedule& cooling_schedule_m; + double starting_temp_m; + double stop_temp_m; + double current_temp_m; + double K_m; + +#if defined (METSLIB_TR1_BOOST) + boost::uniform_real ureal; + boost::mt19937 rng; + boost::variate_generator< boost::mt19937, boost::uniform_real > gen; +#elif defined (METSLIB_HAVE_UNORDERED_MAP) && !defined (METSLIB_TR1_MIXED_NAMESPACE) + std::uniform_real ureal; + std::mt19937 rng; + std::variate_generator< std::mt19937, std::uniform_real > gen; +#else + std::tr1::uniform_real ureal; + std::tr1::mt19937 rng; + std::tr1::variate_generator< std::tr1::mt19937, std::tr1::uniform_real > gen; +#endif + + bool apply_and_evaluate; + }; + + /// @brief Original ECS proposed by Kirkpatrick + class exponential_cooling + : public abstract_cooling_schedule + { + public: + exponential_cooling(double alpha = 0.95) + : abstract_cooling_schedule(), factor_m(alpha) + { if(alpha >= 1) throw std::runtime_error("alpha must be < 1"); } + double + operator()(double temp, feasible_solution& /*fs*/) + { return temp*factor_m; } + protected: + double factor_m; + }; + + /// @brief Alternative LCS proposed by Randelman and Grest + class linear_cooling + : public abstract_cooling_schedule + { + public: + linear_cooling(double delta = 0.1) + : abstract_cooling_schedule(), decrement_m(delta) + { if(delta <= 0) throw std::runtime_error("delta must be > 0"); } + double + operator()(double temp, feasible_solution& /*fs*/) + { return std::max(0.0, temp-decrement_m); } + protected: + double decrement_m; + }; + + /// @} +} + +template +mets::simulated_annealing:: +simulated_annealing(evaluable_solution& working, + solution_recorder& recorder, + move_manager_t& moveman, + termination_criteria_chain& tc, + abstract_cooling_schedule& cs, + double starting_temp, + double stop_temp, + double K) + : abstract_search(working, recorder, moveman), + termination_criteria_m(tc), cooling_schedule_m(cs), + starting_temp_m(starting_temp), stop_temp_m(stop_temp), + current_temp_m(), K_m(K), + ureal(0.0,1.0), rng(), gen(rng, ureal), apply_and_evaluate(false) +{ +} + +template +void +mets::simulated_annealing::search() +{ + using base_t = abstract_search; + + current_temp_m = starting_temp_m; + while(!termination_criteria_m(base_t::working_solution_m) + && current_temp_m > stop_temp_m) + { + gol_type actual_cost = + static_cast(base_t::working_solution_m) + .cost_function(); + /*gol_type best_cost = + static_cast(base_t::working_solution_m) + .cost_function();*/ + + base_t::moves_m.refresh(base_t::working_solution_m); + for(typename move_manager_t::iterator movit = base_t::moves_m.begin(); + movit != base_t::moves_m.end(); ++movit) + { + // apply move and record proposed cost function + gol_type cost; + if(apply_and_evaluate) { + cost = (*movit)->apply_and_evaluate(base_t::working_solution_m); + } else { + cost = (*movit)->evaluate(base_t::working_solution_m); + } + + double delta = (static_cast(cost-actual_cost)); + if(delta < 0 || gen() < exp(-delta/(K_m*current_temp_m))) + { + // accepted: apply, record, exit for and lower temperature + if(!apply_and_evaluate) + (*movit)->apply(base_t::working_solution_m); + + base_t::current_move_m = movit; + if(base_t::solution_recorder_m.accept(base_t::working_solution_m)) + { + base_t::step_m = base_t::IMPROVEMENT_MADE; + this->notify(); + } + + base_t::step_m = base_t::MOVE_MADE; + this->notify(); + break; + } + else if(apply_and_evaluate) + { + (*movit)->unapply(base_t::working_solution_m); + } + } // end for each move + + current_temp_m = + cooling_schedule_m(current_temp_m, base_t::working_solution_m); + } +} +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/tabu-search.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/tabu-search.hh new file mode 100755 index 0000000..491e23b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/tabu-search.hh @@ -0,0 +1,639 @@ +// METSlib source file - tabu-search.hh -*- C++ -*- +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef METS_TABU_SEARCH_HH_ +#define METS_TABU_SEARCH_HH_ + +namespace mets { + + /// @defgroup tabu_search Tabu Search + /// @{ + + /// @brief Function object expressing an + /// aspiration criteria + /// + /// An aspiration criteria is a criteria used + /// to override the tabu list. When the aspiration + /// criteria is met a move is made even if it's in + /// the tabu-list + /// + /// Aspiration critera can be chained so a criteria can decorate + /// another criteria + class aspiration_criteria_chain + { + public: + /// @brief Constructor. + /// + /// @param next Optional next criteria in the chain. + explicit + aspiration_criteria_chain(aspiration_criteria_chain *next = 0) + : next_m(next) + { } + + /// purposely not implemented (see Effective C++) + aspiration_criteria_chain(const aspiration_criteria_chain& other); + /// purposely not implemented (see Effective C++) + aspiration_criteria_chain& + operator=(const aspiration_criteria_chain& other); + + /// @brief Virtual destructor. + virtual + ~aspiration_criteria_chain() + { } + + /// @brief A method to reset this aspiration criteria chain to its + /// original state. + virtual void + reset(); + + /// @brief This is a callback function from the algorithm that + /// tells us that a move was accepted. + /// + /// You can use this function to update the aspiration criteria + /// based on the current search status. (e.g. record the best cost + /// for a best ever criteria) + /// + /// @param fs The current working solution (after applying move). + /// @param mov The accepted move (the move just made). + /// @return True if the move is to be accepted. + virtual void + accept(feasible_solution& fs, move& mov, gol_type evaluation); + + /// @brief The function that decides if we shoud accept a tabu move + /// + /// @param fs The current working solution (before applying move). + /// @param mov The move to be made (the move that is being evaluated). + /// @return True if the move is to be accepted. + virtual bool + operator()(feasible_solution& fs, move& mov, gol_type evaluation) const; + + protected: + aspiration_criteria_chain* next_m; + }; + + /// + /// @brief An abstract tabu list + /// + /// This is chainable so that tabu lists can be decorated with + /// other tabu lists. + class tabu_list_chain + { + public: + tabu_list_chain(); + /// purposely not implemented (see Effective C++) + tabu_list_chain(const tabu_list_chain&); + /// purposely not implemented (see Effective C++) + tabu_list_chain& operator=(const tabu_list_chain&); + + /// Create an abstract tabu list with a certain tenure + explicit + tabu_list_chain(unsigned int tenure) + : next_m(0), tenure_m(tenure) + { } + + /// @brief Create an abstract tabu list with a certain tenure and + /// a chained tabu list that decorates this one + tabu_list_chain(tabu_list_chain* next, unsigned int tenure) + : next_m(next), tenure_m(tenure) + { } + + /// @brief Virtual destructor + virtual + ~tabu_list_chain() + { } + + /// + /// @brief Make a move tabu when starting from a certain solution. + /// + /// Different implementation can remember "tenure" moves, + /// "tenure" solutions or some other peculiar fact that + /// will avoid cycling. + /// + /// Mind you! The solution here is the solution *before* applying + /// the move: this is for efficiency reason. + /// + /// @param sol The current working solution + /// @param mov The move to make tabu + virtual void + tabu(feasible_solution& sol, move& mov) = 0; + + /// @brief True if the move is tabu for the given solution. + /// + /// Different implementation can remember "tenure" moves, + /// "tenure" solutions or some other peculiar fact that + /// will avoid cycling. So it's not defined at this stage + /// if a move will be tabu or not at a certain state of the + /// search: this depends on the implementation. + /// + /// Mind you! The solution here is the solution *before* applying + /// the move: this is for efficiency reason. + /// + /// @param sol The current working solution + /// @param mov The move to make tabu + virtual bool + is_tabu(feasible_solution& sol, move& mov) const = 0; + + /// + /// @brief Tenure of this tabu list. + /// + /// The tenure is the length of the tabu-list (the order of the + /// tabu memory) + /// + virtual unsigned int + tenure() const + { return tenure_m; } + + /// + /// @brief Tenure of this tabu list. + /// + /// @param tenure: the new tenure of the list. + /// + virtual void + tenure(unsigned int tenure) + { tenure_m = tenure; } + + protected: + tabu_list_chain* next_m; + unsigned int tenure_m; + }; + + /// + /// @brief Tabu Search algorithm. + /// + /// This implements decorator pattern. You can build many + /// different solvers decorating tabu_search class in different + /// ways. + /// + template + class tabu_search : public abstract_search + { + public: + using search_type = tabu_search; + /// @brief Creates a tabu Search instance. + /// + /// @param starting_solution The working solution (this + /// will be modified during search). + /// + /// @param best_recorder A solution recorder used to record the + /// best solution found during the search. + /// + /// @param move_manager_inst A problem specific implementation of the + /// move_manager_type used to generate the neighborhood. + /// + /// @param tabus The tabu list used to decorate this search + /// instance. + /// + /// @param aspiration The aspiration criteria to use in this tabu + /// search. + /// + /// @param termination The termination criteria used to terminate the + /// search process, this is an extension to the standard Simulated + /// Annealing: you can give a termination criteria that termiantes + /// when temperature reaches 0. + /// + tabu_search(feasible_solution& starting_solution, + solution_recorder& best_recorder, + move_manager_type& move_manager_inst, + tabu_list_chain& tabus, + aspiration_criteria_chain& aspiration, + termination_criteria_chain& termination); + + tabu_search(const search_type&); + search_type& operator=(const search_type&); + + virtual + ~tabu_search() {} + + /// @brief This method starts the tabu search process. + /// + /// Remember that this is a minimization process. + /// + /// An exception mets::no_moves_error is risen when no move + /// is possible. + void + search(); + + enum { + ASPIRATION_CRITERIA_MET = abstract_search::LAST, + LAST + }; + + /// @brief The tabu list used by this tabu search + const tabu_list_chain& + get_tabu_list() const { return tabu_list_m; } + + /// @brief The aspiration criteria used by this tabu search + const aspiration_criteria_chain& + get_aspiration_criteria() const { return aspiration_criteria_m; } + + /// @brief The termination criteria used by this tabu search + const termination_criteria_chain& + get_termination_criteria() const { return termination_criteria_m; } + protected: + tabu_list_chain& tabu_list_m; + aspiration_criteria_chain& aspiration_criteria_m; + termination_criteria_chain& termination_criteria_m; + }; + + /// @brief Simplistic implementation of a tabu-list. + /// + /// This class implements one of the simplest and less + /// memory hungry tabu lists. This tabu list memorizes + /// only the moves (not the solutions). + /// + /// Moves must be of mets::mana_move type. + /// + /// The comparison between moves is demanded to the + /// move implementation. + /// + /// A mets::mana_move is tabu if it's in the tabu list by means + /// of its operator== and hash function. + class simple_tabu_list + : public tabu_list_chain + { + public: + /// @brief Ctor. Makes a tabu list of the specified tenure. + /// + /// @param tenure Tenure (length) of the tabu list + simple_tabu_list(unsigned int tenure) + : tabu_list_chain(tenure), + tabu_moves_m(), + tabu_hash_m(tenure) {} + + /// @brief Ctor. Makes a tabu list of the specified tenure. + /// + /// @param tenure Tenure (length) of the tabu list + /// @param next Next list to invoke when this returns false + simple_tabu_list(tabu_list_chain* next, unsigned int tenure) + : tabu_list_chain(next, tenure), + tabu_moves_m(), + tabu_hash_m(tenure) {} + + /// @brief Destructor + ~simple_tabu_list(); + + /// @brief Make move a tabu. + /// + /// This implementation simply remembers "tenure" moves. + /// + /// @param sol The current working solution + /// @param mov The move to make tabu + void + tabu(feasible_solution& sol, /* const */ move& mov); + + /// @brief True if the move is tabu for the given solution. + /// + /// This implementation considers tabu each move already made less + /// then tenure() moves ago. + /// + /// @param sol The current working solution + /// @param mov The move to make tabu + /// @return True if this move was already made during the last + /// tenure iterations + bool + is_tabu(feasible_solution& sol, move& mov) const; + + protected: + using move_list_type = std::deque; +#if defined (METSLIB_TR1_BOOST) + typedef boost::unordered_map< + mana_move*, // Key type + int, //insert a move and the number of times it's present in the list + mana_move_hash, + dereferenced_equal_to > move_map_type; +#elif defined (METSLIB_HAVE_UNORDERED_MAP) && !defined (METSLIB_TR1_MIXED_NAMESPACE) + typedef std::unordered_map< + mana_move*, // Key type + int, //insert a move and the number of times it's present in the list + mana_move_hash, + dereferenced_equal_to > move_map_type; +#else + typedef std::tr1::unordered_map< + mana_move*, // Key type + int, //insert a move and the number of times it's present in the list + mana_move_hash, + dereferenced_equal_to > move_map_type; +#endif + move_list_type tabu_moves_m; + move_map_type tabu_hash_m; + }; + + /// @brief Aspiration criteria implementation. + /// + /// This is one of the best known aspiration criteria + /// ready to be used in your tabu-search implementation. + /// + /// This aspiration criteria is met when a tabu move would result in + /// a global improvement. + class best_ever_criteria : public aspiration_criteria_chain + { + public: + explicit + best_ever_criteria(double min_improvement = 1e-6); + + explicit + best_ever_criteria(aspiration_criteria_chain* next, + double min_improvement = 1e-6); + + void + reset(); + + void + accept(feasible_solution& fs, move& mov, gol_type evaluation); + + bool + operator()(feasible_solution& fs, move& mov, gol_type evaluation) const; + + protected: + gol_type best_m; + gol_type tolerance_m; + }; + + /// @} +} + +template +mets::tabu_search:: +tabu_search (feasible_solution& starting_solution, + solution_recorder& best_recorder, + move_manager_t& move_manager_inst, + tabu_list_chain& tabus, + aspiration_criteria_chain& aspiration, + termination_criteria_chain& termination) + : abstract_search(starting_solution, + best_recorder, + move_manager_inst), + tabu_list_m(tabus), + aspiration_criteria_m(aspiration), + termination_criteria_m(termination) +{} + +template +void mets::tabu_search::search() +{ + using base_t = abstract_search; + while(!termination_criteria_m(base_t::working_solution_m)) + { + // call listeners + base_t::step_m = base_t::ITERATION_BEGIN; + this->notify(); + + base_t::moves_m.refresh(base_t::working_solution_m); + + typename move_manager_t::iterator best_movit = base_t::moves_m.end(); + gol_type best_move_cost = std::numeric_limits::max(); + + for(typename move_manager_t::iterator movit = base_t::moves_m.begin(); + movit != base_t::moves_m.end(); ++movit) + { + // evaluate proposed move + gol_type cost = (*movit)->evaluate(base_t::working_solution_m); + + // save tabu status + bool is_tabu = tabu_list_m.is_tabu(base_t::working_solution_m, + **movit); + + // for each non-tabu move record the best one + if(cost < best_move_cost) + { + + bool aspiration_criteria_met = false; + + // not interesting if this is not a tabu move (and if we + // are not improving over other moves) + if(is_tabu) + { + aspiration_criteria_met = + aspiration_criteria_m(base_t::working_solution_m, + **movit, + cost); + } + + if(!is_tabu || aspiration_criteria_met) + { + best_move_cost = cost; + best_movit = base_t::current_move_m = movit; + if(aspiration_criteria_met) + { + base_t::step_m = ASPIRATION_CRITERIA_MET; + this->notify(); + } + } + } + } // end for each move + + if(best_movit == base_t::moves_m.end()) + throw no_moves_error(); + + // make move tabu + tabu_list_m.tabu(base_t::working_solution_m, **best_movit); + + // do the best non tabu move (unless overridden by aspiration + // criteria, of course) + (*best_movit)->apply(base_t::working_solution_m); + + // call listeners + base_t::step_m = base_t::MOVE_MADE; + this->notify(); + + aspiration_criteria_m.accept(base_t::working_solution_m, + **best_movit, + best_move_cost); + + if(base_t::solution_recorder_m.accept(base_t::working_solution_m)) + { + base_t::step_m = base_t::IMPROVEMENT_MADE; + this->notify(); + } + + // call listeners + base_t::step_m = base_t::ITERATION_END; + this->notify(); + + } // end while(!termination) +} + +// chain of responsibility + +inline void +mets::tabu_list_chain::tabu(feasible_solution& sol, /* const */ move& mov) +{ + if(next_m) + next_m->tabu(sol, mov); +} + +inline bool +mets::tabu_list_chain::is_tabu(feasible_solution& sol, /* const */ move& mov) const +{ + if(next_m) + return next_m->is_tabu(sol, mov); + else + return false; +} + +inline mets::simple_tabu_list::~simple_tabu_list() +{ + for(move_map_type::iterator m = tabu_hash_m.begin(); + m!=tabu_hash_m.end(); ++m) + delete m->first; +} + +inline void +mets::simple_tabu_list::tabu(feasible_solution& sol, /* const */ move& mov) +{ + mana_move* mc = + dynamic_cast(mov).opposite_of(); + + // This does nothing if the move was already tabu (can happen when + // aspiration criteria is met). + std::pair + insert_result = tabu_hash_m.insert(std::make_pair(mc, 1)); + + // If it was already in the map, increase the counter + if(!insert_result.second) + { + insert_result.first->second++; + delete mc; + mc = insert_result.first->first; + } + // Always add the move at the end of the list (when aspiration + // criteria is met a move can be present more than one time in this + // list: this is correct, so the last made move is always the last + // in the queue). + tabu_moves_m.push_back(mc); + + // Since we use the hash size, the tenure is the number of different + // moves on the list (the tabu_moves_m can have more than tenure + // elements) + while(tabu_hash_m.size() > this->tenure()) + { + // update hash map *and* list structures + move_map_type::iterator elem = tabu_hash_m.find + (dynamic_cast(tabu_moves_m.front())); + elem->second--; + if(elem->second == 0) + { + mana_move* tmp = elem->first; + tabu_hash_m.erase(elem); + delete tmp; + } + tabu_moves_m.pop_front(); + } + tabu_list_chain::tabu(sol, mov); +} + +inline bool +mets::simple_tabu_list::is_tabu(feasible_solution& sol, move& mov) const +{ + // hash set. very fast but requires C++ ISO TR1 extension + // and an hash function in every move (Omega(1)). + bool tabu = (tabu_hash_m.find(&dynamic_cast(mov)) + != tabu_hash_m.end()); + + if(tabu) + return true; + + return tabu_list_chain::is_tabu(sol, mov); +} + +////////////////////////////////////////////////////////////////////////// +// aspiration_criteria_chain +inline void +mets::aspiration_criteria_chain::reset() +{ + if(next_m) + return next_m->reset(); +} + +inline void +mets::aspiration_criteria_chain::accept(feasible_solution& fs, + move& mov, + gol_type eval) +{ + if(next_m) next_m->accept(fs, mov, eval); +} + +inline bool +mets::aspiration_criteria_chain::operator()(feasible_solution& fs, + move& mov, + gol_type eval) const +{ + if(next_m) + return next_m->operator()(fs, mov, eval); + else + return false; +} + +////////////////////////////////////////////////////////////////////////// +// best_ever_criteria +inline mets::best_ever_criteria::best_ever_criteria(double tolerance) + : aspiration_criteria_chain(), + best_m(std::numeric_limits::max()), + tolerance_m(tolerance) +{ } + +inline mets::best_ever_criteria::best_ever_criteria(aspiration_criteria_chain* next, double tolerance) + : aspiration_criteria_chain(next), + best_m(std::numeric_limits::max()), + tolerance_m(tolerance) +{ } + +inline void +mets::best_ever_criteria::reset() +{ + best_m = std::numeric_limits::max(); + aspiration_criteria_chain::reset(); +} + +inline void +mets::best_ever_criteria::accept(feasible_solution& fs, + move& mov, + gol_type eval) +{ + best_m = std::min(dynamic_cast(fs).cost_function(), best_m); + aspiration_criteria_chain::accept(fs, mov, eval); +} + +inline bool +mets::best_ever_criteria::operator()(feasible_solution& fs, + move& mov, + gol_type eval) const +{ + /// the solution is the solution before applying mov. + if(eval < best_m - tolerance_m) + return true; + else + return aspiration_criteria_chain::operator()(fs, mov, eval); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/termination-criteria.hh b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/termination-criteria.hh new file mode 100755 index 0000000..608eaea --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/3rdparty/metslib/termination-criteria.hh @@ -0,0 +1,287 @@ +// METSlib source file - termination-criteria.hh -*- C++ -*- +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2006-2012, Mirko Maischberger + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef METS_TERMINATION_CRITERIA_HH_ +#define METS_TERMINATION_CRITERIA_HH_ + +namespace mets { + + /// @defgroup common Termination criteria + /// @{ + + /// @brief Function object expressing a termination criteria + /// + /// The search loop ends when the termination criteria is met. + class termination_criteria_chain + { + public: + /// @brief Constructor. + /// + /// @param next Optional next criterium in the chain. + explicit + termination_criteria_chain(termination_criteria_chain* next = 0) + : next_m(next) + { } + /// purposely not implemented (see Effective C++) + termination_criteria_chain(const termination_criteria_chain&); + termination_criteria_chain& operator=(const termination_criteria_chain&); + + /// @brief Virtual destructor. + virtual + ~termination_criteria_chain() + { } + + /// @brief Alternate function that decides if we shoud terminate the + /// search process + /// + /// (chain of responsibility) + /// + /// @param fs The current working solution. + /// @return True if we shoud terminate + virtual bool + operator()(const feasible_solution& fs); + + /// @brief Reset the criterion to its initial state. + /// + /// (chain of responsibility) + /// + virtual void reset(); + + protected: + termination_criteria_chain* next_m; + }; + + /// @brief Termination criteria based on the number of iterations. + /// + /// This termination criteria terminates the tabu-search + /// after a fixed number of itarations. + class iteration_termination_criteria + : public termination_criteria_chain + { + public: + /// @brief Ctor. Max is the number of iterations to do. + iteration_termination_criteria(int max) + : termination_criteria_chain(), + max_m(max), + iterations_m(max) {} + + explicit + iteration_termination_criteria + (termination_criteria_chain* next, int max) + : termination_criteria_chain(next), + max_m(max), + iterations_m(max) {} + + bool + operator()(const feasible_solution& fs) + { + if (iterations_m <= 0) + return true; + + --iterations_m; + return termination_criteria_chain::operator()(fs); + } + + void + reset() + { iterations_m = max_m; termination_criteria_chain::reset(); } + + protected: + int max_m; + int iterations_m; + }; + + /// @brief Termination criteria based on the number + /// of iterations without an improvement. + /// + /// This termination criteria terminates the tabu-search + /// after "max" number of itarations without a single + /// global improvement. + class noimprove_termination_criteria + : public termination_criteria_chain + { + public: + noimprove_termination_criteria(int max, gol_type epsilon = 1e-7) + : termination_criteria_chain(), + best_cost_m(std::numeric_limits::max()), + max_noimprove_m(max), + iterations_left_m(max), + total_iterations_m(0), + resets_m(0), + second_guess_m(0), + epsilon_m(epsilon) + {} + + noimprove_termination_criteria + (termination_criteria_chain* next, int max, gol_type epsilon = 1e-7) + : termination_criteria_chain(next), + best_cost_m(std::numeric_limits::max()), + max_noimprove_m(max), + iterations_left_m(max), + total_iterations_m(0), + resets_m(0), + second_guess_m(0), + epsilon_m(epsilon) + { } + + bool + operator()(const feasible_solution& fs); + void reset() + { iterations_left_m = max_noimprove_m; + second_guess_m = total_iterations_m = resets_m = 0; + best_cost_m = std::numeric_limits::max(); + termination_criteria_chain::reset(); + } + + int second_guess() { return second_guess_m; } + int iteration() { return total_iterations_m; } + int resets() { return resets_m; } + + protected: + gol_type best_cost_m; + int max_noimprove_m; + int iterations_left_m; + int total_iterations_m; + int resets_m; + int second_guess_m; + gol_type epsilon_m; + }; + + /// @brief Termination criteria based on cost value + /// + /// This termination criteria terminates the tabu-search + /// when a certain threshold is reached + class threshold_termination_criteria + : public termination_criteria_chain + { + public: + threshold_termination_criteria(gol_type level, gol_type epsilon = 1e-7) + : termination_criteria_chain(), + level_m(level), + epsilon_m(epsilon) + { } + + threshold_termination_criteria + (termination_criteria_chain* next, gol_type level, gol_type epsilon = 1e-7) + : termination_criteria_chain(next), + level_m(level), + epsilon_m(epsilon) + { } + + bool + operator()(const feasible_solution& fs) + { + mets::gol_type current_cost = + dynamic_cast(fs).cost_function(); + + if(current_cost < level_m + epsilon_m) + return true; + + return termination_criteria_chain::operator()(fs); + } + + void reset() + { termination_criteria_chain::reset(); } + + protected: + gol_type level_m; + gol_type epsilon_m; + }; + + /// The mets::forever termination criterion will never terminate the + /// search. + /// + /// This can be used in the mets::simulated_annealing to stop only + /// when the temperature reaches 0 or in the mets::tabu_search if we + /// want to stop for another reason (e.g. some components or + /// observer raises an exception). + /// + /// The forever termination criterion cannot be chained. When + /// chained behaviour is undetermined. + class forever : public termination_criteria_chain + { + public: + forever() : termination_criteria_chain() {} + bool + operator()(const feasible_solution& /*fs*/) + { return false; } + void reset() + { termination_criteria_chain::reset(); } + }; + + /// @} +} + +//________________________________________________________________________ +inline bool +mets::termination_criteria_chain::operator()(const feasible_solution& fs) +{ + if(next_m) + return next_m->operator()(fs); + else + return false; +} + +//________________________________________________________________________ +inline void +mets::termination_criteria_chain::reset() +{ + if(next_m) next_m->reset(); +} + +//________________________________________________________________________ +inline bool +mets::noimprove_termination_criteria::operator()(const feasible_solution& fs) +{ + mets::gol_type current_cost = + dynamic_cast(fs).cost_function(); + if(current_cost < best_cost_m - epsilon_m) + { + best_cost_m = current_cost; + second_guess_m = std::max(second_guess_m, + (max_noimprove_m - iterations_left_m)); + iterations_left_m = max_noimprove_m; + resets_m++; + } + + + if(iterations_left_m <= 0) + return true; + + total_iterations_m++; + --iterations_left_m; + + return termination_criteria_chain::operator()(fs); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/auxiliary.h b/deps_install/include/pcl-1.10/pcl/recognition/auxiliary.h new file mode 100755 index 0000000..0a553ed --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/auxiliary.h @@ -0,0 +1,464 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +#define AUX_PI_FLOAT 3.14159265358979323846f +#define AUX_HALF_PI 1.57079632679489661923f +#define AUX_DEG_TO_RADIANS (3.14159265358979323846f/180.0f) + +namespace pcl +{ + namespace recognition + { + namespace aux + { + template bool + compareOrderedPairs (const std::pair& a, const std::pair& b) + { + if ( a.first == b.first ) + return a.second < b.second; + + return a.first < b.first; + } + + template T + sqr (T a) + { + return (a*a); + } + + template T + clamp (T value, T min, T max) + { + if ( value < min ) + return min; + if ( value > max ) + return max; + + return value; + } + + /** \brief Expands the destination bounding box 'dst' such that it contains 'src'. */ + template void + expandBoundingBox (T dst[6], const T src[6]) + { + if ( src[0] < dst[0] ) dst[0] = src[0]; + if ( src[2] < dst[2] ) dst[2] = src[2]; + if ( src[4] < dst[4] ) dst[4] = src[4]; + + if ( src[1] > dst[1] ) dst[1] = src[1]; + if ( src[3] > dst[3] ) dst[3] = src[3]; + if ( src[5] > dst[5] ) dst[5] = src[5]; + } + + /** \brief Expands the bounding box 'bbox' such that it contains the point 'p'. */ + template void + expandBoundingBoxToContainPoint (T bbox[6], const T p[3]) + { + if ( p[0] < bbox[0] ) bbox[0] = p[0]; + else if ( p[0] > bbox[1] ) bbox[1] = p[0]; + + if ( p[1] < bbox[2] ) bbox[2] = p[1]; + else if ( p[1] > bbox[3] ) bbox[3] = p[1]; + + if ( p[2] < bbox[4] ) bbox[4] = p[2]; + else if ( p[2] > bbox[5] ) bbox[5] = p[2]; + } + + /** \brief v[0] = v[1] = v[2] = value */ + template void + set3 (T v[3], T value) + { + v[0] = v[1] = v[2] = value; + } + + /** \brief dst = src */ + template void + copy3 (const T src[3], T dst[3]) + { + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + } + + /** \brief dst = src */ + template void + copy3 (const T src[3], pcl::PointXYZ& dst) + { + dst.x = src[0]; + dst.y = src[1]; + dst.z = src[2]; + } + + /** \brief a = -a */ + template void + flip3 (T a[3]) + { + a[0] = -a[0]; + a[1] = -a[1]; + a[2] = -a[2]; + } + + /** \brief a = b */ + template bool + equal3 (const T a[3], const T b[3]) + { + return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]); + } + + /** \brief a += b */ + template void + add3 (T a[3], const T b[3]) + { + a[0] += b[0]; + a[1] += b[1]; + a[2] += b[2]; + } + + /** \brief c = a + b */ + template void + sum3 (const T a[3], const T b[3], T c[3]) + { + c[0] = a[0] + b[0]; + c[1] = a[1] + b[1]; + c[2] = a[2] + b[2]; + } + + /** \brief c = a - b */ + template void + diff3 (const T a[3], const T b[3], T c[3]) + { + c[0] = a[0] - b[0]; + c[1] = a[1] - b[1]; + c[2] = a[2] - b[2]; + } + + template void + cross3 (const T v1[3], const T v2[3], T out[3]) + { + out[0] = v1[1]*v2[2] - v1[2]*v2[1]; + out[1] = v1[2]*v2[0] - v1[0]*v2[2]; + out[2] = v1[0]*v2[1] - v1[1]*v2[0]; + } + + /** \brief Returns the length of v. */ + template T + length3 (const T v[3]) + { + return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); + } + + /** \brief Returns the Euclidean distance between a and b. */ + template T + distance3 (const T a[3], const T b[3]) + { + T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]}; + return (length3 (l)); + } + + /** \brief Returns the squared Euclidean distance between a and b. */ + template T + sqrDistance3 (const T a[3], const T b[3]) + { + return (aux::sqr (a[0]-b[0]) + aux::sqr (a[1]-b[1]) + aux::sqr (a[2]-b[2])); + } + + /** \brief Returns the dot product a*b */ + template T + dot3 (const T a[3], const T b[3]) + { + return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]); + } + + /** \brief Returns the dot product (x, y, z)*(u, v, w) = x*u + y*v + z*w */ + template T + dot3 (T x, T y, T z, T u, T v, T w) + { + return (x*u + y*v + z*w); + } + + /** \brief v = scalar*v. */ + template void + mult3 (T* v, T scalar) + { + v[0] *= scalar; + v[1] *= scalar; + v[2] *= scalar; + } + + /** \brief out = scalar*v. */ + template void + mult3 (const T* v, T scalar, T* out) + { + out[0] = v[0]*scalar; + out[1] = v[1]*scalar; + out[2] = v[2]*scalar; + } + + /** \brief Normalize v */ + template void + normalize3 (T v[3]) + { + T inv_len = (static_cast (1.0))/aux::length3 (v); + v[0] *= inv_len; + v[1] *= inv_len; + v[2] *= inv_len; + } + + /** \brief Returns the square length of v. */ + template T + sqrLength3 (const T v[3]) + { + return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); + } + + /** Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'. */ + template void + projectOnPlane3 (const T x[3], const T planeNormal[3], T out[3]) + { + T dot = aux::dot3 (planeNormal, x); + // Project 'x' on the plane normal + T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]}; + aux::sum3 (x, nproj, out); + } + + /** \brief Sets 'm' to the 3x3 identity matrix. */ + template void + identity3x3 (T m[9]) + { + m[0] = m[4] = m[8] = 1.0; + m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0; + } + + /** \brief out = mat*v. 'm' is an 1D array of 9 elements treated as a 3x3 matrix (row major order). */ + template void + mult3x3(const T m[9], const T v[3], T out[3]) + { + out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2]; + out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5]; + out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8]; + } + + /** Let x, y, z be the columns of the matrix a = [x|y|z]. The method computes out = a*m. + * Note that 'out' is a 1D array of 9 elements and the resulting matrix is stored in row + * major order, i.e., the first matrix row is (out[0] out[1] out[2]), the second + * (out[3] out[4] out[5]) and the third (out[6] out[7] out[8]). */ + template void + mult3x3 (const T x[3], const T y[3], const T z[3], const T m[3][3], T out[9]) + { + out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0]; + out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1]; + out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2]; + + out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0]; + out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1]; + out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2]; + + out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0]; + out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1]; + out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2]; + } + + /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. + * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */ + template void + transform(const T t[12], const T p[3], T out[3]) + { + out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9]; + out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10]; + out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11]; + } + + /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. + * First, (x, y, z) is multiplied by that matrix and then translated. The result is saved in 'out'. */ + template void + transform(const T t[12], T x, T y, T z, T out[3]) + { + out[0] = t[0]*x + t[1]*y + t[2]*z + t[9]; + out[1] = t[3]*x + t[4]*y + t[5]*z + t[10]; + out[2] = t[6]*x + t[7]*y + t[8]*z + t[11]; + } + + /** \brief Compute out = (upper left 3x3 of mat)*p + last column of mat. */ + template void + transform(const Eigen::Matrix& mat, const pcl::PointXYZ& p, pcl::PointXYZ& out) + { + out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3); + out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3); + out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3); + } + + /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. + * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */ + template void + transform(const T t[12], const pcl::PointXYZ& p, T out[3]) + { + out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9]; + out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10]; + out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11]; + } + + /** \brief Returns true if the points 'p1' and 'p2' are co-planar and false otherwise. The method assumes that 'n1' + * is a normal at 'p1' and 'n2' is a normal at 'p2'. 'max_angle' is the threshold used for the test. The bigger + * the value the larger the deviation between the normals can be which still leads to a positive test result. The + * angle has to be in radians. */ + template bool + pointsAreCoplanar (const T p1[3], const T n1[3], const T p2[3], const T n2[3], T max_angle) + { + // Compute the angle between 'n1' and 'n2' and compare it with 'max_angle' + if ( std::acos (aux::clamp (aux::dot3 (n1, n2), -1.0f, 1.0f)) > max_angle ) + return (false); + + T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]}; + aux::normalize3 (cl); + + // Compute the angle between 'cl' and 'n1' + T tmp_angle = std::acos (aux::clamp (aux::dot3 (n1, cl), -1.0f, 1.0f)); + + // 'tmp_angle' should not deviate too much from 90 degrees + if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle ) + return (false); + + // All tests passed => the points are coplanar + return (true); + } + + template void + array12ToMatrix4x4 (const Scalar src[12], Eigen::Matrix& dst) + { + dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9]; + dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10]; + dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11]; + dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0; + } + + template void + matrix4x4ToArray12 (const Eigen::Matrix& src, Scalar dst[12]) + { + dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3); + dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3); + dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3); + } + + /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order. + * dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); + * dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); + * dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); + * */ + template void + eigenMatrix3x3ToArray9RowMajor (const Eigen::Matrix& src, T dst[9]) + { + dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); + dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); + dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); + } + + /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order. + * dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; + * dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; + * dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; + * */ + template void + toEigenMatrix3x3RowMajor (const T src[9], Eigen::Matrix& dst) + { + dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; + dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; + dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; + } + + /** brief Computes a rotation matrix from the provided input vector 'axis_angle'. The direction of 'axis_angle' is the rotation axis + * and its magnitude is the angle of rotation about that axis. 'rotation_matrix' is the output rotation matrix saved in row major order. */ + template void + axisAngleToRotationMatrix (const T axis_angle[3], T rotation_matrix[9]) + { + // Get the angle of rotation + T angle = aux::length3 (axis_angle); + if ( angle == 0.0 ) + { + // Undefined rotation -> set to identity + aux::identity3x3 (rotation_matrix); + return; + } + + // Normalize the input + T normalized_axis_angle[3]; + aux::mult3 (axis_angle, static_cast (1.0)/angle, normalized_axis_angle); + + // The eigen objects + Eigen::Matrix mat_axis(normalized_axis_angle); + Eigen::AngleAxis eigen_angle_axis (angle, mat_axis); + + // Save the output + aux::eigenMatrix3x3ToArray9RowMajor (eigen_angle_axis.toRotationMatrix (), rotation_matrix); + } + + /** brief Extracts the angle-axis representation from 'rotation_matrix', i.e., computes a rotation 'axis' and an 'angle' + * of rotation about that axis from the provided input. The output 'angle' is in the range [0, pi] and 'axis' is normalized. */ + template void + rotationMatrixToAxisAngle (const T rotation_matrix[9], T axis[3], T& angle) + { + // The eigen objects + Eigen::AngleAxis angle_axis; + Eigen::Matrix rot_mat; + // Copy the input matrix to the eigen matrix in row major order + aux::toEigenMatrix3x3RowMajor (rotation_matrix, rot_mat); + + // Do the computation + angle_axis.fromRotationMatrix (rot_mat); + + // Save the result + axis[0] = angle_axis.axis () (0,0); + axis[1] = angle_axis.axis () (1,0); + axis[2] = angle_axis.axis () (2,0); + angle = angle_axis.angle (); + + // Make sure that 'angle' is in the range [0, pi] + if ( angle > AUX_PI_FLOAT ) + { + angle = 2.0f*AUX_PI_FLOAT - angle; + aux::flip3 (axis); + } + } + } // namespace aux + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/boost.h b/deps_install/include/pcl-1.10/pcl/recognition/boost.h new file mode 100755 index 0000000..f129eda --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/boost.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include diff --git a/deps_install/include/pcl-1.10/pcl/recognition/bvh.h b/deps_install/include/pcl-1.10/pcl/recognition/bvh.h new file mode 100755 index 0000000..fd9e054 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/bvh.h @@ -0,0 +1,310 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * bvh.h + * + * Created on: Mar 7, 2013 + * Author: papazov + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + /** \brief This class is an implementation of bounding volume hierarchies. Use the build method to construct + * the data structure. To use the class, construct an std::vector of pointers to BVH::BoundedObject objects + * and pass it to the build method. BVH::BoundedObject is a template class, so you can save user-defined data + * in it. + * + * The tree is built such that each leaf contains exactly one object. */ + template + class PCL_EXPORTS BVH + { + public: + class BoundedObject + { + public: + BoundedObject (const UserData& data) + : data_ (data) + { + } + + virtual ~BoundedObject () + { + } + + /** \brief This method is for std::sort. */ + inline static bool + compareCentroidsXCoordinates (const BoundedObject* a, const BoundedObject* b) + { + return a->getCentroid ()[0] < b->getCentroid ()[0]; + } + + float* + getBounds () + { + return (bounds_); + } + + float* + getCentroid () + { + return (centroid_); + } + + const float* + getCentroid () const + { + return (centroid_); + } + + UserData& + getData () + { + return (data_); + } + + protected: + /** These are the bounds of the object.*/ + float bounds_[6]; + /** This is the centroid. */ + float centroid_[3]; + /** This is the user-defined data object. */ + UserData data_; + }; + + protected: + class Node + { + public: + /** \brief 'sorted_objects' is a sorted vector of bounded objects. It has to be sorted in ascending order according + * to the objects' x-coordinates. The constructor recursively calls itself with the right 'first_id' and 'last_id' + * and with the same vector 'sorted_objects'. */ + Node (std::vector& sorted_objects, int first_id, int last_id) + { + // Initialize the bounds of the node + memcpy (bounds_, sorted_objects[first_id]->getBounds (), 6*sizeof (float)); + + // Expand the bounds of the node + for ( int i = first_id + 1 ; i <= last_id ; ++i ) + aux::expandBoundingBox(bounds_, sorted_objects[i]->getBounds()); + + // Shall we create children? + if ( first_id != last_id ) + { + // Division by 2 + int mid_id = (first_id + last_id) >> 1; + children_[0] = new Node(sorted_objects, first_id, mid_id); + children_[1] = new Node(sorted_objects, mid_id + 1, last_id); + } + else + { + // We reached a leaf + object_ = sorted_objects[first_id]; + children_[0] = children_[1] = nullptr; + } + } + + virtual ~Node () + { + if ( children_[0] ) + { + delete children_[0]; + delete children_[1]; + } + } + + bool + hasChildren () const + { + return static_cast(children_[0]); + } + + Node* + getLeftChild () + { + return children_[0]; + } + + Node* + getRightChild () + { + return children_[1]; + } + + BoundedObject* + getObject () + { + return object_; + } + + bool + isLeaf () const + { + return !static_cast(children_[0]); + } + + /** \brief Returns true if 'box' intersects or touches (with a side or a vertex) this node. */ + inline bool + intersect(const float box[6]) const + { + return !(box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] || + box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5]); + } + + /** \brief Computes and returns the volume of the bounding box of this node. */ + double + computeBoundingBoxVolume() const + { + return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]); + } + + friend class BVH; + + protected: + float bounds_[6]; + Node* children_[2]; + BoundedObject* object_; + }; + + public: + BVH() + : root_ (nullptr), + sorted_objects_ (nullptr) + { + } + + virtual ~BVH() + { + this->clear (); + } + + /** \brief Creates the tree. No need to call clear, it's called within the method. 'objects' is a vector of + * pointers to bounded objects which have to have valid bounds and centroids. Use the getData method of + * BoundedObject to retrieve the user-defined data saved in the object. Note that vector will be sorted within + * the method! + * + * The tree is built such that each leaf contains exactly one object. */ + void + build(std::vector& objects) + { + this->clear(); + + if ( objects.empty () ) + return; + + sorted_objects_ = &objects; + + // Now sort the objects according to the x-coordinates of their centroids + std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates); + + // Create the root -> it recursively creates the children nodes until each leaf contains exactly one object + root_ = new Node (objects, 0, static_cast (objects.size () - 1)); + } + + /** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */ + void + clear() + { + if ( root_ ) + { + delete root_; + root_ = nullptr; + } + } + + inline const std::vector* + getInputObjects () const + { + return (sorted_objects_); + } + + /** \brief Pushes back in 'intersected_objects' the bounded objects intersected by the input 'box' and returns true. + * Returns false if no objects are intersected. */ + inline bool + intersect(const float box[6], std::list& intersected_objects) const + { + if ( !root_ ) + return false; + + bool got_intersection = false; + + // Start the intersection process at the root + std::list working_list; + working_list.push_back (root_); + + while ( !working_list.empty () ) + { + Node* node = working_list.front (); + working_list.pop_front (); + + // Is 'node' intersected by the box? + if ( node->intersect (box) ) + { + // We have to check the children of the intersected 'node' + if ( node->hasChildren () ) + { + working_list.push_back (node->getLeftChild ()); + working_list.push_back (node->getRightChild ()); + } + else // 'node' is a leaf -> save it's object in the output list + { + intersected_objects.push_back (node->getObject ()); + got_intersection = true; + } + } + } + + return (got_intersection); + } + + protected: + Node* root_; + std::vector* sorted_objects_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/cg/correspondence_grouping.h b/deps_install/include/pcl-1.10/pcl/recognition/cg/correspondence_grouping.h new file mode 100755 index 0000000..f2ab0ae --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/cg/correspondence_grouping.h @@ -0,0 +1,199 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Abstract base class for Correspondence Grouping algorithms. + * + * \author Tommaso Cavallari, Federico Tombari, Aitor Aldoma + * \ingroup recognition + */ + template + class CorrespondenceGrouping : public PCLBase + { + public: + using SceneCloud = pcl::PointCloud; + using SceneCloudPtr = typename SceneCloud::Ptr; + using SceneCloudConstPtr = typename SceneCloud::ConstPtr; + + /** \brief Empty constructor. */ + CorrespondenceGrouping () : scene_ () {} + + /** \brief destructor. */ + ~CorrespondenceGrouping() + { + scene_.reset (); + model_scene_corrs_.reset (); + } + + /** \brief Provide a pointer to the scene dataset. + * + * \param[in] scene the const boost shared pointer to a PointCloud message. + */ + virtual inline void + setSceneCloud (const SceneCloudConstPtr &scene) + { + scene_ = scene; + } + + /** \brief Getter for the scene dataset. + * + * \return the const boost shared pointer to a PointCloud message. + */ + inline SceneCloudConstPtr + getSceneCloud () const + { + return (scene_); + } + + /** \brief Provide a pointer to the precomputed correspondences between points in the input dataset and + * points in the scene dataset. The correspondences are going to be clustered into different model hypotheses + * by the algorithm. + * + * \param[in] corrs the correspondences between the model and the scene. + */ + virtual inline void + setModelSceneCorrespondences (const CorrespondencesConstPtr &corrs) + { + model_scene_corrs_ = corrs; + } + + /** \brief Getter for the precomputed correspondences between points in the input dataset and + * points in the scene dataset. + * + * \return the correspondences between the model and the scene. + */ + inline CorrespondencesConstPtr + getModelSceneCorrespondences () const + { + return (model_scene_corrs_); + } + + /** \brief Getter for the vector of characteristic scales associated to each cluster + * + * \return the vector of characteristic scales (assuming scale = model / scene) + */ + inline std::vector + getCharacteristicScales () const + { + return (corr_group_scale_); + } + + /** \brief Clusters the input correspondences belonging to different model instances. + * + * \param[out] clustered_corrs a vector containing the correspondences for each instance of the model found within the input data. + */ + void + cluster (std::vector &clustered_corrs); + + protected: + /** \brief The scene cloud. */ + SceneCloudConstPtr scene_; + + using PCLBase::input_; + + /** \brief The correspondences between points in the input and the scene datasets. */ + CorrespondencesConstPtr model_scene_corrs_; + + /** \brief characteristic scale associated to each correspondence subset; + * if the cg algorithm can not handle scale invariance, the size of the vector will be 0. */ + std::vector corr_group_scale_; + + /** \brief The actual clustering method, should be implemented by each subclass. + * + * \param[out] clustered_corrs a vector containing the correspondences for each instance of the model found within the input data. + */ + virtual void + clusterCorrespondences (std::vector &clustered_corrs) = 0; + + /** \brief This method should get called before starting the actual computation. + * + * Internally, initCompute() does the following: + * - checks if an input dataset is given, and returns false otherwise + * - checks if a scene dataset is given, and returns false otherwise + * - checks if the model-scene correspondences have been given, and returns false otherwise + */ + inline bool + initCompute () + { + if (!PCLBase::initCompute ()) + { + return (false); + } + + if (!scene_) + { + PCL_ERROR ("[initCompute] Scene not set.\n"); + return (false); + } + + if (!input_) + { + PCL_ERROR ("[initCompute] Input not set.\n"); + return (false); + } + + if (!model_scene_corrs_) + { + PCL_ERROR ("[initCompute] Model-Scene Correspondences not set.\n"); + return (false); + } + + return (true); + } + + /** \brief This method should get called after finishing the actual computation. + * + */ + inline bool + deinitCompute () + { + return (true); + } + + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/recognition/cg/geometric_consistency.h b/deps_install/include/pcl-1.10/pcl/recognition/cg/geometric_consistency.h new file mode 100755 index 0000000..41d3ef5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/cg/geometric_consistency.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + + /** \brief Class implementing a 3D correspondence grouping enforcing geometric consistency among feature correspondences + * + * \author Federico Tombari, Tommaso Cavallari, Aitor Aldoma + * \ingroup recognition + */ + template + class GeometricConsistencyGrouping : public CorrespondenceGrouping + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using SceneCloudConstPtr = typename pcl::CorrespondenceGrouping::SceneCloudConstPtr; + + /** \brief Constructor */ + GeometricConsistencyGrouping () + : gc_threshold_ (3) + , gc_size_ (1.0) + {} + + + /** \brief Sets the minimum cluster size + * \param[in] threshold the minimum cluster size + */ + inline void + setGCThreshold (int threshold) + { + gc_threshold_ = threshold; + } + + /** \brief Gets the minimum cluster size. + * + * \return the minimum cluster size used by GC. + */ + inline int + getGCThreshold () const + { + return (gc_threshold_); + } + + /** \brief Sets the consensus set resolution. This should be in metric units. + * + * \param[in] gc_size consensus set resolution. + */ + inline void + setGCSize (double gc_size) + { + gc_size_ = gc_size; + } + + /** \brief Gets the consensus set resolution. + * + * \return the consensus set resolution. + */ + inline double + getGCSize () const + { + return (gc_size_); + } + + /** \brief The main function, recognizes instances of the model into the scene set by the user. + * + * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene. + * + * \return true if the recognition had been successful or false if errors have occurred. + */ + bool + recognize (std::vector > &transformations); + + /** \brief The main function, recognizes instances of the model into the scene set by the user. + * + * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene. + * \param[out] clustered_corrs a vector containing the correspondences for each instance of the model found within the input data (the same output of clusterCorrespondences). + * + * \return true if the recognition had been successful or false if errors have occurred. + */ + bool + recognize (std::vector > &transformations, std::vector &clustered_corrs); + + protected: + using CorrespondenceGrouping::input_; + using CorrespondenceGrouping::scene_; + using CorrespondenceGrouping::model_scene_corrs_; + + /** \brief Minimum cluster size. It shouldn't be less than 3, since at least 3 correspondences are needed to compute the 6DOF pose */ + int gc_threshold_; + + /** \brief Resolution of the consensus set used to cluster correspondences together*/ + double gc_size_; + + /** \brief Transformations found by clusterCorrespondences method. */ + std::vector > found_transformations_; + + /** \brief Cluster the input correspondences in order to distinguish between different instances of the model into the scene. + * + * \param[out] model_instances a vector containing the clustered correspondences for each model found on the scene. + * \return true if the clustering had been successful or false if errors have occurred. + */ + void + clusterCorrespondences (std::vector &model_instances) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/cg/hough_3d.h b/deps_install/include/pcl-1.10/pcl/recognition/cg/hough_3d.h new file mode 100755 index 0000000..8d4d88d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/cg/hough_3d.h @@ -0,0 +1,516 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id:$ + * + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace pcl +{ + namespace recognition + { + /** \brief HoughSpace3D is a 3D voting space. Cast votes can be interpolated in order to better deal with approximations introduced by bin quantization. A weight can also be associated with each vote. + * \author Federico Tombari (original), Tommaso Cavallari (PCL port) + * \ingroup recognition + */ + class PCL_EXPORTS HoughSpace3D + { + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Constructor + * + * \param[in] min_coord minimum (x,y,z) coordinates of the Hough space + * \param[in] bin_size size of each bing of the Hough space. + * \param[in] max_coord maximum (x,y,z) coordinates of the Hough space. + */ + HoughSpace3D (const Eigen::Vector3d &min_coord, const Eigen::Vector3d &bin_size, const Eigen::Vector3d &max_coord); + + /** \brief Reset all cast votes. */ + void + reset (); + + /** \brief Casting a vote for a given position in the Hough space. + * + * \param[in] single_vote_coord coordinates of the vote being cast (in absolute coordinates) + * \param[in] weight weight associated with the vote. + * \param[in] voter_id the numeric id of the voter. Useful to trace back the voting correspondence, if the vote is returned by findMaxima as part of a maximum of the Hough Space. + * \return the index of the bin in which the vote has been cast. + */ + int + vote (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id); + + /** \brief Vote for a given position in the 3D space. The weight is interpolated between the bin pointed by single_vote_coord and its neighbors. + * + * \param[in] single_vote_coord coordinates of the vote being cast. + * \param[in] weight weight associated with the vote. + * \param[in] voter_id the numeric id of the voter. Useful to trace back the voting correspondence, if the vote is returned by findMaxima as a part of a maximum of the Hough Space. + * \return the index of the bin in which the vote has been cast. + */ + int + voteInt (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id); + + /** \brief Find the bins with most votes. + * + * \param[in] min_threshold the minimum number of votes to be included in a bin in order to have its value returned. + * If set to a value between -1 and 0 the Hough space maximum_vote is found and the returned values are all the votes greater than -min_threshold * maximum_vote. + * \param[out] maxima_values the list of Hough Space bin values greater than min_threshold. + * \param[out] maxima_voter_ids for each value returned, a list of the voter ids who cast a vote in that position. + * \return The min_threshold used, either set by the user or found by this method. + */ + double + findMaxima (double min_threshold, std::vector & maxima_values, std::vector > &maxima_voter_ids); + + protected: + + /** \brief Minimum coordinate in the Hough Space. */ + Eigen::Vector3d min_coord_; + + /** \brief Size of each bin in the Hough Space. */ + Eigen::Vector3d bin_size_; + + /** \brief Number of bins for each dimension. */ + Eigen::Vector3i bin_count_; + + /** \brief Used to access hough_space_ as if it was a matrix. */ + int partial_bin_products_[4]; + + /** \brief Total number of bins in the Hough Space. */ + int total_bins_count_; + + /** \brief The Hough Space. */ + std::vector hough_space_; + + /** \brief List of voters for each bin. */ + std::unordered_map > voter_ids_; + }; + } + + /** \brief Class implementing a 3D correspondence grouping algorithm that can deal with multiple instances of a model template + * found into a given scene. Each correspondence casts a vote for a reference point in a 3D Hough Space. + * The remaining 3 DOF are taken into account by associating each correspondence with a local Reference Frame. + * The suggested PointModelRfT is pcl::ReferenceFrame + * + * \note If you use this code in any academic work, please cite the original paper: + * - F. Tombari, L. Di Stefano: + * Object recognition in 3D scenes with occlusions and clutter by Hough voting. + * 2010, Fourth Pacific-Rim Symposium on Image and Video Technology + * + * \author Federico Tombari (original), Tommaso Cavallari (PCL port) + * \ingroup recognition + */ + template + class Hough3DGrouping : public CorrespondenceGrouping + { + public: + using ModelRfCloud = pcl::PointCloud; + using ModelRfCloudPtr = typename ModelRfCloud::Ptr; + using ModelRfCloudConstPtr = typename ModelRfCloud::ConstPtr; + + using SceneRfCloud = pcl::PointCloud; + using SceneRfCloudPtr = typename SceneRfCloud::Ptr; + using SceneRfCloudConstPtr = typename SceneRfCloud::ConstPtr; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using SceneCloudConstPtr = typename pcl::CorrespondenceGrouping::SceneCloudConstPtr; + + /** \brief Constructor */ + Hough3DGrouping () + : input_rf_ () + , scene_rf_ () + , needs_training_ (true) + ,hough_threshold_ (-1) + , hough_bin_size_ (1.0) + , use_interpolation_ (true) + , use_distance_weight_ (false) + , local_rf_normals_search_radius_ (0.0f) + , local_rf_search_radius_ (0.0f) + , hough_space_initialized_ (false) + {} + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const boost shared pointer to a PointCloud message. + */ + inline void + setInputCloud (const PointCloudConstPtr &cloud) override + { + PCLBase::setInputCloud (cloud); + needs_training_ = true; + hough_space_initialized_ = false; + input_rf_.reset(); + } + + /** \brief Provide a pointer to the input dataset's reference frames. + * Each point in the reference frame cloud should be the reference frame of + * the correspondent point in the input dataset. + * + * \param[in] input_rf the pointer to the input cloud's reference frames. + */ + inline void + setInputRf (const ModelRfCloudConstPtr &input_rf) + { + input_rf_ = input_rf; + needs_training_ = true; + hough_space_initialized_ = false; + } + + /** \brief Getter for the input dataset's reference frames. + * Each point in the reference frame cloud should be the reference frame of + * the correspondent point in the input dataset. + * + * \return the pointer to the input cloud's reference frames. + */ + inline ModelRfCloudConstPtr + getInputRf () const + { + return (input_rf_); + } + + /** \brief Provide a pointer to the scene dataset (i.e. the cloud in which the algorithm has to search for instances of the input model) + * + * \param[in] scene the const boost shared pointer to a PointCloud message. + */ + inline void + setSceneCloud (const SceneCloudConstPtr &scene) override + { + scene_ = scene; + hough_space_initialized_ = false; + scene_rf_.reset(); + } + + /** \brief Provide a pointer to the scene dataset's reference frames. + * Each point in the reference frame cloud should be the reference frame of + * the correspondent point in the scene dataset. + * + * \param[in] scene_rf the pointer to the scene cloud's reference frames. + */ + inline void + setSceneRf (const SceneRfCloudConstPtr &scene_rf) + { + scene_rf_ = scene_rf; + hough_space_initialized_ = false; + } + + /** \brief Getter for the scene dataset's reference frames. + * Each point in the reference frame cloud should be the reference frame of + * the correspondent point in the scene dataset. + * + * \return the pointer to the scene cloud's reference frames. + */ + inline SceneRfCloudConstPtr + getSceneRf () const + { + return (scene_rf_); + } + + /** \brief Provide a pointer to the precomputed correspondences between points in the input dataset and + * points in the scene dataset. The correspondences are going to be clustered into different model instances + * by the algorithm. + * + * \param[in] corrs the correspondences between the model and the scene. + */ + inline void + setModelSceneCorrespondences (const CorrespondencesConstPtr &corrs) override + { + model_scene_corrs_ = corrs; + hough_space_initialized_ = false; + } + + /** \brief Sets the minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud. + * + * \param[in] threshold the threshold for the Hough space voting, if set between -1 and 0 the maximum vote in the + * entire space is automatically calculated and -threshold the maximum value is used as a threshold. This means + * that a value between -1 and 0 should be used only if at least one instance of the model is always present in + * the scene, or if this false positive can be filtered later. + */ + inline void + setHoughThreshold (double threshold) + { + hough_threshold_ = threshold; + } + + /** \brief Gets the minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud. + * + * \return the threshold for the Hough space voting. + */ + inline double + getHoughThreshold () const + { + return (hough_threshold_); + } + + /** \brief Sets the size of each bin into the Hough space. + * + * \param[in] bin_size the size of each Hough space's bin. + */ + inline void + setHoughBinSize (double bin_size) + { + hough_bin_size_ = bin_size; + hough_space_initialized_ = false; + } + + /** \brief Gets the size of each bin into the Hough space. + * + * \return the size of each Hough space's bin. + */ + inline double + getHoughBinSize () const + { + return (hough_bin_size_); + } + + /** \brief Sets whether the vote casting procedure interpolates + * the score between neighboring bins of the Hough space or not. + * + * \param[in] use_interpolation the algorithm should interpolate the vote score between neighboring bins. + */ + inline void + setUseInterpolation (bool use_interpolation) + { + use_interpolation_ = use_interpolation; + hough_space_initialized_ = false; + } + + /** \brief Gets whether the vote casting procedure interpolates + * the score between neighboring bins of the Hough space or not. + * + * \return if the algorithm should interpolate the vote score between neighboring bins. + */ + inline bool + getUseInterpolation () const + { + return (use_interpolation_); + } + + /** \brief Sets whether the vote casting procedure uses the correspondence's distance as a score. + * + * \param[in] use_distance_weight the algorithm should use the weighted distance when calculating the Hough voting score. + */ + inline void + setUseDistanceWeight (bool use_distance_weight) + { + use_distance_weight_ = use_distance_weight; + hough_space_initialized_ = false; + } + + /** \brief Gets whether the vote casting procedure uses the correspondence's distance as a score. + * + * \return if the algorithm should use the weighted distance when calculating the Hough voting score. + */ + inline bool + getUseDistanceWeight () const + { + return (use_distance_weight_); + } + + /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud, + * this algorithm makes the computation itself but needs a suitable search radius to compute the normals + * in order to subsequently compute the RF (if not set a default 15 nearest neighbors search is performed). + * + * \param[in] local_rf_normals_search_radius the normals search radius for the local reference frame calculation. + */ + inline void + setLocalRfNormalsSearchRadius (float local_rf_normals_search_radius) + { + local_rf_normals_search_radius_ = local_rf_normals_search_radius; + needs_training_ = true; + hough_space_initialized_ = false; + } + + /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud, + * this algorithm makes the computation itself but needs a suitable search radius to compute the normals + * in order to subsequently compute the RF (if not set a default 15 nearest neighbors search is performed). + * + * \return the normals search radius for the local reference frame calculation. + */ + inline float + getLocalRfNormalsSearchRadius () const + { + return (local_rf_normals_search_radius_); + } + + /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud, + * this algorithm makes the computation itself but needs a suitable search radius to do so. + * \attention This parameter NEEDS to be set if the reference frames are not precomputed externally, + * otherwise the recognition results won't be correct. + * + * \param[in] local_rf_search_radius the search radius for the local reference frame calculation. + */ + inline void + setLocalRfSearchRadius (float local_rf_search_radius) + { + local_rf_search_radius_ = local_rf_search_radius; + needs_training_ = true; + hough_space_initialized_ = false; + } + + /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud, + * this algorithm makes the computation itself but needs a suitable search radius to do so. + * \attention This parameter NEEDS to be set if the reference frames are not precomputed externally, + * otherwise the recognition results won't be correct. + * + * \return the search radius for the local reference frame calculation. + */ + inline float + getLocalRfSearchRadius () const + { + return (local_rf_search_radius_); + } + + /** \brief Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform an off line training of the algorithm. This might be useful if one wants to perform once and for all a pre-computation of votes that only concern the models, increasing the on-line efficiency of the grouping algorithm. + * The algorithm is automatically trained on the first invocation of the recognize method or the cluster method if this training function has not been manually invoked. + * + * \return true if the training had been successful or false if errors have occurred. + */ + bool + train (); + + /** \brief The main function, recognizes instances of the model into the scene set by the user. + * + * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene. + * + * \return true if the recognition had been successful or false if errors have occurred. + */ + bool + recognize (std::vector > &transformations); + + /** \brief The main function, recognizes instances of the model into the scene set by the user. + * + * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene. + * \param[out] clustered_corrs a vector containing the correspondences for each instance of the model found within the input data (the same output of clusterCorrespondences). + * + * \return true if the recognition had been successful or false if errors have occurred. + */ + bool + recognize (std::vector > &transformations, std::vector &clustered_corrs); + + protected: + using CorrespondenceGrouping::input_; + using CorrespondenceGrouping::scene_; + using CorrespondenceGrouping::model_scene_corrs_; + + /** \brief The input Rf cloud. */ + ModelRfCloudConstPtr input_rf_; + + /** \brief The scene Rf cloud. */ + SceneRfCloudConstPtr scene_rf_; + + /** \brief If the training of the Hough space is needed; set on change of either the input cloud or the input_rf. */ + bool needs_training_; + + /** \brief The result of the training. The vector between each model point and the centroid of the model adjusted by its local reference frame.*/ + std::vector > model_votes_; + + /** \brief The minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud. */ + double hough_threshold_; + + /** \brief The size of each bin of the hough space. */ + double hough_bin_size_; + + /** \brief Use the interpolation between neighboring Hough bins when casting votes. */ + bool use_interpolation_; + + /** \brief Use the weighted correspondence distance when casting votes. */ + bool use_distance_weight_; + + /** \brief Normals search radius for the potential Rf calculation. */ + float local_rf_normals_search_radius_; + + /** \brief Search radius for the potential Rf calculation. */ + float local_rf_search_radius_; + + /** \brief The Hough space. */ + pcl::recognition::HoughSpace3D::Ptr hough_space_; + + /** \brief Transformations found by clusterCorrespondences method. */ + std::vector > found_transformations_; + + /** \brief Whether the Hough space already contains the correct votes for the current input parameters and so the cluster and recognize calls don't need to recompute each value. + * Reset on the change of any parameter except the hough_threshold. + */ + bool hough_space_initialized_; + + /** \brief Cluster the input correspondences in order to distinguish between different instances of the model into the scene. + * + * \param[out] model_instances a vector containing the clustered correspondences for each model found on the scene. + * \return true if the clustering had been successful or false if errors have occurred. + */ + void + clusterCorrespondences (std::vector &model_instances) override; + + /* \brief Finds the transformation matrix between the input and the scene cloud for a set of correspondences using a RANSAC algorithm. + * \param[in] the scene cloud in which the PointSceneT has been converted to PointModelT. + * \param[in] corrs a set of correspondences. + * \param[out] transform the transformation matrix between the input cloud and the scene cloud that aligns the found correspondences. + * \return true if the recognition had been successful or false if errors have occurred. + */ + //bool + //getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform); + + /** \brief The Hough space voting procedure. + * \return true if the voting had been successful or false if errors have occurred. + */ + bool + houghVoting (); + + /** \brief Computes the reference frame for an input cloud. + * \param[in] input the input cloud. + * \param[out] rf the resulting reference frame. + */ + template void + computeRf (const typename pcl::PointCloud::ConstPtr &input, pcl::PointCloud &rf); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/color_gradient_dot_modality.h b/deps_install/include/pcl-1.10/pcl/recognition/color_gradient_dot_modality.h new file mode 100755 index 0000000..6bedd2b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/color_gradient_dot_modality.h @@ -0,0 +1,462 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + + +namespace pcl +{ + template + class ColorGradientDOTModality + : public DOTModality, public PCLBase + { + protected: + using PCLBase::input_; + + struct Candidate + { + GradientXY gradient; + + int x; + int y; + + bool operator< (const Candidate & rhs) + { + return (gradient.magnitude > rhs.gradient.magnitude); + } + }; + + public: + using PointCloudIn = pcl::PointCloud; + + ColorGradientDOTModality (std::size_t bin_size); + + virtual ~ColorGradientDOTModality (); + + inline void + setGradientMagnitudeThreshold (const float threshold) + { + gradient_magnitude_threshold_ = threshold; + } + + //inline QuantizedMap & + //getDominantQuantizedMap () + //{ + // return (dominant_quantized_color_gradients_); + //} + + inline QuantizedMap & + getDominantQuantizedMap () + { + return (dominant_quantized_color_gradients_); + } + + QuantizedMap + computeInvariantQuantizedMap (const MaskMap & mask, + const RegionXY & region); + + /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) + * \param cloud the const boost shared pointer to a PointCloud message + */ + virtual void + setInputCloud (const typename PointCloudIn::ConstPtr & cloud) + { + input_ = cloud; + //processInputData (); + } + + virtual void + processInputData (); + + protected: + + void + computeMaxColorGradients (); + + void + computeDominantQuantizedGradients (); + + //void + //computeInvariantQuantizedGradients (); + + private: + std::size_t bin_size_; + + float gradient_magnitude_threshold_; + pcl::PointCloud color_gradients_; + + pcl::QuantizedMap dominant_quantized_color_gradients_; + //pcl::QuantizedMap invariant_quantized_color_gradients_; + + }; + +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ColorGradientDOTModality:: +ColorGradientDOTModality (const std::size_t bin_size) + : bin_size_ (bin_size), gradient_magnitude_threshold_ (80.0f), color_gradients_ (), dominant_quantized_color_gradients_ () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ColorGradientDOTModality:: +~ColorGradientDOTModality () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientDOTModality:: +processInputData () +{ + // extract color gradients + computeMaxColorGradients (); + + // compute dominant quantized gradient map + computeDominantQuantizedGradients (); + + // compute invariant quantized gradient map + //computeInvariantQuantizedGradients (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientDOTModality:: +computeMaxColorGradients () +{ + const int width = input_->width; + const int height = input_->height; + + color_gradients_.points.resize (width*height); + color_gradients_.width = width; + color_gradients_.height = height; + + constexpr float pi = std::tan(1.0f)*4; + for (int row_index = 0; row_index < height-2; ++row_index) + { + for (int col_index = 0; col_index < width-2; ++col_index) + { + const int index0 = row_index*width+col_index; + const int index_c = row_index*width+col_index+2; + const int index_r = (row_index+2)*width+col_index; + + const unsigned char r0 = input_->points[index0].r; + const unsigned char g0 = input_->points[index0].g; + const unsigned char b0 = input_->points[index0].b; + + const unsigned char r_c = input_->points[index_c].r; + const unsigned char g_c = input_->points[index_c].g; + const unsigned char b_c = input_->points[index_c].b; + + const unsigned char r_r = input_->points[index_r].r; + const unsigned char g_r = input_->points[index_r].g; + const unsigned char b_r = input_->points[index_r].b; + + const float r_dx = static_cast (r_c) - static_cast (r0); + const float g_dx = static_cast (g_c) - static_cast (g0); + const float b_dx = static_cast (b_c) - static_cast (b0); + + const float r_dy = static_cast (r_r) - static_cast (r0); + const float g_dy = static_cast (g_r) - static_cast (g0); + const float b_dy = static_cast (b_r) - static_cast (b0); + + const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy; + const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy; + const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy; + + GradientXY gradient; + gradient.x = col_index; + gradient.y = row_index; + if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b) + { + gradient.magnitude = sqrt (sqr_mag_r); + gradient.angle = std::atan2 (r_dy, r_dx) * 180.0f / pi; + } + else if (sqr_mag_g > sqr_mag_b) + { + gradient.magnitude = sqrt (sqr_mag_g); + gradient.angle = std::atan2 (g_dy, g_dx) * 180.0f / pi; + } + else + { + gradient.magnitude = sqrt (sqr_mag_b); + gradient.angle = std::atan2 (b_dy, b_dx) * 180.0f / pi; + } + + assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 && + color_gradients_ (col_index+1, row_index+1).angle <= 180); + + color_gradients_ (col_index+1, row_index+1) = gradient; + } + } + + return; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientDOTModality:: +computeDominantQuantizedGradients () +{ + const std::size_t input_width = input_->width; + const std::size_t input_height = input_->height; + + const std::size_t output_width = input_width / bin_size_; + const std::size_t output_height = input_height / bin_size_; + + dominant_quantized_color_gradients_.resize (output_width, output_height); + + constexpr std::size_t num_gradient_bins = 7; + + constexpr float divisor = 180.0f / (num_gradient_bins - 1.0f); + + unsigned char * peak_pointer = dominant_quantized_color_gradients_.getData (); + memset (peak_pointer, 0, output_width*output_height); + + for (std::size_t row_bin_index = 0; row_bin_index < output_height; ++row_bin_index) + { + for (std::size_t col_bin_index = 0; col_bin_index < output_width; ++col_bin_index) + { + const std::size_t x_position = col_bin_index * bin_size_; + const std::size_t y_position = row_bin_index * bin_size_; + + float max_gradient = 0.0f; + std::size_t max_gradient_pos_x = 0; + std::size_t max_gradient_pos_y = 0; + + // find next location and value of maximum gradient magnitude in current region + for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index) + { + for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index) + { + const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; + + if (magnitude > max_gradient) + { + max_gradient = magnitude; + max_gradient_pos_x = col_sub_index; + max_gradient_pos_y = row_sub_index; + } + } + } + + if (max_gradient >= gradient_magnitude_threshold_) + { + const std::size_t angle = static_cast (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f); + const std::size_t bin_index = static_cast ((angle >= 180 ? angle-180 : angle)/divisor); + + *peak_pointer |= 1 << bin_index; + } + + if (*peak_pointer == 0) + { + *peak_pointer |= 1 << 7; + } + + ++peak_pointer; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::QuantizedMap +pcl::ColorGradientDOTModality:: +computeInvariantQuantizedMap (const MaskMap & mask, + const RegionXY & region) +{ + const std::size_t input_width = input_->width; + const std::size_t input_height = input_->height; + + const std::size_t output_width = input_width / bin_size_; + const std::size_t output_height = input_height / bin_size_; + + const std::size_t sub_start_x = region.x / bin_size_; + const std::size_t sub_start_y = region.y / bin_size_; + const std::size_t sub_width = region.width / bin_size_; + const std::size_t sub_height = region.height / bin_size_; + + QuantizedMap map; + map.resize (sub_width, sub_height); + + constexpr std::size_t num_gradient_bins = 7; + constexpr std::size_t max_num_of_gradients = 7; + + const float divisor = 180.0f / (num_gradient_bins - 1.0f); + + float global_max_gradient = 0.0f; + float local_max_gradient = 0.0f; + + unsigned char * peak_pointer = map.getData (); + + for (std::size_t row_bin_index = 0; row_bin_index < sub_height; ++row_bin_index) + { + for (std::size_t col_bin_index = 0; col_bin_index < sub_width; ++col_bin_index) + { + std::vector x_coordinates; + std::vector y_coordinates; + std::vector values; + + for (int row_pixel_index = -static_cast (bin_size_)/2; + row_pixel_index <= static_cast (bin_size_)/2; + row_pixel_index += static_cast (bin_size_)/2) + { + const std::size_t y_position = row_pixel_index + (sub_start_y + row_bin_index)*bin_size_; + + if (y_position < 0 || y_position >= input_height) + continue; + + for (int col_pixel_index = -static_cast (bin_size_)/2; + col_pixel_index <= static_cast (bin_size_)/2; + col_pixel_index += static_cast (bin_size_)/2) + { + const std::size_t x_position = col_pixel_index + (sub_start_x + col_bin_index)*bin_size_; + std::size_t counter = 0; + + if (x_position < 0 || x_position >= input_width) + continue; + + // find maximum gradient magnitude in current bin + { + local_max_gradient = 0.0f; + for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index) + { + for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index) + { + const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; + + if (magnitude > local_max_gradient) + local_max_gradient = magnitude; + } + } + } + + if (local_max_gradient > global_max_gradient) + { + global_max_gradient = local_max_gradient; + } + + // iteratively search for the largest gradients, set it to -1, search the next largest ... etc. + while (true) + { + float max_gradient; + std::size_t max_gradient_pos_x; + std::size_t max_gradient_pos_y; + + // find next location and value of maximum gradient magnitude in current region + { + max_gradient = 0.0f; + for (std::size_t row_sub_index = 0; row_sub_index < bin_size_; ++row_sub_index) + { + for (std::size_t col_sub_index = 0; col_sub_index < bin_size_; ++col_sub_index) + { + const float magnitude = color_gradients_ (col_sub_index + x_position, row_sub_index + y_position).magnitude; + + if (magnitude > max_gradient) + { + max_gradient = magnitude; + max_gradient_pos_x = col_sub_index; + max_gradient_pos_y = row_sub_index; + } + } + } + } + + // TODO: really localMaxGradient and not maxGradient??? + if (local_max_gradient < gradient_magnitude_threshold_) + { + //*peakPointer |= 1 << (numOfGradientBins-1); + break; + } + + // TODO: replace gradient_magnitude_threshold_ here by a fixed ratio? + if (/*max_gradient < (local_max_gradient * gradient_magnitude_threshold_) ||*/ + counter >= max_num_of_gradients) + { + break; + } + + ++counter; + + const std::size_t angle = static_cast (180 + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).angle + 0.5f); + const std::size_t bin_index = static_cast ((angle >= 180 ? angle-180 : angle)/divisor); + + *peak_pointer |= 1 << bin_index; + + x_coordinates.push_back (max_gradient_pos_x + x_position); + y_coordinates.push_back (max_gradient_pos_y + y_position); + values.push_back (max_gradient); + + color_gradients_ (max_gradient_pos_x + x_position, max_gradient_pos_y + y_position).magnitude = -1.0f; + } + + // reset values which have been set to -1 + for (std::size_t value_index = 0; value_index < values.size (); ++value_index) + { + color_gradients_ (x_coordinates[value_index], y_coordinates[value_index]).magnitude = values[value_index]; + } + + x_coordinates.clear (); + y_coordinates.clear (); + values.clear (); + } + } + + if (*peak_pointer == 0) + { + *peak_pointer |= 1 << 7; + } + ++peak_pointer; + } + } + + return map; +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/color_gradient_modality.h b/deps_install/include/pcl-1.10/pcl/recognition/color_gradient_modality.h new file mode 100755 index 0000000..d1aacb3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/color_gradient_modality.h @@ -0,0 +1,1115 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include + +namespace pcl +{ + + /** \brief Modality based on max-RGB gradients. + * \author Stefan Holzer + */ + template + class ColorGradientModality + : public QuantizableModality, public PCLBase + { + protected: + using PCLBase::input_; + + /** \brief Candidate for a feature (used in feature extraction methods). */ + struct Candidate + { + /** \brief The gradient. */ + GradientXY gradient; + + /** \brief The x-position. */ + int x; + /** \brief The y-position. */ + int y; + + /** \brief Operator for comparing to candidates (by magnitude of the gradient). + * \param[in] rhs the candidate to compare with. + */ + bool operator< (const Candidate & rhs) const + { + return (gradient.magnitude > rhs.gradient.magnitude); + } + }; + + public: + using PointCloudIn = pcl::PointCloud; + + /** \brief Different methods for feature selection/extraction. */ + enum FeatureSelectionMethod + { + MASK_BORDER_HIGH_GRADIENTS, + MASK_BORDER_EQUALLY, // this gives templates most equally to the OpenCV implementation + DISTANCE_MAGNITUDE_SCORE + }; + + /** \brief Constructor. */ + ColorGradientModality (); + /** \brief Destructor. */ + ~ColorGradientModality (); + + /** \brief Sets the threshold for the gradient magnitude which is used when quantizing the data. + * Gradients with a smaller magnitude are ignored. + * \param[in] threshold the new gradient magnitude threshold. + */ + inline void + setGradientMagnitudeThreshold (const float threshold) + { + gradient_magnitude_threshold_ = threshold; + } + + /** \brief Sets the threshold for the gradient magnitude which is used for feature extraction. + * Gradients with a smaller magnitude are ignored. + * \param[in] threshold the new gradient magnitude threshold. + */ + inline void + setGradientMagnitudeThresholdForFeatureExtraction (const float threshold) + { + gradient_magnitude_threshold_feature_extraction_ = threshold; + } + + /** \brief Sets the feature selection method. + * \param[in] method the feature selection method. + */ + inline void + setFeatureSelectionMethod (const FeatureSelectionMethod method) + { + feature_selection_method_ = method; + } + + /** \brief Sets the spreading size for spreading the quantized data. */ + inline void + setSpreadingSize (const std::size_t spreading_size) + { + spreading_size_ = spreading_size; + } + + /** \brief Sets whether variable feature numbers for feature extraction is enabled. + * \param[in] enabled enables/disables variable feature numbers for feature extraction. + */ + inline void + setVariableFeatureNr (const bool enabled) + { + variable_feature_nr_ = enabled; + } + + /** \brief Returns a reference to the internally computed quantized map. */ + inline QuantizedMap & + getQuantizedMap () override + { + return (filtered_quantized_color_gradients_); + } + + /** \brief Returns a reference to the internally computed spread quantized map. */ + inline QuantizedMap & + getSpreadedQuantizedMap () override + { + return (spreaded_filtered_quantized_color_gradients_); + } + + /** \brief Returns a point cloud containing the max-RGB gradients. */ + inline pcl::PointCloud & + getMaxColorGradients () + { + return (color_gradients_); + } + + /** \brief Extracts features from this modality within the specified mask. + * \param[in] mask defines the areas where features are searched in. + * \param[in] nr_features defines the number of features to be extracted + * (might be less if not sufficient information is present in the modality). + * \param[in] modalityIndex the index which is stored in the extracted features. + * \param[out] features the destination for the extracted features. + */ + void + extractFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modalityIndex, + std::vector & features) const override; + + /** \brief Extracts all possible features from the modality within the specified mask. + * \param[in] mask defines the areas where features are searched in. + * \param[in] nr_features IGNORED (TODO: remove this parameter). + * \param[in] modalityIndex the index which is stored in the extracted features. + * \param[out] features the destination for the extracted features. + */ + void + extractAllFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modalityIndex, + std::vector & features) const override; + + /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) + * \param cloud the const boost shared pointer to a PointCloud message + */ + void + setInputCloud (const typename PointCloudIn::ConstPtr & cloud) override + { + input_ = cloud; + } + + /** \brief Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading). */ + virtual void + processInputData (); + + /** \brief Processes the input data assuming that everything up to filtering is already done/available + * (so only spreading is performed). */ + virtual void + processInputDataFromFiltered (); + + protected: + + /** \brief Computes the Gaussian kernel used for smoothing. + * \param[in] kernel_size the size of the Gaussian kernel. + * \param[in] sigma the sigma. + * \param[out] kernel_values the destination for the values of the kernel. */ + void + computeGaussianKernel (const std::size_t kernel_size, const float sigma, std::vector & kernel_values); + + /** \brief Computes the max-RGB gradients for the specified cloud. + * \param[in] cloud the cloud for which the gradients are computed. + */ + void + computeMaxColorGradients (const typename pcl::PointCloud::ConstPtr & cloud); + + /** \brief Computes the max-RGB gradients for the specified cloud using sobel. + * \param[in] cloud the cloud for which the gradients are computed. + */ + void + computeMaxColorGradientsSobel (const typename pcl::PointCloud::ConstPtr & cloud); + + /** \brief Quantizes the color gradients. */ + void + quantizeColorGradients (); + + /** \brief Filters the quantized gradients. */ + void + filterQuantizedColorGradients (); + + /** \brief Erodes a mask. + * \param[in] mask_in the mask which will be eroded. + * \param[out] mask_out the destination for the eroded mask. + */ + static void + erode (const pcl::MaskMap & mask_in, pcl::MaskMap & mask_out); + + private: + + /** \brief Determines whether variable numbers of features are extracted or not. */ + bool variable_feature_nr_; + + /** \brief Stores a smoothed version of the input cloud. */ + pcl::PointCloud::Ptr smoothed_input_; + + /** \brief Defines which feature selection method is used. */ + FeatureSelectionMethod feature_selection_method_; + + /** \brief The threshold applied on the gradient magnitudes (for quantization). */ + float gradient_magnitude_threshold_; + /** \brief The threshold applied on the gradient magnitudes for feature extraction. */ + float gradient_magnitude_threshold_feature_extraction_; + + /** \brief The point cloud which holds the max-RGB gradients. */ + pcl::PointCloud color_gradients_; + + /** \brief The spreading size. */ + std::size_t spreading_size_; + + /** \brief The map which holds the quantized max-RGB gradients. */ + pcl::QuantizedMap quantized_color_gradients_; + /** \brief The map which holds the filtered quantized data. */ + pcl::QuantizedMap filtered_quantized_color_gradients_; + /** \brief The map which holds the spread quantized data. */ + pcl::QuantizedMap spreaded_filtered_quantized_color_gradients_; + + }; + +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ColorGradientModality:: +ColorGradientModality () + : variable_feature_nr_ (false) + , smoothed_input_ (new pcl::PointCloud ()) + , feature_selection_method_ (DISTANCE_MAGNITUDE_SCORE) + , gradient_magnitude_threshold_ (10.0f) + , gradient_magnitude_threshold_feature_extraction_ (55.0f) + , spreading_size_ (8) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ColorGradientModality:: +~ColorGradientModality () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ColorGradientModality:: +computeGaussianKernel (const std::size_t kernel_size, const float sigma, std::vector & kernel_values) +{ + // code taken from OpenCV + const int n = int (kernel_size); + const int SMALL_GAUSSIAN_SIZE = 7; + static const float small_gaussian_tab[][SMALL_GAUSSIAN_SIZE] = + { + {1.f}, + {0.25f, 0.5f, 0.25f}, + {0.0625f, 0.25f, 0.375f, 0.25f, 0.0625f}, + {0.03125f, 0.109375f, 0.21875f, 0.28125f, 0.21875f, 0.109375f, 0.03125f} + }; + + const float* fixed_kernel = n % 2 == 1 && n <= SMALL_GAUSSIAN_SIZE && sigma <= 0 ? + small_gaussian_tab[n>>1] : nullptr; + + //CV_Assert( ktype == CV_32F || ktype == CV_64F ); + /*Mat kernel(n, 1, ktype);*/ + kernel_values.resize (n); + float* cf = &(kernel_values[0]); + //double* cd = (double*)kernel.data; + + double sigmaX = sigma > 0 ? sigma : ((n-1)*0.5 - 1)*0.3 + 0.8; + double scale2X = -0.5/(sigmaX*sigmaX); + double sum = 0; + + for( int i = 0; i < n; i++ ) + { + double x = i - (n-1)*0.5; + double t = fixed_kernel ? double (fixed_kernel[i]) : std::exp (scale2X*x*x); + + cf[i] = float (t); + sum += cf[i]; + } + + sum = 1./sum; + for ( int i = 0; i < n; i++ ) + { + cf[i] = float (cf[i]*sum); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientModality:: +processInputData () +{ + // compute gaussian kernel values + const std::size_t kernel_size = 7; + std::vector kernel_values; + computeGaussianKernel (kernel_size, 0.0f, kernel_values); + + // smooth input + pcl::filters::Convolution convolution; + Eigen::ArrayXf gaussian_kernel(kernel_size); + //gaussian_kernel << 1.f/16, 1.f/8, 3.f/16, 2.f/8, 3.f/16, 1.f/8, 1.f/16; + //gaussian_kernel << 16.f/1600.f, 32.f/1600.f, 64.f/1600.f, 128.f/1600.f, 256.f/1600.f, 128.f/1600.f, 64.f/1600.f, 32.f/1600.f, 16.f/1600.f; + gaussian_kernel << kernel_values[0], kernel_values[1], kernel_values[2], kernel_values[3], kernel_values[4], kernel_values[5], kernel_values[6]; + + pcl::PointCloud::Ptr rgb_input_ (new pcl::PointCloud()); + + const std::uint32_t width = input_->width; + const std::uint32_t height = input_->height; + + rgb_input_->resize (width*height); + rgb_input_->width = width; + rgb_input_->height = height; + rgb_input_->is_dense = input_->is_dense; + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + (*rgb_input_) (col_index, row_index).r = (*input_) (col_index, row_index).r; + (*rgb_input_) (col_index, row_index).g = (*input_) (col_index, row_index).g; + (*rgb_input_) (col_index, row_index).b = (*input_) (col_index, row_index).b; + } + } + + convolution.setInputCloud (rgb_input_); + convolution.setKernel (gaussian_kernel); + + convolution.convolve (*smoothed_input_); + + // extract color gradients + computeMaxColorGradientsSobel (smoothed_input_); + + // quantize gradients + quantizeColorGradients (); + + // filter quantized gradients to get only dominants one + thresholding + filterQuantizedColorGradients (); + + // spread filtered quantized gradients + //spreadFilteredQunatizedColorGradients (); + pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_, + spreaded_filtered_quantized_color_gradients_, + spreading_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientModality:: +processInputDataFromFiltered () +{ + // spread filtered quantized gradients + //spreadFilteredQunatizedColorGradients (); + pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_color_gradients_, + spreaded_filtered_quantized_color_gradients_, + spreading_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::ColorGradientModality:: +extractFeatures (const MaskMap & mask, const std::size_t nr_features, const std::size_t modality_index, + std::vector & features) const +{ + const std::size_t width = mask.getWidth (); + const std::size_t height = mask.getHeight (); + + std::list list1; + std::list list2; + + + if (feature_selection_method_ == DISTANCE_MAGNITUDE_SCORE) + { + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + const GradientXY & gradient = color_gradients_ (col_index, row_index); + if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_ + && filtered_quantized_color_gradients_ (col_index, row_index) != 0) + { + Candidate candidate; + candidate.gradient = gradient; + candidate.x = static_cast (col_index); + candidate.y = static_cast (row_index); + + list1.push_back (candidate); + } + } + } + } + + list1.sort(); + + if (variable_feature_nr_) + { + list2.push_back (*(list1.begin ())); + //while (list2.size () != nr_features) + bool feature_selection_finished = false; + while (!feature_selection_finished) + { + float best_score = 0.0f; + typename std::list::iterator best_iter = list1.end (); + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + // find smallest distance + float smallest_distance = std::numeric_limits::max (); + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + const float dx = static_cast (iter1->x) - static_cast (iter2->x); + const float dy = static_cast (iter1->y) - static_cast (iter2->y); + + const float distance = dx*dx + dy*dy; + + if (distance < smallest_distance) + { + smallest_distance = distance; + } + } + + const float score = smallest_distance * iter1->gradient.magnitude; + + if (score > best_score) + { + best_score = score; + best_iter = iter1; + } + } + + + float min_min_sqr_distance = std::numeric_limits::max (); + float max_min_sqr_distance = 0; + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + float min_sqr_distance = std::numeric_limits::max (); + for (typename std::list::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3) + { + if (iter2 == iter3) + continue; + + const float dx = static_cast (iter2->x) - static_cast (iter3->x); + const float dy = static_cast (iter2->y) - static_cast (iter3->y); + + const float sqr_distance = dx*dx + dy*dy; + + if (sqr_distance < min_sqr_distance) + { + min_sqr_distance = sqr_distance; + } + + //std::cerr << min_sqr_distance; + } + //std::cerr << std::endl; + + // check current feature + { + const float dx = static_cast (iter2->x) - static_cast (best_iter->x); + const float dy = static_cast (iter2->y) - static_cast (best_iter->y); + + const float sqr_distance = dx*dx + dy*dy; + + if (sqr_distance < min_sqr_distance) + { + min_sqr_distance = sqr_distance; + } + } + + if (min_sqr_distance < min_min_sqr_distance) + min_min_sqr_distance = min_sqr_distance; + if (min_sqr_distance > max_min_sqr_distance) + max_min_sqr_distance = min_sqr_distance; + + //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl; + } + + if (best_iter != list1.end ()) + { + //std::cerr << "feature_index: " << list2.size () << std::endl; + //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl; + //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl; + + if (min_min_sqr_distance < 50) + { + feature_selection_finished = true; + break; + } + + list2.push_back (*best_iter); + } + } + } + else + { + if (list1.size () <= nr_features) + { + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + QuantizedMultiModFeature feature; + + feature.x = iter1->x; + feature.y = iter1->y; + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y); + + features.push_back (feature); + } + return; + } + + list2.push_back (*(list1.begin ())); + while (list2.size () != nr_features) + { + float best_score = 0.0f; + typename std::list::iterator best_iter = list1.end (); + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + // find smallest distance + float smallest_distance = std::numeric_limits::max (); + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + const float dx = static_cast (iter1->x) - static_cast (iter2->x); + const float dy = static_cast (iter1->y) - static_cast (iter2->y); + + const float distance = dx*dx + dy*dy; + + if (distance < smallest_distance) + { + smallest_distance = distance; + } + } + + const float score = smallest_distance * iter1->gradient.magnitude; + + if (score > best_score) + { + best_score = score; + best_iter = iter1; + } + } + + if (best_iter != list1.end ()) + { + list2.push_back (*best_iter); + } + else + { + break; + } + } + } + } + else if (feature_selection_method_ == MASK_BORDER_HIGH_GRADIENTS || feature_selection_method_ == MASK_BORDER_EQUALLY) + { + MaskMap eroded_mask; + erode (mask, eroded_mask); + + auto diff_mask = MaskMap::getDifferenceMask (mask, eroded_mask); + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + if (diff_mask (col_index, row_index) != 0) + { + const GradientXY & gradient = color_gradients_ (col_index, row_index); + if ((feature_selection_method_ == MASK_BORDER_EQUALLY || gradient.magnitude > gradient_magnitude_threshold_feature_extraction_) + && filtered_quantized_color_gradients_ (col_index, row_index) != 0) + { + Candidate candidate; + candidate.gradient = gradient; + candidate.x = static_cast (col_index); + candidate.y = static_cast (row_index); + + list1.push_back (candidate); + } + } + } + } + + list1.sort(); + + if (list1.size () <= nr_features) + { + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + QuantizedMultiModFeature feature; + + feature.x = iter1->x; + feature.y = iter1->y; + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y); + + features.push_back (feature); + } + return; + } + + std::size_t distance = list1.size () / nr_features + 1; // ??? + while (list2.size () != nr_features) + { + const std::size_t sqr_distance = distance*distance; + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + bool candidate_accepted = true; + + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + const int dx = iter1->x - iter2->x; + const int dy = iter1->y - iter2->y; + const unsigned int tmp_distance = dx*dx + dy*dy; + + //if (tmp_distance < distance) + if (tmp_distance < sqr_distance) + { + candidate_accepted = false; + break; + } + } + + if (candidate_accepted) + list2.push_back (*iter1); + + if (list2.size () == nr_features) + break; + } + --distance; + } + } + + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + QuantizedMultiModFeature feature; + + feature.x = iter2->x; + feature.y = iter2->y; + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_color_gradients_ (iter2->x, iter2->y); + + features.push_back (feature); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ColorGradientModality:: +extractAllFeatures (const MaskMap & mask, const std::size_t, const std::size_t modality_index, + std::vector & features) const +{ + const std::size_t width = mask.getWidth (); + const std::size_t height = mask.getHeight (); + + std::list list1; + std::list list2; + + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + const GradientXY & gradient = color_gradients_ (col_index, row_index); + if (gradient.magnitude > gradient_magnitude_threshold_feature_extraction_ + && filtered_quantized_color_gradients_ (col_index, row_index) != 0) + { + Candidate candidate; + candidate.gradient = gradient; + candidate.x = static_cast (col_index); + candidate.y = static_cast (row_index); + + list1.push_back (candidate); + } + } + } + } + + list1.sort(); + + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + QuantizedMultiModFeature feature; + + feature.x = iter1->x; + feature.y = iter1->y; + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_color_gradients_ (iter1->x, iter1->y); + + features.push_back (feature); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientModality:: +computeMaxColorGradients (const typename pcl::PointCloud::ConstPtr & cloud) +{ + const int width = cloud->width; + const int height = cloud->height; + + color_gradients_.points.resize (width*height); + color_gradients_.width = width; + color_gradients_.height = height; + + const float pi = tan (1.0f) * 2; + for (int row_index = 0; row_index < height-2; ++row_index) + { + for (int col_index = 0; col_index < width-2; ++col_index) + { + const int index0 = row_index*width+col_index; + const int index_c = row_index*width+col_index+2; + const int index_r = (row_index+2)*width+col_index; + + //const int index_d = (row_index+1)*width+col_index+1; + + const unsigned char r0 = cloud->points[index0].r; + const unsigned char g0 = cloud->points[index0].g; + const unsigned char b0 = cloud->points[index0].b; + + const unsigned char r_c = cloud->points[index_c].r; + const unsigned char g_c = cloud->points[index_c].g; + const unsigned char b_c = cloud->points[index_c].b; + + const unsigned char r_r = cloud->points[index_r].r; + const unsigned char g_r = cloud->points[index_r].g; + const unsigned char b_r = cloud->points[index_r].b; + + const float r_dx = static_cast (r_c) - static_cast (r0); + const float g_dx = static_cast (g_c) - static_cast (g0); + const float b_dx = static_cast (b_c) - static_cast (b0); + + const float r_dy = static_cast (r_r) - static_cast (r0); + const float g_dy = static_cast (g_r) - static_cast (g0); + const float b_dy = static_cast (b_r) - static_cast (b0); + + const float sqr_mag_r = r_dx*r_dx + r_dy*r_dy; + const float sqr_mag_g = g_dx*g_dx + g_dy*g_dy; + const float sqr_mag_b = b_dx*b_dx + b_dy*b_dy; + + if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b) + { + GradientXY gradient; + gradient.magnitude = sqrt (sqr_mag_r); + gradient.angle = std::atan2 (r_dy, r_dx) * 180.0f / pi; + gradient.x = static_cast (col_index); + gradient.y = static_cast (row_index); + + color_gradients_ (col_index+1, row_index+1) = gradient; + } + else if (sqr_mag_g > sqr_mag_b) + { + GradientXY gradient; + gradient.magnitude = sqrt (sqr_mag_g); + gradient.angle = std::atan2 (g_dy, g_dx) * 180.0f / pi; + gradient.x = static_cast (col_index); + gradient.y = static_cast (row_index); + + color_gradients_ (col_index+1, row_index+1) = gradient; + } + else + { + GradientXY gradient; + gradient.magnitude = sqrt (sqr_mag_b); + gradient.angle = std::atan2 (b_dy, b_dx) * 180.0f / pi; + gradient.x = static_cast (col_index); + gradient.y = static_cast (row_index); + + color_gradients_ (col_index+1, row_index+1) = gradient; + } + + assert (color_gradients_ (col_index+1, row_index+1).angle >= -180 && + color_gradients_ (col_index+1, row_index+1).angle <= 180); + } + } + + return; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientModality:: +computeMaxColorGradientsSobel (const typename pcl::PointCloud::ConstPtr & cloud) +{ + const int width = cloud->width; + const int height = cloud->height; + + color_gradients_.points.resize (width*height); + color_gradients_.width = width; + color_gradients_.height = height; + + const float pi = tanf (1.0f) * 2.0f; + for (int row_index = 1; row_index < height-1; ++row_index) + { + for (int col_index = 1; col_index < width-1; ++col_index) + { + const int r7 = static_cast (cloud->points[(row_index-1)*width + (col_index-1)].r); + const int g7 = static_cast (cloud->points[(row_index-1)*width + (col_index-1)].g); + const int b7 = static_cast (cloud->points[(row_index-1)*width + (col_index-1)].b); + const int r8 = static_cast (cloud->points[(row_index-1)*width + (col_index)].r); + const int g8 = static_cast (cloud->points[(row_index-1)*width + (col_index)].g); + const int b8 = static_cast (cloud->points[(row_index-1)*width + (col_index)].b); + const int r9 = static_cast (cloud->points[(row_index-1)*width + (col_index+1)].r); + const int g9 = static_cast (cloud->points[(row_index-1)*width + (col_index+1)].g); + const int b9 = static_cast (cloud->points[(row_index-1)*width + (col_index+1)].b); + const int r4 = static_cast (cloud->points[(row_index)*width + (col_index-1)].r); + const int g4 = static_cast (cloud->points[(row_index)*width + (col_index-1)].g); + const int b4 = static_cast (cloud->points[(row_index)*width + (col_index-1)].b); + const int r6 = static_cast (cloud->points[(row_index)*width + (col_index+1)].r); + const int g6 = static_cast (cloud->points[(row_index)*width + (col_index+1)].g); + const int b6 = static_cast (cloud->points[(row_index)*width + (col_index+1)].b); + const int r1 = static_cast (cloud->points[(row_index+1)*width + (col_index-1)].r); + const int g1 = static_cast (cloud->points[(row_index+1)*width + (col_index-1)].g); + const int b1 = static_cast (cloud->points[(row_index+1)*width + (col_index-1)].b); + const int r2 = static_cast (cloud->points[(row_index+1)*width + (col_index)].r); + const int g2 = static_cast (cloud->points[(row_index+1)*width + (col_index)].g); + const int b2 = static_cast (cloud->points[(row_index+1)*width + (col_index)].b); + const int r3 = static_cast (cloud->points[(row_index+1)*width + (col_index+1)].r); + const int g3 = static_cast (cloud->points[(row_index+1)*width + (col_index+1)].g); + const int b3 = static_cast (cloud->points[(row_index+1)*width + (col_index+1)].b); + + //const int r_tmp1 = - r7 + r3; + //const int r_tmp2 = - r1 + r9; + //const int g_tmp1 = - g7 + g3; + //const int g_tmp2 = - g1 + g9; + //const int b_tmp1 = - b7 + b3; + //const int b_tmp2 = - b1 + b9; + ////const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9; + ////const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3; + //const int r_dx = r_tmp1 + r_tmp2 - (r4<<2) + (r6<<2); + //const int r_dy = r_tmp1 - r_tmp2 - (r8<<2) + (r2<<2); + //const int g_dx = g_tmp1 + g_tmp2 - (g4<<2) + (g6<<2); + //const int g_dy = g_tmp1 - g_tmp2 - (g8<<2) + (g2<<2); + //const int b_dx = b_tmp1 + b_tmp2 - (b4<<2) + (b6<<2); + //const int b_dy = b_tmp1 - b_tmp2 - (b8<<2) + (b2<<2); + + //const int r_tmp1 = - r7 + r3; + //const int r_tmp2 = - r1 + r9; + //const int g_tmp1 = - g7 + g3; + //const int g_tmp2 = - g1 + g9; + //const int b_tmp1 = - b7 + b3; + //const int b_tmp2 = - b1 + b9; + //const int gx = - r7 - (r4<<2) - r1 + r3 + (r6<<2) + r9; + //const int gy = - r7 - (r8<<2) - r9 + r1 + (r2<<2) + r3; + const int r_dx = r9 + 2*r6 + r3 - (r7 + 2*r4 + r1); + const int r_dy = r1 + 2*r2 + r3 - (r7 + 2*r8 + r9); + const int g_dx = g9 + 2*g6 + g3 - (g7 + 2*g4 + g1); + const int g_dy = g1 + 2*g2 + g3 - (g7 + 2*g8 + g9); + const int b_dx = b9 + 2*b6 + b3 - (b7 + 2*b4 + b1); + const int b_dy = b1 + 2*b2 + b3 - (b7 + 2*b8 + b9); + + const int sqr_mag_r = r_dx*r_dx + r_dy*r_dy; + const int sqr_mag_g = g_dx*g_dx + g_dy*g_dy; + const int sqr_mag_b = b_dx*b_dx + b_dy*b_dy; + + if (sqr_mag_r > sqr_mag_g && sqr_mag_r > sqr_mag_b) + { + GradientXY gradient; + gradient.magnitude = std::sqrt (static_cast (sqr_mag_r)); + gradient.angle = std::atan2 (static_cast (r_dy), static_cast (r_dx)) * 180.0f / pi; + if (gradient.angle < -180.0f) gradient.angle += 360.0f; + if (gradient.angle >= 180.0f) gradient.angle -= 360.0f; + gradient.x = static_cast (col_index); + gradient.y = static_cast (row_index); + + color_gradients_ (col_index, row_index) = gradient; + } + else if (sqr_mag_g > sqr_mag_b) + { + GradientXY gradient; + gradient.magnitude = std::sqrt (static_cast (sqr_mag_g)); + gradient.angle = std::atan2 (static_cast (g_dy), static_cast (g_dx)) * 180.0f / pi; + if (gradient.angle < -180.0f) gradient.angle += 360.0f; + if (gradient.angle >= 180.0f) gradient.angle -= 360.0f; + gradient.x = static_cast (col_index); + gradient.y = static_cast (row_index); + + color_gradients_ (col_index, row_index) = gradient; + } + else + { + GradientXY gradient; + gradient.magnitude = std::sqrt (static_cast (sqr_mag_b)); + gradient.angle = std::atan2 (static_cast (b_dy), static_cast (b_dx)) * 180.0f / pi; + if (gradient.angle < -180.0f) gradient.angle += 360.0f; + if (gradient.angle >= 180.0f) gradient.angle -= 360.0f; + gradient.x = static_cast (col_index); + gradient.y = static_cast (row_index); + + color_gradients_ (col_index, row_index) = gradient; + } + + assert (color_gradients_ (col_index, row_index).angle >= -180 && + color_gradients_ (col_index, row_index).angle <= 180); + } + } + + return; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientModality:: +quantizeColorGradients () +{ + //std::cerr << "quantize this, bastard!!!" << std::endl; + + //unsigned char quantization_map[16] = {0,1,2,3,4,5,6,7,0,1,2,3,4,5,6,7}; + //unsigned char quantization_map[16] = {1,2,3,4,5,6,7,8,1,2,3,4,5,6,7,8}; + + //for (float angle = 0.0f; angle < 360.0f; angle += 1.0f) + //{ + // const int quantized_value = quantization_map[static_cast (angle * angleScale)]; + // std::cerr << angle << ": " << quantized_value << std::endl; + //} + + + const std::size_t width = input_->width; + const std::size_t height = input_->height; + + quantized_color_gradients_.resize (width, height); + + const float angleScale = 16.0f/360.0f; + + //float min_angle = std::numeric_limits::max (); + //float max_angle = -std::numeric_limits::max (); + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + if (color_gradients_ (col_index, row_index).magnitude < gradient_magnitude_threshold_) + { + quantized_color_gradients_ (col_index, row_index) = 0; + continue; + } + + const float angle = 11.25f + color_gradients_ (col_index, row_index).angle + 180.0f; + const int quantized_value = (static_cast (angle * angleScale)) & 7; + quantized_color_gradients_ (col_index, row_index) = static_cast (quantized_value + 1); + + //const float angle = color_gradients_ (col_index, row_index).angle + 180.0f; + + //min_angle = std::min (min_angle, angle); + //max_angle = std::max (max_angle, angle); + + //if (angle < 0.0f || angle >= 360.0f) + //{ + // std::cerr << "angle shitty: " << angle << std::endl; + //} + + //const int quantized_value = quantization_map[static_cast (angle * angleScale)]; + //quantized_color_gradients_ (col_index, row_index) = static_cast (quantized_value); + + //assert (0 <= quantized_value && quantized_value < 16); + //quantized_color_gradients_ (col_index, row_index) = quantization_map[quantized_value]; + //quantized_color_gradients_ (col_index, row_index) = static_cast ((quantized_value & 7) + 1); // = (quantized_value % 8) + 1 + } + } + + //std::cerr << ">>>>> " << min_angle << ", " << max_angle << std::endl; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientModality:: +filterQuantizedColorGradients () +{ + const std::size_t width = input_->width; + const std::size_t height = input_->height; + + filtered_quantized_color_gradients_.resize (width, height); + + // filter data + for (std::size_t row_index = 1; row_index < height-1; ++row_index) + { + for (std::size_t col_index = 1; col_index < width-1; ++col_index) + { + unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0}; + + { + const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index-1)*width+col_index-1; + assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9); + ++histogram[data_ptr[0]]; + ++histogram[data_ptr[1]]; + ++histogram[data_ptr[2]]; + } + { + const unsigned char * data_ptr = quantized_color_gradients_.getData () + row_index*width+col_index-1; + assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9); + ++histogram[data_ptr[0]]; + ++histogram[data_ptr[1]]; + ++histogram[data_ptr[2]]; + } + { + const unsigned char * data_ptr = quantized_color_gradients_.getData () + (row_index+1)*width+col_index-1; + assert (data_ptr[0] < 9 && data_ptr[1] < 9 && data_ptr[2] < 9); + ++histogram[data_ptr[0]]; + ++histogram[data_ptr[1]]; + ++histogram[data_ptr[2]]; + } + + unsigned char max_hist_value = 0; + int max_hist_index = -1; + + // for (int i = 0; i < 8; ++i) + // { + // if (max_hist_value < histogram[i+1]) + // { + // max_hist_index = i; + // max_hist_value = histogram[i+1] + // } + // } + // Unrolled for performance optimization: + if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];} + if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];} + if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];} + if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];} + if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];} + if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];} + if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];} + if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];} + + if (max_hist_index != -1 && max_hist_value >= 5) + filtered_quantized_color_gradients_ (col_index, row_index) = static_cast (0x1 << max_hist_index); + else + filtered_quantized_color_gradients_ (col_index, row_index) = 0; + + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorGradientModality:: +erode (const pcl::MaskMap & mask_in, + pcl::MaskMap & mask_out) +{ + const std::size_t width = mask_in.getWidth (); + const std::size_t height = mask_in.getHeight (); + + mask_out.resize (width, height); + + for (std::size_t row_index = 1; row_index < height-1; ++row_index) + { + for (std::size_t col_index = 1; col_index < width-1; ++col_index) + { + if (mask_in (col_index, row_index-1) == 0 || + mask_in (col_index-1, row_index) == 0 || + mask_in (col_index+1, row_index) == 0 || + mask_in (col_index, row_index+1) == 0) + { + mask_out (col_index, row_index) = 0; + } + else + { + mask_out (col_index, row_index) = 255; + } + } + } +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/color_modality.h b/deps_install/include/pcl-1.10/pcl/recognition/color_modality.h new file mode 100755 index 0000000..9b7c704 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/color_modality.h @@ -0,0 +1,552 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace pcl +{ + + // -------------------------------------------------------------------------- + + template + class ColorModality + : public QuantizableModality, public PCLBase + { + protected: + using PCLBase::input_; + + struct Candidate + { + float distance; + + unsigned char bin_index; + + std::size_t x; + std::size_t y; + + bool + operator< (const Candidate & rhs) + { + return (distance > rhs.distance); + } + }; + + public: + using PointCloudIn = pcl::PointCloud; + + ColorModality (); + + virtual ~ColorModality (); + + inline QuantizedMap & + getQuantizedMap () + { + return (filtered_quantized_colors_); + } + + inline QuantizedMap & + getSpreadedQuantizedMap () + { + return (spreaded_filtered_quantized_colors_); + } + + void + extractFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modalityIndex, + std::vector & features) const; + + /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) + * \param cloud the const boost shared pointer to a PointCloud message + */ + virtual void + setInputCloud (const typename PointCloudIn::ConstPtr & cloud) + { + input_ = cloud; + } + + virtual void + processInputData (); + + protected: + + void + quantizeColors (); + + void + filterQuantizedColors (); + + static inline int + quantizeColorOnRGBExtrema (const float r, + const float g, + const float b); + + void + computeDistanceMap (const MaskMap & input, DistanceMap & output) const; + + private: + float feature_distance_threshold_; + + pcl::QuantizedMap quantized_colors_; + pcl::QuantizedMap filtered_quantized_colors_; + pcl::QuantizedMap spreaded_filtered_quantized_colors_; + + }; + +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ColorModality::ColorModality () + : feature_distance_threshold_ (1.0f), quantized_colors_ (), filtered_quantized_colors_ (), spreaded_filtered_quantized_colors_ () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ColorModality::~ColorModality () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorModality::processInputData () +{ + // quantize gradients + quantizeColors (); + + // filter quantized gradients to get only dominants one + thresholding + filterQuantizedColors (); + + // spread filtered quantized gradients + //spreadFilteredQunatizedColorGradients (); + const int spreading_size = 8; + pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_colors_, + spreaded_filtered_quantized_colors_, spreading_size); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::ColorModality::extractFeatures (const MaskMap & mask, + const std::size_t nr_features, + const std::size_t modality_index, + std::vector & features) const +{ + const std::size_t width = mask.getWidth (); + const std::size_t height = mask.getHeight (); + + MaskMap mask_maps[8]; + for (std::size_t map_index = 0; map_index < 8; ++map_index) + mask_maps[map_index].resize (width, height); + + unsigned char map[255]; + memset(map, 0, 255); + + map[0x1<<0] = 0; + map[0x1<<1] = 1; + map[0x1<<2] = 2; + map[0x1<<3] = 3; + map[0x1<<4] = 4; + map[0x1<<5] = 5; + map[0x1<<6] = 6; + map[0x1<<7] = 7; + + QuantizedMap distance_map_indices (width, height); + //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height); + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); + const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index); + + if (quantized_value == 0) + continue; + const int dist_map_index = map[quantized_value]; + + distance_map_indices (col_index, row_index) = dist_map_index; + //distance_maps[dist_map_index].at(row_index, col_index) = 255; + mask_maps[dist_map_index] (col_index, row_index) = 255; + } + } + } + + DistanceMap distance_maps[8]; + for (int map_index = 0; map_index < 8; ++map_index) + computeDistanceMap (mask_maps[map_index], distance_maps[map_index]); + + std::list list1; + std::list list2; + + float weights[8] = {0,0,0,0,0,0,0,0}; + + const std::size_t off = 4; + for (std::size_t row_index = off; row_index < height-off; ++row_index) + { + for (std::size_t col_index = off; col_index < width-off; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); + const unsigned char quantized_value = filtered_quantized_colors_ (col_index, row_index); + + //const float nx = surface_normals_ (col_index, row_index).normal_x; + //const float ny = surface_normals_ (col_index, row_index).normal_y; + //const float nz = surface_normals_ (col_index, row_index).normal_z; + + if (quantized_value != 0) + { + const int distance_map_index = map[quantized_value]; + + //const float distance = distance_maps[distance_map_index].at (row_index, col_index); + const float distance = distance_maps[distance_map_index] (col_index, row_index); + + if (distance >= feature_distance_threshold_) + { + Candidate candidate; + + candidate.distance = distance; + candidate.x = col_index; + candidate.y = row_index; + candidate.bin_index = distance_map_index; + + list1.push_back (candidate); + + ++weights[distance_map_index]; + } + } + } + } + } + + for (typename std::list::iterator iter = list1.begin (); iter != list1.end (); ++iter) + iter->distance *= 1.0f / weights[iter->bin_index]; + + list1.sort (); + + if (list1.size () <= nr_features) + { + features.reserve (list1.size ()); + for (typename std::list::iterator iter = list1.begin (); iter != list1.end (); ++iter) + { + QuantizedMultiModFeature feature; + + feature.x = static_cast (iter->x); + feature.y = static_cast (iter->y); + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_colors_ (iter->x, iter->y); + + features.push_back (feature); + } + + return; + } + + int distance = static_cast (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:! + while (list2.size () != nr_features) + { + const int sqr_distance = distance*distance; + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + bool candidate_accepted = true; + + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + const int dx = static_cast (iter1->x) - static_cast (iter2->x); + const int dy = static_cast (iter1->y) - static_cast (iter2->y); + const int tmp_distance = dx*dx + dy*dy; + + if (tmp_distance < sqr_distance) + { + candidate_accepted = false; + break; + } + } + + if (candidate_accepted) + list2.push_back (*iter1); + + if (list2.size () == nr_features) break; + } + --distance; + } + + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + QuantizedMultiModFeature feature; + + feature.x = static_cast (iter2->x); + feature.y = static_cast (iter2->y); + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_colors_ (iter2->x, iter2->y); + + features.push_back (feature); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorModality::quantizeColors () +{ + const std::size_t width = input_->width; + const std::size_t height = input_->height; + + quantized_colors_.resize (width, height); + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + const float r = static_cast ((*input_) (col_index, row_index).r); + const float g = static_cast ((*input_) (col_index, row_index).g); + const float b = static_cast ((*input_) (col_index, row_index).b); + + quantized_colors_ (col_index, row_index) = quantizeColorOnRGBExtrema (r, g, b); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::ColorModality::filterQuantizedColors () +{ + const std::size_t width = input_->width; + const std::size_t height = input_->height; + + filtered_quantized_colors_.resize (width, height); + + // filter data + for (std::size_t row_index = 1; row_index < height-1; ++row_index) + { + for (std::size_t col_index = 1; col_index < width-1; ++col_index) + { + unsigned char histogram[8] = {0,0,0,0,0,0,0,0}; + + { + const unsigned char * data_ptr = quantized_colors_.getData () + (row_index-1)*width+col_index-1; + assert (0 <= data_ptr[0] && data_ptr[0] < 9 && + 0 <= data_ptr[1] && data_ptr[1] < 9 && + 0 <= data_ptr[2] && data_ptr[2] < 9); + ++histogram[data_ptr[0]]; + ++histogram[data_ptr[1]]; + ++histogram[data_ptr[2]]; + } + { + const unsigned char * data_ptr = quantized_colors_.getData () + row_index*width+col_index-1; + assert (0 <= data_ptr[0] && data_ptr[0] < 9 && + 0 <= data_ptr[1] && data_ptr[1] < 9 && + 0 <= data_ptr[2] && data_ptr[2] < 9); + ++histogram[data_ptr[0]]; + ++histogram[data_ptr[1]]; + ++histogram[data_ptr[2]]; + } + { + const unsigned char * data_ptr = quantized_colors_.getData () + (row_index+1)*width+col_index-1; + assert (0 <= data_ptr[0] && data_ptr[0] < 9 && + 0 <= data_ptr[1] && data_ptr[1] < 9 && + 0 <= data_ptr[2] && data_ptr[2] < 9); + ++histogram[data_ptr[0]]; + ++histogram[data_ptr[1]]; + ++histogram[data_ptr[2]]; + } + + unsigned char max_hist_value = 0; + int max_hist_index = -1; + + // for (int i = 0; i < 8; ++i) + // { + // if (max_hist_value < histogram[i+1]) + // { + // max_hist_index = i; + // max_hist_value = histogram[i+1] + // } + // } + // Unrolled for performance optimization: + if (max_hist_value < histogram[0]) {max_hist_index = 0; max_hist_value = histogram[0];} + if (max_hist_value < histogram[1]) {max_hist_index = 1; max_hist_value = histogram[1];} + if (max_hist_value < histogram[2]) {max_hist_index = 2; max_hist_value = histogram[2];} + if (max_hist_value < histogram[3]) {max_hist_index = 3; max_hist_value = histogram[3];} + if (max_hist_value < histogram[4]) {max_hist_index = 4; max_hist_value = histogram[4];} + if (max_hist_value < histogram[5]) {max_hist_index = 5; max_hist_value = histogram[5];} + if (max_hist_value < histogram[6]) {max_hist_index = 6; max_hist_value = histogram[6];} + if (max_hist_value < histogram[7]) {max_hist_index = 7; max_hist_value = histogram[7];} + + //if (max_hist_index != -1 && max_hist_value >= 5) + filtered_quantized_colors_ (col_index, row_index) = 0x1 << max_hist_index; + //else + // filtered_quantized_color_gradients_ (col_index, row_index) = 0; + + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int +pcl::ColorModality::quantizeColorOnRGBExtrema (const float r, + const float g, + const float b) +{ + const float r_inv = 255.0f-r; + const float g_inv = 255.0f-g; + const float b_inv = 255.0f-b; + + const float dist_0 = (r*r + g*g + b*b)*2.0f; + const float dist_1 = r*r + g*g + b_inv*b_inv; + const float dist_2 = r*r + g_inv*g_inv+ b*b; + const float dist_3 = r*r + g_inv*g_inv + b_inv*b_inv; + const float dist_4 = r_inv*r_inv + g*g + b*b; + const float dist_5 = r_inv*r_inv + g*g + b_inv*b_inv; + const float dist_6 = r_inv*r_inv + g_inv*g_inv+ b*b; + const float dist_7 = (r_inv*r_inv + g_inv*g_inv + b_inv*b_inv)*1.5f; + + const float min_dist = std::min (std::min (std::min (dist_0, dist_1), std::min (dist_2, dist_3)), std::min (std::min (dist_4, dist_5), std::min (dist_6, dist_7))); + + if (min_dist == dist_0) + { + return 0; + } + if (min_dist == dist_1) + { + return 1; + } + if (min_dist == dist_2) + { + return 2; + } + if (min_dist == dist_3) + { + return 3; + } + if (min_dist == dist_4) + { + return 4; + } + if (min_dist == dist_5) + { + return 5; + } + if (min_dist == dist_6) + { + return 6; + } + return 7; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ColorModality::computeDistanceMap (const MaskMap & input, + DistanceMap & output) const +{ + const std::size_t width = input.getWidth (); + const std::size_t height = input.getHeight (); + + output.resize (width, height); + + // compute distance map + //float *distance_map = new float[input_->points.size ()]; + const unsigned char * mask_map = input.getData (); + float * distance_map = output.getData (); + for (std::size_t index = 0; index < width*height; ++index) + { + if (mask_map[index] == 0) + distance_map[index] = 0.0f; + else + distance_map[index] = static_cast (width + height); + } + + // first pass + float * previous_row = distance_map; + float * current_row = previous_row + width; + for (std::size_t ri = 1; ri < height; ++ri) + { + for (std::size_t ci = 1; ci < width; ++ci) + { + const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f; + const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f; + const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f; + const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f; + const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; + + const float min_value = std::min (std::min (up_left, up), std::min (left, up_right)); + + if (min_value < center) + current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value; + } + previous_row = current_row; + current_row += width; + } + + // second pass + float * next_row = distance_map + width * (height - 1); + current_row = next_row - width; + for (int ri = static_cast (height)-2; ri >= 0; --ri) + { + for (int ci = static_cast (width)-2; ci >= 0; --ci) + { + const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f; + const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f; + const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f; + const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f; + const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; + + const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right)); + + if (min_value < center) + current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value; + } + next_row = current_row; + current_row -= width; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/crh_alignment.h b/deps_install/include/pcl-1.10/pcl/recognition/crh_alignment.h new file mode 100755 index 0000000..3a44425 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/crh_alignment.h @@ -0,0 +1,271 @@ +/* + * crh_recognition.h + * + * Created on: Mar 30, 2012 + * Author: aitor + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + + /** \brief CRHAlignment uses two Camera Roll Histograms (CRH) to find the + * roll rotation that aligns both views. See: + * - CAD-Model Recognition and 6 DOF Pose Estimation + * A. Aldoma, N. Blodow, D. Gossow, S. Gedikli, R.B. Rusu, M. Vincze and G. Bradski + * ICCV 2011, 3D Representation and Recognition (3dRR11) workshop + * Barcelona, Spain, (2011) + * + * \author Aitor Aldoma + * \ingroup recognition + */ + + template + class PCL_EXPORTS CRHAlignment + { + private: + + /** \brief Sorts peaks */ + struct peaks_ordering + { + bool + operator() (std::pair const& a, std::pair const& b) + { + return a.first > b.first; + } + }; + + using PointTPtr = typename pcl::PointCloud::Ptr; + + /** \brief View of the model to be aligned to input_view_ */ + PointTPtr target_view_; + /** \brief View of the input */ + PointTPtr input_view_; + /** \brief Centroid of the model_view_ */ + Eigen::Vector3f centroid_target_; + /** \brief Centroid of the input_view_ */ + Eigen::Vector3f centroid_input_; + /** \brief transforms from model view to input view */ + std::vector > transforms_; + /** \brief Allowed maximum number of peaks */ + int max_peaks_; + /** \brief Quantile of peaks after sorting to be checked */ + float quantile_; + /** \brief Threshold for a peak to be accepted. + * If peak_i >= (max_peak * accept_threhsold_) => peak is accepted + */ + float accept_threshold_; + + /** \brief computes the transformation to the z-axis + * \param[in] centroid + * \param[out] trasnformation to z-axis + */ + void + computeTransformToZAxes (Eigen::Vector3f & centroid, Eigen::Affine3f & transform) + { + Eigen::Vector3f plane_normal; + plane_normal[0] = -centroid[0]; + plane_normal[1] = -centroid[1]; + plane_normal[2] = -centroid[2]; + Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ (); + plane_normal.normalize (); + Eigen::Vector3f axis = plane_normal.cross (z_vector); + double rotation = -asin (axis.norm ()); + axis.normalize (); + transform = Eigen::Affine3f (Eigen::AngleAxisf (static_cast(rotation), axis)); + } + + /** \brief computes the roll transformation + * \param[in] centroid input + * \param[in] centroid view + * \param[in] roll_angle + * \param[out] roll transformation + */ + void + computeRollTransform (Eigen::Vector3f & centroidInput, Eigen::Vector3f & centroidResult, double roll_angle, Eigen::Affine3f & final_trans) + { + Eigen::Affine3f transformInputToZ; + computeTransformToZAxes (centroidInput, transformInputToZ); + + transformInputToZ = transformInputToZ.inverse (); + Eigen::Affine3f transformRoll (Eigen::AngleAxisf (-static_cast(roll_angle * M_PI / 180), Eigen::Vector3f::UnitZ ())); + Eigen::Affine3f transformDBResultToZ; + computeTransformToZAxes (centroidResult, transformDBResultToZ); + + final_trans = transformInputToZ * transformRoll * transformDBResultToZ; + } + public: + + /** \brief Constructor. */ + CRHAlignment() { + max_peaks_ = 5; + quantile_ = 0.2f; + accept_threshold_ = 0.8f; + } + + /** \brief returns the computed transformations + * \param[out] transforms transformations + */ + void getTransforms(std::vector > & transforms) { + transforms = transforms_; + } + + /** \brief sets model and input views + * \param[in] input_view + * \param[in] target_view + */ + void + setInputAndTargetView (PointTPtr & input_view, PointTPtr & target_view) + { + target_view_ = target_view; + input_view_ = input_view; + } + + /** \brief sets model and input centroids + * \param[in] c1 model view centroid + * \param[in] c2 input view centroid + */ + void + setInputAndTargetCentroids (Eigen::Vector3f & c1, Eigen::Vector3f & c2) + { + centroid_target_ = c2; + centroid_input_ = c1; + } + + /** \brief Computes the transformation aligning model to input + * \param[in] input_ftt CRH histogram of the input cloud + * \param[in] target_ftt CRH histogram of the target cloud + */ + void + align (pcl::PointCloud > & input_ftt, pcl::PointCloud > & target_ftt) + { + + transforms_.clear(); //clear from last round... + + std::vector peaks; + computeRollAngle (input_ftt, target_ftt, peaks); + + //if the number of peaks is too big, we should try to reduce using siluette matching + + for (const float &peak : peaks) + { + Eigen::Affine3f rollToRot; + computeRollTransform (centroid_input_, centroid_target_, peak, rollToRot); + + Eigen::Matrix4f rollHomMatrix = Eigen::Matrix4f (); + rollHomMatrix.setIdentity (4, 4); + rollHomMatrix = rollToRot.matrix (); + + Eigen::Matrix4f translation2; + translation2.setIdentity (4, 4); + Eigen::Vector3f centr = rollToRot * centroid_target_; + translation2 (0, 3) = centroid_input_[0] - centr[0]; + translation2 (1, 3) = centroid_input_[1] - centr[1]; + translation2 (2, 3) = centroid_input_[2] - centr[2]; + + Eigen::Matrix4f resultHom (translation2 * rollHomMatrix); + transforms_.push_back(resultHom.inverse()); + } + + } + + /** \brief Computes the roll angle that aligns input to model. + * \param[in] input_ftt CRH histogram of the input cloud + * \param[in] target_ftt CRH histogram of the target cloud + * \param[out] peaks Vector containing angles where the histograms correlate + */ + void + computeRollAngle (pcl::PointCloud > & input_ftt, pcl::PointCloud > & target_ftt, + std::vector & peaks) + { + + pcl::PointCloud > input_ftt_negate (input_ftt); + + for (int i = 2; i < (nbins_); i += 2) + input_ftt_negate.points[0].histogram[i] = -input_ftt_negate.points[0].histogram[i]; + + int nr_bins_after_padding = 180; + int peak_distance = 5; + int cutoff = nbins_ - 1; + + kiss_fft_cpx * multAB = new kiss_fft_cpx[nr_bins_after_padding]; + for (int i = 0; i < nr_bins_after_padding; i++) + multAB[i].r = multAB[i].i = 0.f; + + int k = 0; + multAB[k].r = input_ftt_negate.points[0].histogram[0] * target_ftt.points[0].histogram[0]; + k++; + + float a, b, c, d; + for (int i = 1; i < cutoff; i += 2, k++) + { + a = input_ftt_negate.points[0].histogram[i]; + b = input_ftt_negate.points[0].histogram[i + 1]; + c = target_ftt.points[0].histogram[i]; + d = target_ftt.points[0].histogram[i + 1]; + multAB[k].r = a * c - b * d; + multAB[k].i = b * c + a * d; + + float tmp = std::sqrt (multAB[k].r * multAB[k].r + multAB[k].i * multAB[k].i); + + multAB[k].r /= tmp; + multAB[k].i /= tmp; + } + + multAB[nbins_ - 1].r = input_ftt_negate.points[0].histogram[nbins_ - 1] * target_ftt.points[0].histogram[nbins_ - 1]; + + kiss_fft_cfg mycfg = kiss_fft_alloc (nr_bins_after_padding, 1, nullptr, nullptr); + kiss_fft_cpx * invAB = new kiss_fft_cpx[nr_bins_after_padding]; + kiss_fft (mycfg, multAB, invAB); + + std::vector < std::pair > scored_peaks (nr_bins_after_padding); + for (int i = 0; i < nr_bins_after_padding; i++) + scored_peaks[i] = std::make_pair (invAB[i].r, i); + + std::sort (scored_peaks.begin (), scored_peaks.end (), peaks_ordering ()); + + std::vector peaks_indices; + std::vector peaks_values; + + // we look at the upper quantile_ + float quantile = quantile_; + int max_inserted= max_peaks_; + + int inserted=0; + bool stop=false; + for (int i = 0; (i < static_cast (quantile * static_cast (nr_bins_after_padding))) && !stop; i++) + { + if (scored_peaks[i].first >= scored_peaks[0].first * accept_threshold_) + { + bool insert = true; + + for (const int &peaks_index : peaks_indices) + { //check inserted peaks, first pick always inserted + if ((std::abs (peaks_index - scored_peaks[i].second) <= peak_distance) || + (std::abs (peaks_index - (scored_peaks[i].second - nr_bins_after_padding)) <= peak_distance)) + { + insert = false; + break; + } + } + + if (insert) + { + peaks_indices.push_back (scored_peaks[i].second); + peaks_values.push_back (scored_peaks[i].first); + peaks.push_back (static_cast (scored_peaks[i].second * (360 / nr_bins_after_padding))); + inserted++; + if(inserted >= max_inserted) + stop = true; + } + } + } + } + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/dense_quantized_multi_mod_template.h b/deps_install/include/pcl-1.10/pcl/recognition/dense_quantized_multi_mod_template.h new file mode 100755 index 0000000..892f78a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/dense_quantized_multi_mod_template.h @@ -0,0 +1,114 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + + struct DenseQuantizedSingleModTemplate + { + std::vector features; + + void + serialize (std::ostream & stream) const + { + const std::size_t num_of_features = static_cast (features.size ()); + write (stream, num_of_features); + for (std::size_t feature_index = 0; feature_index < num_of_features; ++feature_index) + { + write (stream, features[feature_index]); + } + } + + void + deserialize (std::istream & stream) + { + features.clear (); + + std::size_t num_of_features; + read (stream, num_of_features); + features.resize (num_of_features); + for (std::size_t feature_index = 0; feature_index < num_of_features; ++feature_index) + { + read (stream, features[feature_index]); + } + } + }; + + struct DenseQuantizedMultiModTemplate + { + std::vector modalities; + float response_factor; + + RegionXY region; + + void + serialize (std::ostream & stream) const + { + const std::size_t num_of_modalities = static_cast (modalities.size ()); + write (stream, num_of_modalities); + for (std::size_t modality_index = 0; modality_index < num_of_modalities; ++modality_index) + { + modalities[modality_index].serialize (stream); + } + + region.serialize (stream); + } + + void + deserialize (std::istream & stream) + { + modalities.clear (); + + std::size_t num_of_modalities; + read (stream, num_of_modalities); + modalities.resize (num_of_modalities); + for (std::size_t modality_index = 0; modality_index < num_of_modalities; ++modality_index) + { + modalities[modality_index].deserialize (stream); + } + + region.deserialize (stream); + } + }; + +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/distance_map.h b/deps_install/include/pcl-1.10/pcl/recognition/distance_map.h new file mode 100755 index 0000000..defb00d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/distance_map.h @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +namespace pcl +{ + + /** \brief Represents a distance map obtained from a distance transformation. + * \author Stefan Holzer + */ + class DistanceMap + { + public: + /** \brief Constructor. */ + DistanceMap () : data_ (0), width_ (0), height_ (0) {} + /** \brief Destructor. */ + virtual ~DistanceMap () {} + + /** \brief Returns the width of the map. */ + inline std::size_t + getWidth () const + { + return (width_); + } + + /** \brief Returns the height of the map. */ + inline std::size_t + getHeight () const + { + return (height_); + } + + /** \brief Returns a pointer to the beginning of map. */ + inline float * + getData () + { + return (&data_[0]); + } + + /** \brief Resizes the map to the specified size. + * \param[in] width the new width of the map. + * \param[in] height the new height of the map. + */ + void + resize (const std::size_t width, const std::size_t height) + { + data_.resize (width*height); + width_ = width; + height_ = height; + } + + /** \brief Operator to access an element of the map. + * \param[in] col_index the column index of the element to access. + * \param[in] row_index the row index of the element to access. + */ + inline float & + operator() (const std::size_t col_index, const std::size_t row_index) + { + return (data_[row_index*width_ + col_index]); + } + + /** \brief Operator to access an element of the map. + * \param[in] col_index the column index of the element to access. + * \param[in] row_index the row index of the element to access. + */ + inline const float & + operator() (const std::size_t col_index, const std::size_t row_index) const + { + return (data_[row_index*width_ + col_index]); + } + + protected: + /** \brief The storage for the distance map data. */ + std::vector data_; + /** \brief The width of the map. */ + std::size_t width_; + /** \brief The height of the map. */ + std::size_t height_; + }; + +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/dot_modality.h b/deps_install/include/pcl-1.10/pcl/recognition/dot_modality.h new file mode 100755 index 0000000..f07106e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/dot_modality.h @@ -0,0 +1,65 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + class PCL_EXPORTS DOTModality + { + public: + + virtual ~DOTModality () {}; + + //virtual QuantizedMap & + //getDominantQuantizedMap () = 0; + + virtual QuantizedMap & + getDominantQuantizedMap () = 0; + + virtual QuantizedMap + computeInvariantQuantizedMap (const MaskMap & mask, + const RegionXY & region) = 0; + + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/dotmod.h b/deps_install/include/pcl-1.10/pcl/recognition/dotmod.h new file mode 100755 index 0000000..2664c92 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/dotmod.h @@ -0,0 +1,128 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + + struct DOTMODDetection + { + std::size_t bin_x; + std::size_t bin_y; + std::size_t template_id; + float score; + }; + + /** + * \brief Template matching using the DOTMOD approach. + * \author Stefan Holzer, Stefan Hinterstoisser + */ + class PCL_EXPORTS DOTMOD + { + public: + /** \brief Constructor */ + DOTMOD (std::size_t template_width, + std::size_t template_height); + + /** \brief Destructor */ + virtual ~DOTMOD (); + + /** \brief Creates a template from the specified data and adds it to the matching queue. + * \param modalities + * \param masks + * \param template_anker_x + * \param template_anker_y + * \param region + */ + std::size_t + createAndAddTemplate (const std::vector & modalities, + const std::vector & masks, + std::size_t template_anker_x, + std::size_t template_anker_y, + const RegionXY & region); + + void + detectTemplates (const std::vector & modalities, + float template_response_threshold, + std::vector & detections, + const std::size_t bin_size) const; + + inline const DenseQuantizedMultiModTemplate & + getTemplate (std::size_t template_id) const + { + return (templates_[template_id]); + } + + inline std::size_t + getNumOfTemplates () + { + return (templates_.size ()); + } + + void + saveTemplates (const char * file_name) const; + + void + loadTemplates (const char * file_name); + + void + serialize (std::ostream & stream) const; + + void + deserialize (std::istream & stream); + + + private: + /** template width */ + std::size_t template_width_; + /** template height */ + std::size_t template_height_; + /** template storage */ + std::vector templates_; + }; + +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/face_detection/face_common.h b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/face_common.h new file mode 100755 index 0000000..fda2962 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/face_common.h @@ -0,0 +1,166 @@ +#pragma once + +#include +#include + +#include + +namespace pcl +{ + namespace face_detection + { + class TrainingExample + { + public: + std::vector::Ptr> iimages_; //also pointer to the respective integral image + int row_, col_; + int wsize_; + int label_; + + //save pose head information + Eigen::Vector3f trans_; + Eigen::Vector3f rot_; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + class FeatureType + { + public: + int row1_, col1_; + int row2_, col2_; + + int wsizex1_, wsizey1_; + int wsizex2_, wsizey2_; + + float threshold_; + int used_ii_; + + FeatureType() + { + used_ii_ = 0; + } + + void serialize(std::ostream & stream) const + { + stream.write (reinterpret_cast (&row1_), sizeof(row1_)); + stream.write (reinterpret_cast (&col1_), sizeof(col1_)); + stream.write (reinterpret_cast (&row2_), sizeof(row2_)); + stream.write (reinterpret_cast (&col2_), sizeof(col2_)); + stream.write (reinterpret_cast (&wsizex1_), sizeof(wsizex1_)); + stream.write (reinterpret_cast (&wsizex2_), sizeof(wsizex2_)); + stream.write (reinterpret_cast (&wsizey1_), sizeof(wsizey1_)); + stream.write (reinterpret_cast (&wsizey2_), sizeof(wsizey2_)); + stream.write (reinterpret_cast (&threshold_), sizeof(threshold_)); + stream.write (reinterpret_cast (&used_ii_), sizeof(used_ii_)); + } + + inline void deserialize(std::istream & stream) + { + stream.read (reinterpret_cast (&row1_), sizeof(row1_)); + stream.read (reinterpret_cast (&col1_), sizeof(col1_)); + stream.read (reinterpret_cast (&row2_), sizeof(row2_)); + stream.read (reinterpret_cast (&col2_), sizeof(col2_)); + stream.read (reinterpret_cast (&wsizex1_), sizeof(wsizex1_)); + stream.read (reinterpret_cast (&wsizex2_), sizeof(wsizex2_)); + stream.read (reinterpret_cast (&wsizey1_), sizeof(wsizey1_)); + stream.read (reinterpret_cast (&wsizey2_), sizeof(wsizey2_)); + stream.read (reinterpret_cast (&threshold_), sizeof(threshold_)); + stream.read (reinterpret_cast (&used_ii_), sizeof(used_ii_)); + } + }; + + template + class RFTreeNode + { + public: + float threshold; + FeatureType feature; + std::vector sub_nodes; + float value; + float variance; + + Eigen::Vector3d trans_mean_; + Eigen::Vector3d rot_mean_; + + float purity_; + Eigen::Matrix3d covariance_trans_; + Eigen::Matrix3d covariance_rot_; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + void serialize(::std::ostream & stream) const + { + + const int num_of_sub_nodes = static_cast (sub_nodes.size ()); + stream.write (reinterpret_cast (&num_of_sub_nodes), sizeof(num_of_sub_nodes)); + + if (!sub_nodes.empty ()) + { + feature.serialize (stream); + stream.write (reinterpret_cast (&threshold), sizeof(threshold)); + } + + stream.write (reinterpret_cast (&value), sizeof(value)); + stream.write (reinterpret_cast (&variance), sizeof(variance)); + + for (std::size_t i = 0; i < 3; i++) + stream.write (reinterpret_cast (&trans_mean_[i]), sizeof(trans_mean_[i])); + + for (std::size_t i = 0; i < 3; i++) + stream.write (reinterpret_cast (&rot_mean_[i]), sizeof(rot_mean_[i])); + + for (std::size_t i = 0; i < 3; i++) + for (std::size_t j = 0; j < 3; j++) + stream.write (reinterpret_cast (&covariance_trans_ (i, j)), sizeof(covariance_trans_ (i, j))); + + for (std::size_t i = 0; i < 3; i++) + for (std::size_t j = 0; j < 3; j++) + stream.write (reinterpret_cast (&covariance_rot_ (i, j)), sizeof(covariance_rot_ (i, j))); + + for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; ++sub_node_index) + { + sub_nodes[sub_node_index].serialize (stream); + } + } + + inline void deserialize(::std::istream & stream) + { + int num_of_sub_nodes; + stream.read (reinterpret_cast (&num_of_sub_nodes), sizeof(num_of_sub_nodes)); + + if (num_of_sub_nodes > 0) + { + feature.deserialize (stream); + stream.read (reinterpret_cast (&threshold), sizeof(threshold)); + } + + stream.read (reinterpret_cast (&value), sizeof(value)); + stream.read (reinterpret_cast (&variance), sizeof(variance)); + + for (std::size_t i = 0; i < 3; i++) + stream.read (reinterpret_cast (&trans_mean_[i]), sizeof(trans_mean_[i])); + + for (std::size_t i = 0; i < 3; i++) + stream.read (reinterpret_cast (&rot_mean_[i]), sizeof(rot_mean_[i])); + + for (std::size_t i = 0; i < 3; i++) + for (std::size_t j = 0; j < 3; j++) + stream.read (reinterpret_cast (&covariance_trans_ (i, j)), sizeof(covariance_trans_ (i, j))); + + for (std::size_t i = 0; i < 3; i++) + for (std::size_t j = 0; j < 3; j++) + stream.read (reinterpret_cast (&covariance_rot_ (i, j)), sizeof(covariance_rot_ (i, j))); + + sub_nodes.resize (num_of_sub_nodes); + + if (num_of_sub_nodes > 0) + { + for (int sub_node_index = 0; sub_node_index < num_of_sub_nodes; ++sub_node_index) + { + sub_nodes[sub_node_index].deserialize (stream); + } + } + } + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/face_detection/face_detector_data_provider.h b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/face_detector_data_provider.h new file mode 100755 index 0000000..669dbbc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/face_detector_data_provider.h @@ -0,0 +1,164 @@ +/* + * face_detector_data_provider.h + * + * Created on: Sep 2, 2012 + * Author: aitor + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace bf = boost::filesystem; + +namespace pcl +{ + namespace face_detection + { + template + class FaceDetectorDataProvider: public pcl::DecisionTreeTrainerDataProvider + { + private: + int num_images_; + std::vector image_files_; + bool USE_NORMALS_; + int w_size_; + int patches_per_image_; + int min_images_per_bin_; + + void getFilesInDirectory(bf::path & dir, std::string & rel_path_so_far, std::vector & relative_paths, std::string & ext) + { + for (const auto& dir_entry : bf::directory_iterator(dir)) + { + //check if its a directory, then get models in it + if (bf::is_directory (dir_entry)) + { + std::string so_far = rel_path_so_far + (dir_entry.path ().filename ()).string () + "/"; + bf::path curr_path = dir_entry.path (); + getFilesInDirectory (curr_path, so_far, relative_paths, ext); + } else + { + //check that it is a ply file and then add, otherwise ignore.. + std::vector < std::string > strs; + std::string file = (dir_entry.path ().filename ()).string (); + boost::split (strs, file, boost::is_any_of (".")); + std::string extension = strs[strs.size () - 1]; + + if (extension == ext) + { + std::string path = rel_path_so_far + (dir_entry.path ().filename ()).string (); + relative_paths.push_back (path); + } + } + } + } + + inline bool readMatrixFromFile(std::string file, Eigen::Matrix4f & matrix) + { + + std::ifstream in; + in.open (file.c_str (), std::ifstream::in); + if (!in.is_open ()) + { + return false; + } + + char linebuf[1024]; + in.getline (linebuf, 1024); + std::string line (linebuf); + std::vector < std::string > strs_2; + boost::split (strs_2, line, boost::is_any_of (" ")); + + for (int i = 0; i < 16; i++) + { + matrix (i / 4, i % 4) = static_cast (atof (strs_2[i].c_str ())); + } + + return true; + } + + bool check_inside(int col, int row, int min_col, int max_col, int min_row, int max_row) + { + return col >= min_col && col <= max_col && row >= min_row && row <= max_row; + } + + template + void cropCloud(int min_col, int max_col, int min_row, int max_row, pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out) + { + cloud_out.width = max_col - min_col + 1; + cloud_out.height = max_row - min_row + 1; + cloud_out.points.resize (cloud_out.width * cloud_out.height); + for (unsigned int u = 0; u < cloud_out.width; u++) + { + for (unsigned int v = 0; v < cloud_out.height; v++) + { + cloud_out.at (u, v) = cloud_in.at (min_col + u, min_row + v); + } + } + + cloud_out.is_dense = cloud_in.is_dense; + } + + public: + + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + FaceDetectorDataProvider() + { + w_size_ = 80; + USE_NORMALS_ = false; + num_images_ = 10; + patches_per_image_ = 20; + min_images_per_bin_ = -1; + } + + virtual ~FaceDetectorDataProvider() + { + + } + + void setPatchesPerImage(int n) + { + patches_per_image_ = n; + } + + void setMinImagesPerBin(int n) + { + min_images_per_bin_ = n; + } + + void setUseNormals(bool use) + { + USE_NORMALS_ = use; + } + + void setWSize(int size) + { + w_size_ = size; + } + + void setNumImages(int n) + { + num_images_ = n; + } + + void initialize(std::string & data_dir); + + //shuffle file and get the first num_images_ as requested by a tree + //extract positive and negative samples + //create training examples and labels + void getDatasetAndLabels(DataSet & data_set, std::vector & label_data, std::vector & examples) override; + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/face_detection/rf_face_detector_trainer.h b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/rf_face_detector_trainer.h new file mode 100755 index 0000000..68d652c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/rf_face_detector_trainer.h @@ -0,0 +1,260 @@ +/* + * rf_face_detector_trainer.h + * + * Created on: 22 Sep 2012 + * Author: Aitor Aldoma + */ + +#pragma once + +#include "pcl/recognition/face_detection/face_detector_data_provider.h" +#include "pcl/recognition/face_detection/rf_face_utils.h" +#include "pcl/ml/dt/decision_forest.h" +#include + +namespace pcl +{ + class PCL_EXPORTS RFFaceDetectorTrainer + { + private: + int w_size_; + int max_patch_size_; + int stride_sw_; + int ntrees_; + std::string forest_filename_; + int nfeatures_; + float thres_face_; + int num_images_; + float trans_max_variance_; + std::size_t min_votes_size_; + int used_for_pose_; + bool use_normals_; + std::string directory_; + float HEAD_ST_DIAMETER_; + float larger_radius_ratio_; + std::vector > head_center_votes_; + std::vector > > head_center_votes_clustered_; + std::vector > > head_center_original_votes_clustered_; + std::vector > angle_votes_; + std::vector uncertainties_; + std::vector > head_clusters_centers_; + std::vector > head_clusters_rotation_; + + pcl::PointCloud::Ptr input_; + pcl::PointCloud::Ptr face_heat_map_; + + using NodeType = face_detection::RFTreeNode; + pcl::DecisionForest forest_; + + std::string model_path_; + bool pose_refinement_; + int icp_iterations_; + + pcl::PointCloud::Ptr model_original_; + float res_; + + public: + + RFFaceDetectorTrainer() + { + w_size_ = 80; + max_patch_size_ = 40; + stride_sw_ = 4; + ntrees_ = 10; + forest_filename_ = std::string ("forest.txt"); + nfeatures_ = 10000; + thres_face_ = 1.f; + num_images_ = 1000; + trans_max_variance_ = 1600.f; + used_for_pose_ = std::numeric_limits::max (); + use_normals_ = false; + directory_ = std::string (""); + HEAD_ST_DIAMETER_ = 0.2364f; + larger_radius_ratio_ = 1.5f; + face_heat_map_.reset (); + model_path_ = std::string ("face_mesh.ply"); + pose_refinement_ = false; + res_ = 0.005f; + } + + virtual ~RFFaceDetectorTrainer() + { + + } + + /* + * Common parameters + */ + void setForestFilename(std::string & ff) + { + forest_filename_ = ff; + } + + void setUseNormals(bool use) + { + use_normals_ = use; + } + + void setWSize(int s) + { + w_size_ = s; + } + + /* + * Training parameters + */ + + void setDirectory(std::string & dir) + { + directory_ = dir; + } + void setNumTrainingImages(int num) + { + num_images_ = num; + } + + void setNumTrees(int num) + { + ntrees_ = num; + } + + void setNumFeatures(int num) + { + nfeatures_ = num; + } + + /* + * Detection parameters + */ + + void setModelPath(std::string & model); + + void setPoseRefinement(bool do_it, int iters = 5) + { + pose_refinement_ = do_it; + icp_iterations_ = iters; + } + + void setLeavesFaceThreshold(float p) + { + thres_face_ = p; + } + + void setLeavesFaceMaxVariance(float max) + { + trans_max_variance_ = max; + } + + void setWStride(int s) + { + stride_sw_ = s; + } + + void setFaceMinVotes(int mv) + { + min_votes_size_ = mv; + } + + void setNumVotesUsedForPose(int n) + { + used_for_pose_ = n; + } + + void setForest(pcl::DecisionForest & forest) + { + forest_ = forest; + } + + /* + * Get functions + */ + + void getFaceHeatMap(pcl::PointCloud::Ptr & heat_map) + { + heat_map = face_heat_map_; + } + + //get votes + void getVotes(pcl::PointCloud::Ptr & votes_cloud) + { + votes_cloud->points.resize (head_center_votes_.size ()); + votes_cloud->width = static_cast(head_center_votes_.size ()); + votes_cloud->height = 1; + + for (std::size_t i = 0; i < head_center_votes_.size (); i++) + { + votes_cloud->points[i].getVector3fMap () = head_center_votes_[i]; + } + } + + void getVotes(pcl::PointCloud::Ptr & votes_cloud) + { + votes_cloud->points.resize (head_center_votes_.size ()); + votes_cloud->width = static_cast(head_center_votes_.size ()); + votes_cloud->height = 1; + + int p = 0; + for (std::size_t i = 0; i < head_center_votes_clustered_.size (); i++) + { + for (std::size_t j = 0; j < head_center_votes_clustered_[i].size (); j++, p++) + { + votes_cloud->points[p].getVector3fMap () = head_center_votes_clustered_[i][j]; + votes_cloud->points[p].intensity = 0.1f * static_cast (i); + } + } + + votes_cloud->points.resize (p); + } + + void getVotes2(pcl::PointCloud::Ptr & votes_cloud) + { + votes_cloud->points.resize (head_center_votes_.size ()); + votes_cloud->width = static_cast(head_center_votes_.size ()); + votes_cloud->height = 1; + + int p = 0; + for (std::size_t i = 0; i < head_center_original_votes_clustered_.size (); i++) + { + for (std::size_t j = 0; j < head_center_original_votes_clustered_[i].size (); j++, p++) + { + votes_cloud->points[p].getVector3fMap () = head_center_original_votes_clustered_[i][j]; + votes_cloud->points[p].intensity = 0.1f * static_cast (i); + } + } + + votes_cloud->points.resize (p); + } + + //get heads + void getDetectedFaces(std::vector & faces) + { + for (std::size_t i = 0; i < head_clusters_centers_.size (); i++) + { + Eigen::VectorXf head (6); + head[0] = head_clusters_centers_[i][0]; + head[1] = head_clusters_centers_[i][1]; + head[2] = head_clusters_centers_[i][2]; + head[3] = head_clusters_rotation_[i][0]; + head[4] = head_clusters_rotation_[i][1]; + head[5] = head_clusters_rotation_[i][2]; + faces.push_back (head); + } + } + /* + * Other functions + */ + void setInputCloud(pcl::PointCloud::Ptr & cloud) + { + input_ = cloud; + } + + void setFaceHeatMapCloud(pcl::PointCloud::Ptr & heat_map) + { + face_heat_map_ = heat_map; + } + + void trainWithDataProvider(); + void faceVotesClustering(); + void detectFaces(); + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/face_detection/rf_face_utils.h b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/rf_face_utils.h new file mode 100755 index 0000000..a592c00 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/face_detection/rf_face_utils.h @@ -0,0 +1,537 @@ +/* + * fanellis_face_detector.h + * + * Created on: 22 Sep 2012 + * Author: Aitor Aldoma + */ + +#pragma once + +#include "pcl/recognition/face_detection/face_common.h" +#include +#include +#include + +namespace pcl +{ + namespace face_detection + { + template + class FeatureHandlerDepthAverage: public pcl::FeatureHandler + { + + private: + int wsize_; //size of the window + int max_patch_size_; //max size of the smaller patches + int num_channels_; //the number of feature channels + float min_valid_small_patch_depth_; //percentage of valid depth in a small patch + public: + + FeatureHandlerDepthAverage() + { + wsize_ = 80; + max_patch_size_ = 40; + num_channels_ = 1; + min_valid_small_patch_depth_ = 0.5f; + } + + /** \brief Sets the size of the window to extract features. + * \param[in] w Window size. + */ + void setWSize(int w) + { + wsize_ = w; + } + + /** \brief Sets the number of channels a feature has (i.e. 1 - depth, 4 - depth + normals) + * \param[in] nf Number of channels. + */ + void setNumChannels(int nf) + { + num_channels_ = nf; + } + + /** \brief Create a set of random tests to evaluate examples. + * \param[in] w Number features to generate. + */ + void setMaxPatchSize(int w) + { + max_patch_size_ = w; + } + + /** \brief Create a set of random tests to evaluate examples. + * \param[in] num_of_features Number features to generated. + * \param[out] features Generated features. + */ + /*void createRandomFeatures(const std::size_t num_of_features, std::vector & features) + { + srand (time(NULL)); + int min_s = 10; + float range_d = 0.03f; + for (std::size_t i = 0; i < num_of_features; i++) + { + FT f; + + f.row1_ = rand () % (wsize_ - max_patch_size_ - 1); + f.col1_ = rand () % (wsize_ / 2 - max_patch_size_ - 1); + f.wsizex1_ = min_s + (rand () % (max_patch_size_ - min_s - 1)); + f.wsizey1_ = min_s + (rand () % (max_patch_size_ - min_s - 1)); + + f.row2_ = rand () % (wsize_ - max_patch_size_ - 1); + f.col2_ = wsize_ / 2 + rand () % (wsize_ / 2 - max_patch_size_ - 1); + f.wsizex2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s)); + f.wsizey2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s)); + + f.used_ii_ = 0; + if(num_channels_ > 1) + f.used_ii_ = rand() % num_channels_; + + f.threshold_ = -range_d + (rand () / static_cast (RAND_MAX)) * (range_d * 2.f); + features.push_back (f); + } + }*/ + + void createRandomFeatures(const std::size_t num_of_features, std::vector & features) override + { + srand (static_cast(time (nullptr))); + int min_s = 20; + float range_d = 0.05f; + float incr_d = 0.01f; + + std::vector < FT > windows_and_functions; + + for (std::size_t i = 0; i < num_of_features; i++) + { + FT f; + + f.row1_ = rand () % (wsize_ - max_patch_size_ - 1); + f.col1_ = rand () % (wsize_ / 2 - max_patch_size_ - 1); + f.wsizex1_ = min_s + (rand () % (max_patch_size_ - min_s - 1)); + f.wsizey1_ = min_s + (rand () % (max_patch_size_ - min_s - 1)); + + f.row2_ = rand () % (wsize_ - max_patch_size_ - 1); + f.col2_ = wsize_ / 2 + rand () % (wsize_ / 2 - max_patch_size_ - 1); + f.wsizex2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s)); + f.wsizey2_ = min_s + (rand () % (max_patch_size_ - 1 - min_s)); + + f.used_ii_ = 0; + if (num_channels_ > 1) + f.used_ii_ = rand () % num_channels_; + + windows_and_functions.push_back (f); + } + + for (std::size_t i = 0; i < windows_and_functions.size (); i++) + { + FT f = windows_and_functions[i]; + for (std::size_t j = 0; j <= 10; j++) + { + f.threshold_ = -range_d + static_cast (j) * incr_d; + features.push_back (f); + } + } + } + + /** \brief Evaluates a feature on the specified set of examples. + * \param[in] feature The feature to evaluate. + * \param[in] data_set The data set on which the feature is evaluated. + * \param[in] examples The set of examples of the data set the feature is evaluated on. + * \param[out] results The destination for the results of the feature evaluation. + * \param[out] flags Flags that are supplied together with the results. + */ + void evaluateFeature(const FT & feature, DataSet & data_set, std::vector & examples, std::vector & results, + std::vector & flags) const override + { + results.resize (examples.size ()); + for (std::size_t i = 0; i < examples.size (); i++) + { + evaluateFeature (feature, data_set, examples[i], results[i], flags[i]); + } + } + + /** \brief Evaluates a feature on the specified example. + * \param[in] feature The feature to evaluate. + * \param[in] data_set The data set on which the feature is evaluated. + * \param[in] example The example of the data set the feature is evaluated on. + * \param[out] result The destination for the result of the feature evaluation. + * \param[out] flag Flags that are supplied together with the results. + */ + void evaluateFeature(const FT & feature, DataSet & data_set, const ExampleIndex & example, float & result, unsigned char & flag) const override + { + TrainingExample te = data_set[example]; + int el_f1 = te.iimages_[feature.used_ii_]->getFiniteElementsCount (te.col_ + feature.col1_, te.row_ + feature.row1_, feature.wsizex1_, + feature.wsizey1_); + int el_f2 = te.iimages_[feature.used_ii_]->getFiniteElementsCount (te.col_ + feature.col2_, te.row_ + feature.row2_, feature.wsizex2_, + feature.wsizey2_); + + float sum_f1 = static_cast(te.iimages_[feature.used_ii_]->getFirstOrderSum (te.col_ + feature.col1_, te.row_ + feature.row1_, feature.wsizex1_, feature.wsizey1_)); + float sum_f2 = static_cast(te.iimages_[feature.used_ii_]->getFirstOrderSum (te.col_ + feature.col2_, te.row_ + feature.row2_, feature.wsizex2_, feature.wsizey2_)); + + float f = min_valid_small_patch_depth_; + if (el_f1 == 0 || el_f2 == 0 || (el_f1 <= static_cast (f * static_cast(feature.wsizex1_ * feature.wsizey1_))) + || (el_f2 <= static_cast (f * static_cast(feature.wsizex2_ * feature.wsizey2_)))) + { + result = static_cast (pcl_round (static_cast(rand ()) / static_cast (RAND_MAX))); + flag = 1; + } else + { + result = static_cast ((sum_f1 / static_cast(el_f1) - sum_f2 / static_cast(el_f2)) > feature.threshold_); + flag = 0; + } + + } + + /** \brief Generates evaluation code for the specified feature and writes it to the specified stream. + */ + // param[in] feature The feature for which code is generated. + // param[out] stream The destination for the code. + void generateCodeForEvaluation(const FT &/*feature*/, ::std::ostream &/*stream*/) const override + { + + } + }; + + /** \brief Statistics estimator for regression trees which optimizes information gain and pose parameters error. */ + template + class PoseClassRegressionVarianceStatsEstimator: public pcl::StatsEstimator + { + + public: + /** \brief Constructor. */ + PoseClassRegressionVarianceStatsEstimator(BranchEstimator * branch_estimator) : + branch_estimator_ (branch_estimator) + { + } + + /** \brief Destructor. */ + ~PoseClassRegressionVarianceStatsEstimator() + { + } + + /** \brief Returns the number of branches the corresponding tree has. */ + inline std::size_t getNumOfBranches() const override + { + return branch_estimator_->getNumOfBranches (); + } + + /** \brief Returns the label of the specified node. + * \param[in] node The node which label is returned. + */ + inline LabelDataType getLabelOfNode(NodeType & node) const override + { + return node.value; + } + + /** \brief Computes the covariance matrix for translation offsets. + * \param[in] data_set The corresponding data set. + * \param[in] examples A set of examples from the dataset. + * \param[out] covariance_matrix The covariance matrix. + * \param[out] centroid The mean of the data. + */ + inline unsigned int computeMeanAndCovarianceOffset(DataSet & data_set, std::vector & examples, Eigen::Matrix3d & covariance_matrix, + Eigen::Vector3d & centroid) const + { + Eigen::Matrix accu = Eigen::Matrix::Zero (); + unsigned int point_count = static_cast (examples.size ()); + + for (std::size_t i = 0; i < point_count; ++i) + { + TrainingExample te = data_set[examples[i]]; + accu[0] += te.trans_[0] * te.trans_[0]; + accu[1] += te.trans_[0] * te.trans_[1]; + accu[2] += te.trans_[0] * te.trans_[2]; + accu[3] += te.trans_[1] * te.trans_[1]; + accu[4] += te.trans_[1] * te.trans_[2]; + accu[5] += te.trans_[2] * te.trans_[2]; + accu[6] += te.trans_[0]; + accu[7] += te.trans_[1]; + accu[8] += te.trans_[2]; + } + + if (point_count != 0) + { + accu /= static_cast (point_count); + centroid.head<3> ().matrix () = accu.tail<3> (); + covariance_matrix.coeffRef (0) = accu[0] - accu[6] * accu[6]; + covariance_matrix.coeffRef (1) = accu[1] - accu[6] * accu[7]; + covariance_matrix.coeffRef (2) = accu[2] - accu[6] * accu[8]; + covariance_matrix.coeffRef (4) = accu[3] - accu[7] * accu[7]; + covariance_matrix.coeffRef (5) = accu[4] - accu[7] * accu[8]; + covariance_matrix.coeffRef (8) = accu[5] - accu[8] * accu[8]; + covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); + covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); + covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); + } + + return point_count; + } + + /** \brief Computes the covariance matrix for rotation values. + * \param[in] data_set The corresponding data set. + * \param[in] examples A set of examples from the dataset. + * \param[out] covariance_matrix The covariance matrix. + * \param[out] centroid The mean of the data. + */ + inline unsigned int computeMeanAndCovarianceAngles(DataSet & data_set, std::vector & examples, Eigen::Matrix3d & covariance_matrix, + Eigen::Vector3d & centroid) const + { + Eigen::Matrix accu = Eigen::Matrix::Zero (); + unsigned int point_count = static_cast (examples.size ()); + + for (std::size_t i = 0; i < point_count; ++i) + { + TrainingExample te = data_set[examples[i]]; + accu[0] += te.rot_[0] * te.rot_[0]; + accu[1] += te.rot_[0] * te.rot_[1]; + accu[2] += te.rot_[0] * te.rot_[2]; + accu[3] += te.rot_[1] * te.rot_[1]; + accu[4] += te.rot_[1] * te.rot_[2]; + accu[5] += te.rot_[2] * te.rot_[2]; + accu[6] += te.rot_[0]; + accu[7] += te.rot_[1]; + accu[8] += te.rot_[2]; + } + + if (point_count != 0) + { + accu /= static_cast (point_count); + centroid.head<3> ().matrix () = accu.tail<3> (); + covariance_matrix.coeffRef (0) = accu[0] - accu[6] * accu[6]; + covariance_matrix.coeffRef (1) = accu[1] - accu[6] * accu[7]; + covariance_matrix.coeffRef (2) = accu[2] - accu[6] * accu[8]; + covariance_matrix.coeffRef (4) = accu[3] - accu[7] * accu[7]; + covariance_matrix.coeffRef (5) = accu[4] - accu[7] * accu[8]; + covariance_matrix.coeffRef (8) = accu[5] - accu[8] * accu[8]; + covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); + covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); + covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); + } + + return point_count; + } + + /** \brief Computes the information gain obtained by the specified threshold. + * \param[in] data_set The data set corresponding to the supplied result data. + * \param[in] examples The examples used for extracting the supplied result data. + * \param[in] label_data The label data corresponding to the specified examples. + * \param[in] results The results computed using the specified examples. + * \param[in] flags The flags corresponding to the results. + * \param[in] threshold The threshold for which the information gain is computed. + */ + float computeInformationGain(DataSet & data_set, std::vector & examples, std::vector & label_data, + std::vector & results, std::vector & flags, const float threshold) const override + { + const std::size_t num_of_examples = examples.size (); + const std::size_t num_of_branches = getNumOfBranches (); + + // compute variance + std::vector < LabelDataType > sums (num_of_branches + 1, 0.f); + std::vector < LabelDataType > sqr_sums (num_of_branches + 1, 0.f); + std::vector < std::size_t > branch_element_count (num_of_branches + 1, 0.f); + + for (std::size_t branch_index = 0; branch_index < num_of_branches; ++branch_index) + { + branch_element_count[branch_index] = 1; + ++branch_element_count[num_of_branches]; + } + + for (std::size_t example_index = 0; example_index < num_of_examples; ++example_index) + { + unsigned char branch_index; + computeBranchIndex (results[example_index], flags[example_index], threshold, branch_index); + + LabelDataType label = label_data[example_index]; + + ++branch_element_count[branch_index]; + ++branch_element_count[num_of_branches]; + + sums[branch_index] += label; + sums[num_of_branches] += label; + } + + std::vector hp (num_of_branches + 1, 0.f); + for (std::size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index) + { + float pf = sums[branch_index] / static_cast (branch_element_count[branch_index]); + float pnf = (static_cast(branch_element_count[branch_index]) - sums[branch_index] + 1.f) + / static_cast (branch_element_count[branch_index]); + hp[branch_index] -= static_cast(pf * std::log (pf) + pnf * std::log (pnf)); + } + + //use mean of the examples as purity + float purity = sums[num_of_branches] / static_cast(branch_element_count[num_of_branches]); + float tp = 0.8f; + + if (purity >= tp) + { + //compute covariance matrices from translation offsets and angles for the whole set and children + //consider only positive examples... + std::vector < std::size_t > branch_element_count (num_of_branches + 1, 0); + std::vector < std::vector > positive_examples; + positive_examples.resize (num_of_branches + 1); + + std::size_t pos = 0; + for (std::size_t example_index = 0; example_index < num_of_examples; ++example_index) + { + unsigned char branch_index; + computeBranchIndex (results[example_index], flags[example_index], threshold, branch_index); + + LabelDataType label = label_data[example_index]; + + if (label == 1 /*&& !flags[example_index]*/) + { + ++branch_element_count[branch_index]; + ++branch_element_count[num_of_branches]; + + positive_examples[branch_index].push_back (examples[example_index]); + positive_examples[num_of_branches].push_back (examples[example_index]); + pos++; + } + } + + //compute covariance from offsets and angles for all branchs + std::vector < Eigen::Matrix3d > offset_covariances; + std::vector < Eigen::Matrix3d > angle_covariances; + + std::vector < Eigen::Vector3d > offset_centroids; + std::vector < Eigen::Vector3d > angle_centroids; + + offset_covariances.resize (num_of_branches + 1); + angle_covariances.resize (num_of_branches + 1); + offset_centroids.resize (num_of_branches + 1); + angle_centroids.resize (num_of_branches + 1); + + for (std::size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index) + { + computeMeanAndCovarianceOffset (data_set, positive_examples[branch_index], offset_covariances[branch_index], + offset_centroids[branch_index]); + computeMeanAndCovarianceAngles (data_set, positive_examples[branch_index], angle_covariances[branch_index], + angle_centroids[branch_index]); + } + + //update information_gain + std::vector hr (num_of_branches + 1, 0.f); + for (std::size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index) + { + hr[branch_index] = static_cast(0.5f * std::log (std::pow (2 * M_PI, 3) + * offset_covariances[branch_index].determinant ()) + + 0.5f * std::log (std::pow (2 * M_PI, 3) + * angle_covariances[branch_index].determinant ())); + } + + for (std::size_t branch_index = 0; branch_index < (num_of_branches + 1); ++branch_index) + { + hp[branch_index] += std::max (sums[branch_index] / static_cast (branch_element_count[branch_index]) - tp, 0.f) * hr[branch_index]; + } + } + + float information_gain = hp[num_of_branches + 1]; + for (std::size_t branch_index = 0; branch_index < (num_of_branches); ++branch_index) + { + information_gain -= static_cast (branch_element_count[branch_index]) / static_cast (branch_element_count[num_of_branches]) + * hp[branch_index]; + } + + return information_gain; + } + + /** \brief Computes the branch indices for all supplied results. + * \param[in] results The results the branch indices will be computed for. + * \param[in] flags The flags corresponding to the specified results. + * \param[in] threshold The threshold used to compute the branch indices. + * \param[out] branch_indices The destination for the computed branch indices. + */ + void computeBranchIndices(std::vector & results, std::vector & flags, const float threshold, + std::vector & branch_indices) const override + { + const std::size_t num_of_results = results.size (); + + branch_indices.resize (num_of_results); + for (std::size_t result_index = 0; result_index < num_of_results; ++result_index) + { + unsigned char branch_index; + computeBranchIndex (results[result_index], flags[result_index], threshold, branch_index); + branch_indices[result_index] = branch_index; + } + } + + /** \brief Computes the branch index for the specified result. + * \param[in] result The result the branch index will be computed for. + * \param[in] flag The flag corresponding to the specified result. + * \param[in] threshold The threshold used to compute the branch index. + * \param[out] branch_index The destination for the computed branch index. + */ + inline void computeBranchIndex(const float result, const unsigned char flag, const float threshold, unsigned char & branch_index) const override + { + branch_estimator_->computeBranchIndex (result, flag, threshold, branch_index); + } + + /** \brief Computes and sets the statistics for a node. + * \param[in] data_set The data set which is evaluated. + * \param[in] examples The examples which define which parts of the data set are used for evaluation. + * \param[in] label_data The label_data corresponding to the examples. + * \param[out] node The destination node for the statistics. + */ + void computeAndSetNodeStats(DataSet & data_set, std::vector & examples, std::vector & label_data, NodeType & node) const override + { + const std::size_t num_of_examples = examples.size (); + + LabelDataType sum = 0.0f; + LabelDataType sqr_sum = 0.0f; + for (std::size_t example_index = 0; example_index < num_of_examples; ++example_index) + { + const LabelDataType label = label_data[example_index]; + + sum += label; + sqr_sum += label * label; + } + + sum /= static_cast(num_of_examples); + sqr_sum /= static_cast(num_of_examples); + + const float variance = sqr_sum - sum * sum; + + node.value = sum; + node.variance = variance; + + //set node stats regarding pose regression + std::vector < ExampleIndex > positive_examples; + + for (std::size_t example_index = 0; example_index < num_of_examples; ++example_index) + { + LabelDataType label = label_data[example_index]; + + if (label == 1) + positive_examples.push_back (examples[example_index]); + + } + + //compute covariance from offsets and angles + computeMeanAndCovarianceOffset (data_set, positive_examples, node.covariance_trans_, node.trans_mean_); + computeMeanAndCovarianceAngles (data_set, positive_examples, node.covariance_rot_, node.rot_mean_); + } + + /** \brief Generates code for branch index computation. + * \param[out] stream The destination for the generated code. + */ + // param[in] node The node for which code is generated. + void generateCodeForBranchIndexComputation(NodeType & /*node*/, std::ostream & stream) const override + { + stream << "ERROR: RegressionVarianceStatsEstimator does not implement generateCodeForBranchIndex(...)"; + } + + /** \brief Generates code for label output. + * \param[out] stream The destination for the generated code. + */ + // param[in] node The node for which code is generated. + void generateCodeForOutput(NodeType & /*node*/, std::ostream & stream) const override + { + stream << "ERROR: RegressionVarianceStatsEstimator does not implement generateCodeForBranchIndex(...)"; + } + + private: + /** \brief The branch estimator. */ + pcl::BranchEstimator * branch_estimator_; + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/hv/greedy_verification.h b/deps_install/include/pcl-1.10/pcl/recognition/hv/greedy_verification.h new file mode 100755 index 0000000..a06bb68 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/hv/greedy_verification.h @@ -0,0 +1,185 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include + +#include + +namespace pcl +{ + + /** + * \brief A greedy hypothesis verification method + * \author Aitor Aldoma + */ + + template + class PCL_EXPORTS GreedyVerification : public HypothesisVerification + { + using HypothesisVerification::mask_; + using HypothesisVerification::scene_cloud_downsampled_; + using HypothesisVerification::scene_downsampled_tree_; + using HypothesisVerification::visible_models_; + using HypothesisVerification::resolution_; + using HypothesisVerification::inliers_threshold_; + + /* + * \brief Recognition model using during the verification + */ + class RecognitionModel + { + public: + std::vector explained_; + typename pcl::PointCloud::Ptr cloud_; + int bad_information_; + int good_information_; + int id_; + float regularizer_; + }; + + using RecognitionModelPtr = std::shared_ptr; + + /* + * \brief Sorts recognition models based on the number of explained scene points and visible outliers + */ + struct sortModelsClass + { + bool + operator() (const RecognitionModelPtr & n1, const RecognitionModelPtr & n2) + { + float val1 = static_cast(n1->good_information_) - static_cast(n1->bad_information_) * n1->regularizer_; + float val2 = static_cast(n2->good_information_) - static_cast(n2->bad_information_) * n2->regularizer_; + return val1 > val2; + } + } sortModelsOp; + + + /* + * \brief Recognition model indices to keep track of the sorted recognition hypotheses + */ + struct modelIndices + { + int index_; + RecognitionModelPtr model_; + }; + + /* + * \brief Sorts model indices similar to sortModelsClass + */ + struct sortModelIndicesClass + { + bool + operator() (const modelIndices & n1, const modelIndices & n2) + { + float val1 = static_cast(n1.model_->good_information_) - static_cast(n1.model_->bad_information_) * n1.model_->regularizer_; + float val2 = static_cast(n2.model_->good_information_) - static_cast(n2.model_->bad_information_) * n2.model_->regularizer_; + return val1 > val2; + } + } sortModelsIndicesOp; + + /** \brief Recognition model and indices */ + std::vector indices_models_; + + /** \brief Recognition models (hypotheses to be verified) */ + std::vector recognition_models_; + + /** \brief Recognition models that explain a scene points. */ + std::vector> points_explained_by_rm_; + + /** \brief Weighting for outliers */ + float regularizer_; + + /** \brief Initialize the data structures */ + void + initialize (); + + /** \brief Sorts the hypotheses for the greedy approach */ + void + sortModels () + { + indices_models_.clear (); + for (std::size_t i = 0; i < recognition_models_.size (); i++) + { + modelIndices mi; + mi.index_ = static_cast (i); + mi.model_ = recognition_models_[i]; + indices_models_.push_back (mi); + } + + std::sort (indices_models_.begin (), indices_models_.end (), sortModelsIndicesOp); + //sort also recognition models + std::sort (recognition_models_.begin (), recognition_models_.end (), sortModelsOp); + } + + /** \brief Updates conflicting recognition hypotheses when a hypothesis is accepted */ + void + updateGoodInformation (int i) + { + for (std::size_t k = 0; k < recognition_models_[i]->explained_.size (); k++) + { + //update good_information_ for all hypotheses that were explaining the same points as hypothesis i + for (std::size_t kk = 0; kk < points_explained_by_rm_[recognition_models_[i]->explained_[k]].size (); kk++) + { + (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->good_information_--; + (points_explained_by_rm_[recognition_models_[i]->explained_[k]])[kk]->bad_information_++; + } + } + } + + public: + + /** \brief Constructor + * \param[in] reg Regularizer value + **/ + GreedyVerification (float reg = 1.5f) : + HypothesisVerification () + { + regularizer_ = reg; + } + + /** \brief Starts verification */ + void + verify () override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/hv/hv_go.h b/deps_install/include/pcl-1.10/pcl/recognition/hv/hv_go.h new file mode 100755 index 0000000..e39f3eb --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/hv/hv_go.h @@ -0,0 +1,505 @@ +/* + * go.h + * + * Created on: Jun 4, 2012 + * Author: aitor + */ + +#pragma once + +#include + +#include +#include + +//includes required by mets.hh +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace pcl +{ + + /** \brief A hypothesis verification method proposed in + * "A Global Hypotheses Verification Method for 3D Object Recognition", A. Aldoma and F. Tombari and L. Di Stefano and Markus Vincze, ECCV 2012 + * \author Aitor Aldoma + */ + template + class PCL_EXPORTS GlobalHypothesesVerification: public HypothesisVerification + { + private: + + //Helper classes + struct RecognitionModel + { + public: + std::vector explained_; //indices vector referencing explained_by_RM_ + std::vector explained_distances_; //closest distances to the scene for point i + std::vector unexplained_in_neighborhood; //indices vector referencing unexplained_by_RM_neighboorhods + std::vector unexplained_in_neighborhood_weights; //weights for the points not being explained in the neighborhood of a hypothesis + std::vector outlier_indices_; //outlier indices of this model + std::vector complete_cloud_occupancy_indices_; + typename pcl::PointCloud::Ptr cloud_; + typename pcl::PointCloud::Ptr complete_cloud_; + int bad_information_; + float outliers_weight_; + pcl::PointCloud::Ptr normals_; + int id_; + }; + + using RecognitionModelPtr = std::shared_ptr; + + using SAOptimizerT = GlobalHypothesesVerification; + class SAModel: public mets::evaluable_solution + { + public: + std::vector solution_; + SAOptimizerT * opt_; + mets::gol_type cost_; + + //Evaluates the current solution + mets::gol_type cost_function() const override + { + return cost_; + } + + void copy_from(const mets::copyable& o) override + { + const SAModel& s = dynamic_cast (o); + solution_ = s.solution_; + opt_ = s.opt_; + cost_ = s.cost_; + } + + mets::gol_type what_if(int /*index*/, bool /*val*/) const + { + /*std::vector tmp (solution_); + tmp[index] = val; + mets::gol_type sol = opt_->evaluateSolution (solution_, index); //evaluate without updating status + return sol;*/ + return static_cast(0); + } + + mets::gol_type apply_and_evaluate(int index, bool val) + { + solution_[index] = val; + mets::gol_type sol = opt_->evaluateSolution (solution_, index); //this will update the state of the solution + cost_ = sol; + return sol; + } + + void apply(int /*index*/, bool /*val*/) + { + + } + + void unapply(int index, bool val) + { + solution_[index] = val; + //update optimizer solution + cost_ = opt_->evaluateSolution (solution_, index); //this will update the cost function in opt_ + } + void setSolution(std::vector & sol) + { + solution_ = sol; + } + + void setOptimizer(SAOptimizerT * opt) + { + opt_ = opt; + } + }; + + /* + * Represents a move, deactivate a hypothesis + */ + + class move: public mets::move + { + int index_; + public: + move(int i) : + index_ (i) + { + } + + mets::gol_type evaluate(const mets::feasible_solution& /*cs*/) const override + { + return static_cast(0); + } + + mets::gol_type apply_and_evaluate(mets::feasible_solution& cs) + { + SAModel& model = dynamic_cast (cs); + return model.apply_and_evaluate (index_, !model.solution_[index_]); + } + + void apply(mets::feasible_solution& /*s*/) const override + { + } + + void unapply(mets::feasible_solution& s) const + { + SAModel& model = dynamic_cast (s); + model.unapply (index_, !model.solution_[index_]); + } + }; + + class move_manager + { + public: + std::vector moves_m; + using iterator = typename std::vector::iterator; + iterator begin() + { + return moves_m.begin (); + } + iterator end() + { + return moves_m.end (); + } + + move_manager(int problem_size) + { + for (int ii = 0; ii != problem_size; ++ii) + moves_m.push_back (new move (ii)); + } + + ~move_manager() + { + // delete all moves + for (iterator ii = begin (); ii != end (); ++ii) + delete (*ii); + } + + void refresh(mets::feasible_solution& /*s*/) + { + std::shuffle (moves_m.begin (), moves_m.end (), std::mt19937(std::random_device()())); + } + + }; + + //inherited class attributes + using HypothesisVerification::mask_; + using HypothesisVerification::scene_cloud_downsampled_; + using HypothesisVerification::scene_downsampled_tree_; + using HypothesisVerification::visible_models_; + using HypothesisVerification::complete_models_; + using HypothesisVerification::resolution_; + using HypothesisVerification::inliers_threshold_; + + //class attributes + using NormalEstimator_ = pcl::NormalEstimation; + pcl::PointCloud::Ptr scene_normals_; + pcl::PointCloud::Ptr clusters_cloud_; + + std::vector complete_cloud_occupancy_by_RM_; + float res_occupancy_grid_; + float w_occupied_multiple_cm_; + + std::vector explained_by_RM_; //represents the points of scene_cloud_ that are explained by the recognition models + std::vector explained_by_RM_distance_weighted; //represents the points of scene_cloud_ that are explained by the recognition models + std::vector unexplained_by_RM_neighboorhods; //represents the points of scene_cloud_ that are not explained by the active hypotheses in the neighboorhod of the recognition models + std::vector recognition_models_; + std::vector indices_; + + float regularizer_; + float clutter_regularizer_; + bool detect_clutter_; + float radius_neighborhood_GO_; + float radius_normals_; + + float previous_explained_value; + int previous_duplicity_; + int previous_duplicity_complete_models_; + float previous_bad_info_; + float previous_unexplained_; + + int max_iterations_; //max iterations without improvement + SAModel best_seen_; + float initial_temp_; + + int n_cc_; + std::vector > cc_; + + void setPreviousBadInfo(float f) + { + previous_bad_info_ = f; + } + + float getPreviousBadInfo() + { + return previous_bad_info_; + } + + void setPreviousExplainedValue(float v) + { + previous_explained_value = v; + } + + void setPreviousDuplicity(int v) + { + previous_duplicity_ = v; + } + + void setPreviousDuplicityCM(int v) + { + previous_duplicity_complete_models_ = v; + } + + void setPreviousUnexplainedValue(float v) + { + previous_unexplained_ = v; + } + + float getPreviousUnexplainedValue() + { + return previous_unexplained_; + } + + float getExplainedValue() + { + return previous_explained_value; + } + + int getDuplicity() + { + return previous_duplicity_; + } + + int getDuplicityCM() + { + return previous_duplicity_complete_models_; + } + + void updateUnexplainedVector(std::vector & unexplained_, std::vector & unexplained_distances, std::vector & unexplained_by_RM, + std::vector & explained, std::vector & explained_by_RM, float val) + { + { + + float add_to_unexplained = 0.f; + + for (std::size_t i = 0; i < unexplained_.size (); i++) + { + + bool prev_unexplained = (unexplained_by_RM[unexplained_[i]] > 0) && (explained_by_RM[unexplained_[i]] == 0); + unexplained_by_RM[unexplained_[i]] += val * unexplained_distances[i]; + + if (val < 0) //the hypothesis is being removed + { + if (prev_unexplained) + { + //decrease by 1 + add_to_unexplained -= unexplained_distances[i]; + } + } else //the hypothesis is being added and unexplains unexplained_[i], so increase by 1 unless its explained by another hypothesis + { + if (explained_by_RM[unexplained_[i]] == 0) + add_to_unexplained += unexplained_distances[i]; + } + } + + for (const int &i : explained) + { + if (val < 0) + { + //the hypothesis is being removed, check that there are no points that become unexplained and have clutter unexplained hypotheses + if ((explained_by_RM[i] == 0) && (unexplained_by_RM[i] > 0)) + { + add_to_unexplained += unexplained_by_RM[i]; //the points become unexplained + } + } else + { + //std::cout << "being added..." << add_to_unexplained << " " << unexplained_by_RM[explained[i]] << std::endl; + if ((explained_by_RM[i] == 1) && (unexplained_by_RM[i] > 0)) + { //the only hypothesis explaining that point + add_to_unexplained -= unexplained_by_RM[i]; //the points are not unexplained any longer because this hypothesis explains them + } + } + } + + //std::cout << add_to_unexplained << std::endl; + previous_unexplained_ += add_to_unexplained; + } + } + + void updateExplainedVector(std::vector & vec, std::vector & vec_float, std::vector & explained_, + std::vector & explained_by_RM_distance_weighted, float sign) + { + float add_to_explained = 0.f; + int add_to_duplicity_ = 0; + + for (std::size_t i = 0; i < vec.size (); i++) + { + bool prev_dup = explained_[vec[i]] > 1; + + explained_[vec[i]] += static_cast (sign); + explained_by_RM_distance_weighted[vec[i]] += vec_float[i] * sign; + + add_to_explained += vec_float[i] * sign; + + if ((explained_[vec[i]] > 1) && prev_dup) + { //its still a duplicate, we are adding + add_to_duplicity_ += static_cast (sign); //so, just add or remove one + } else if ((explained_[vec[i]] == 1) && prev_dup) + { //if was duplicate before, now its not, remove 2, we are removing the hypothesis + add_to_duplicity_ -= 2; + } else if ((explained_[vec[i]] > 1) && !prev_dup) + { //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point + add_to_duplicity_ += 2; + } + } + + //update explained and duplicity values... + previous_explained_value += add_to_explained; + previous_duplicity_ += add_to_duplicity_; + } + + void updateCMDuplicity(std::vector & vec, std::vector & occupancy_vec, float sign) { + int add_to_duplicity_ = 0; + for (const int &i : vec) + { + bool prev_dup = occupancy_vec[i] > 1; + occupancy_vec[i] += static_cast (sign); + if ((occupancy_vec[i] > 1) && prev_dup) + { //its still a duplicate, we are adding + add_to_duplicity_ += static_cast (sign); //so, just add or remove one + } else if ((occupancy_vec[i] == 1) && prev_dup) + { //if was duplicate before, now its not, remove 2, we are removing the hypothesis + add_to_duplicity_ -= 2; + } else if ((occupancy_vec[i] > 1) && !prev_dup) + { //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point + add_to_duplicity_ += 2; + } + } + + previous_duplicity_complete_models_ += add_to_duplicity_; + } + + float getTotalExplainedInformation(std::vector & explained_, std::vector & explained_by_RM_distance_weighted, int * duplicity_) + { + float explained_info = 0; + int duplicity = 0; + + for (std::size_t i = 0; i < explained_.size (); i++) + { + if (explained_[i] > 0) + explained_info += explained_by_RM_distance_weighted[i]; + + if (explained_[i] > 1) + duplicity += explained_[i]; + } + + *duplicity_ = duplicity; + + return explained_info; + } + + float getTotalBadInformation(std::vector & recog_models) + { + float bad_info = 0; + for (std::size_t i = 0; i < recog_models.size (); i++) + bad_info += recog_models[i]->outliers_weight_ * static_cast (recog_models[i]->bad_information_); + + return bad_info; + } + + float getUnexplainedInformationInNeighborhood(std::vector & unexplained, std::vector & explained) + { + float unexplained_sum = 0.f; + for (std::size_t i = 0; i < unexplained.size (); i++) + { + if (unexplained[i] > 0 && explained[i] == 0) + unexplained_sum += unexplained[i]; + } + + return unexplained_sum; + } + + //Performs smooth segmentation of the scene cloud and compute the model cues + void + initialize(); + + mets::gol_type + evaluateSolution(const std::vector & active, int changed); + + bool + addModel(typename pcl::PointCloud::ConstPtr & model, typename pcl::PointCloud::ConstPtr & complete_model, RecognitionModelPtr & recog_model); + + void + computeClutterCue(RecognitionModelPtr & recog_model); + + void + SAOptimize(std::vector & cc_indices, std::vector & sub_solution); + + public: + GlobalHypothesesVerification() : HypothesisVerification() + { + resolution_ = 0.005f; + max_iterations_ = 5000; + regularizer_ = 1.f; + radius_normals_ = 0.01f; + initial_temp_ = 1000; + detect_clutter_ = true; + radius_neighborhood_GO_ = 0.03f; + clutter_regularizer_ = 5.f; + res_occupancy_grid_ = 0.01f; + w_occupied_multiple_cm_ = 4.f; + } + + void + verify() override; + + void setResolutionOccupancyGrid(float r) + { + res_occupancy_grid_ = r; + } + + void setRadiusNormals(float r) + { + radius_normals_ = r; + } + + void setMaxIterations(int i) + { + max_iterations_ = i; + } + + void setInitialTemp(float t) + { + initial_temp_ = t; + } + + void setRegularizer(float r) + { + regularizer_ = r; + } + + void setRadiusClutter(float r) + { + radius_neighborhood_GO_ = r; + } + + void setClutterRegularizer(float cr) + { + clutter_regularizer_ = cr; + } + + void setDetectClutter(bool d) + { + detect_clutter_ = d; + } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/hv/hv_papazov.h b/deps_install/include/pcl-1.10/pcl/recognition/hv/hv_papazov.h new file mode 100755 index 0000000..d24f1c7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/hv/hv_papazov.h @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace pcl +{ + + /** \brief A hypothesis verification method proposed in + * "An Efficient RANSAC for 3D Object Recognition in Noisy and Occluded Scenes", C. Papazov and D. Burschka, ACCV 2010 + * \author Aitor Aldoma, Federico Tombari + */ + + template + class PCL_EXPORTS PapazovHV : public HypothesisVerification + { + using HypothesisVerification::mask_; + using HypothesisVerification::scene_cloud_downsampled_; + using HypothesisVerification::scene_downsampled_tree_; + using HypothesisVerification::visible_models_; + using HypothesisVerification::complete_models_; + using HypothesisVerification::resolution_; + using HypothesisVerification::inliers_threshold_; + + float conflict_threshold_size_; + float penalty_threshold_; + float support_threshold_; + + class RecognitionModel + { + public: + std::vector explained_; //indices vector referencing explained_by_RM_ + typename pcl::PointCloud::Ptr cloud_; + typename pcl::PointCloud::Ptr complete_cloud_; + int bad_information_; + int id_; + }; + + using RecognitionModelPtr = std::shared_ptr; + + std::vector explained_by_RM_; //represents the points of scene_cloud_ that are explained by the recognition models + std::vector recognition_models_; + std::vector> points_explained_by_rm_; //if inner size > 1, conflict + std::map graph_id_model_map_; + + using Graph = boost::adjacency_list; + Graph conflict_graph_; + + //builds the conflict_graph + void buildConflictGraph(); + //non-maxima suppresion on the conflict graph + void nonMaximaSuppresion(); + //create recognition models + void initialize(); + + public: + PapazovHV() : HypothesisVerification() { + support_threshold_ = 0.1f; + penalty_threshold_ = 0.1f; + conflict_threshold_size_ = 0.02f; + } + + void setConflictThreshold(float t) { + conflict_threshold_size_ = t; + } + + void setSupportThreshold(float t) { + support_threshold_ = t; + } + + void setPenaltyThreshold(float t) { + penalty_threshold_ = t; + } + + //build conflict graph + //non-maxima supression + void verify() override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/hv/hypotheses_verification.h b/deps_install/include/pcl-1.10/pcl/recognition/hv/hypotheses_verification.h new file mode 100755 index 0000000..9a7bf80 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/hv/hypotheses_verification.h @@ -0,0 +1,330 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include "pcl/recognition/hv/occlusion_reasoning.h" +#include "pcl/recognition/impl/hv/occlusion_reasoning.hpp" +#include +#include +#include + +namespace pcl +{ + + /** + * \brief Abstract class for hypotheses verification methods + * \author Aitor Aldoma, Federico Tombari + */ + + template + class PCL_EXPORTS HypothesisVerification + { + + protected: + /* + * \brief Boolean vector indicating if a hypothesis is accepted/rejected (output of HV stage) + */ + std::vector mask_; + /* + * \brief Scene point cloud + */ + typename pcl::PointCloud::ConstPtr scene_cloud_; + + /* + * \brief Scene point cloud + */ + typename pcl::PointCloud::ConstPtr occlusion_cloud_; + + bool occlusion_cloud_set_; + + /* + * \brief Downsampled scene point cloud + */ + typename pcl::PointCloud::Ptr scene_cloud_downsampled_; + + /* + * \brief Scene tree of the downsampled cloud + */ + typename pcl::search::KdTree::Ptr scene_downsampled_tree_; + + /* + * \brief Vector of point clouds representing the 3D models after occlusion reasoning + * the 3D models are pruned of occluded points, and only visible points are left. + * the coordinate system is that of the scene cloud + */ + typename std::vector::ConstPtr> visible_models_; + + std::vector::ConstPtr> visible_normal_models_; + /* + * \brief Vector of point clouds representing the complete 3D model (in same coordinates as the scene cloud) + */ + typename std::vector::ConstPtr> complete_models_; + + std::vector::ConstPtr> complete_normal_models_; + /* + * \brief Resolutions in pixel for the depth scene buffer + */ + int zbuffer_scene_resolution_; + /* + * \brief Resolutions in pixel for the depth model self-occlusion buffer + */ + int zbuffer_self_occlusion_resolution_; + /* + * \brief The resolution of models and scene used to verify hypotheses (in meters) + */ + float resolution_; + + /* + * \brief Threshold for inliers + */ + float inliers_threshold_; + + /* + * \brief Threshold to consider a point being occluded + */ + float occlusion_thres_; + + /* + * \brief Whether the HV method requires normals or not, by default = false + */ + bool requires_normals_; + + /* + * \brief Whether the normals have been set + */ + bool normals_set_; + public: + + HypothesisVerification () + { + zbuffer_scene_resolution_ = 100; + zbuffer_self_occlusion_resolution_ = 150; + resolution_ = 0.005f; + inliers_threshold_ = static_cast(resolution_); + occlusion_cloud_set_ = false; + occlusion_thres_ = 0.005f; + normals_set_ = false; + requires_normals_ = false; + } + + virtual + ~HypothesisVerification() = default; + + bool getRequiresNormals() { + return requires_normals_; + } + + /* + * \brief Sets the resolution of scene cloud and models used to verify hypotheses + * mask r resolution + */ + void + setResolution(float r) { + resolution_ = r; + } + + /* + * \brief Sets the occlusion threshold + * mask t threshold + */ + void + setOcclusionThreshold(float t) { + occlusion_thres_ = t; + } + + /* + * \brief Sets the resolution of scene cloud and models used to verify hypotheses + * mask r resolution + */ + void + setInlierThreshold(float r) { + inliers_threshold_ = r; + } + + /* + * \brief Returns a vector of booleans representing which hypotheses have been accepted/rejected (true/false) + * mask vector of booleans + */ + + void + getMask (std::vector & mask) + { + mask = mask_; + } + + /* + * \brief Sets the 3D complete models. NOTE: If addModels is called with occlusion_reasoning=true, then + * there is no need to call this function. + * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_) + */ + + void + addCompleteModels (std::vector::ConstPtr> & complete_models) + { + complete_models_ = complete_models; + } + + /* + * \brief Sets the normals of the 3D complete models and sets normals_set_ to true. + * Normals need to be added before calling the addModels method. + * complete_models The normals of the models. + */ + void + addNormalsClouds (std::vector::ConstPtr> & complete_models) + { + complete_normal_models_ = complete_models; + normals_set_ = true; + } + + /* + * \brief Sets the models (recognition hypotheses) - requires the scene_cloud_ to be set first if reasoning about occlusions + * mask models Vector of point clouds representing the models (in same coordinates as the scene_cloud_) + */ + void + addModels (std::vector::ConstPtr> & models, bool occlusion_reasoning = false) + { + + mask_.clear(); + if(!occlusion_cloud_set_) { + PCL_WARN("Occlusion cloud not set, using scene_cloud instead...\n"); + occlusion_cloud_ = scene_cloud_; + } + + if (!occlusion_reasoning) + visible_models_ = models; + else + { + //we need to reason about occlusions before setting the model + if (scene_cloud_ == nullptr) + { + PCL_ERROR("setSceneCloud should be called before adding the model if reasoning about occlusions..."); + } + + if (!occlusion_cloud_->isOrganized ()) + { + PCL_WARN("Scene not organized... filtering using computed depth buffer\n"); + } + + pcl::occlusion_reasoning::ZBuffering zbuffer_scene (zbuffer_scene_resolution_, zbuffer_scene_resolution_, 1.f); + if (!occlusion_cloud_->isOrganized ()) + { + zbuffer_scene.computeDepthMap (occlusion_cloud_, true); + } + + for (std::size_t i = 0; i < models.size (); i++) + { + + //self-occlusions + typename pcl::PointCloud::Ptr filtered (new pcl::PointCloud ()); + typename pcl::occlusion_reasoning::ZBuffering zbuffer_self_occlusion (75, 75, 1.f); + zbuffer_self_occlusion.computeDepthMap (models[i], true); + std::vector indices; + zbuffer_self_occlusion.filter (models[i], indices, 0.005f); + pcl::copyPointCloud (*models[i], indices, *filtered); + + if(normals_set_ && requires_normals_) { + pcl::PointCloud::Ptr filtered_normals (new pcl::PointCloud ()); + pcl::copyPointCloud(*complete_normal_models_[i], indices, *filtered_normals); + visible_normal_models_.push_back(filtered_normals); + } + + typename pcl::PointCloud::ConstPtr const_filtered(new pcl::PointCloud (*filtered)); + //typename pcl::PointCloud::ConstPtr const_filtered(new pcl::PointCloud (*models[i])); + //scene-occlusions + if (occlusion_cloud_->isOrganized ()) + { + filtered = pcl::occlusion_reasoning::filter (occlusion_cloud_, const_filtered, 525.f, occlusion_thres_); + } + else + { + zbuffer_scene.filter (const_filtered, filtered, occlusion_thres_); + } + + visible_models_.push_back (filtered); + } + + complete_models_ = models; + } + + occlusion_cloud_set_ = false; + normals_set_ = false; + } + + /* + * \brief Sets the scene cloud + * scene_cloud Point cloud representing the scene + */ + + void + setSceneCloud (const typename pcl::PointCloud::Ptr & scene_cloud) + { + + complete_models_.clear(); + visible_models_.clear(); + visible_normal_models_.clear(); + + scene_cloud_ = scene_cloud; + scene_cloud_downsampled_.reset(new pcl::PointCloud()); + + pcl::VoxelGrid voxel_grid; + voxel_grid.setInputCloud (scene_cloud); + voxel_grid.setLeafSize (resolution_, resolution_, resolution_); + voxel_grid.setDownsampleAllData(true); + voxel_grid.filter (*scene_cloud_downsampled_); + + //initialize kdtree for search + scene_downsampled_tree_.reset (new pcl::search::KdTree); + scene_downsampled_tree_->setInputCloud(scene_cloud_downsampled_); + } + + void setOcclusionCloud (const typename pcl::PointCloud::Ptr & occ_cloud) + { + occlusion_cloud_ = occ_cloud; + occlusion_cloud_set_ = true; + } + + /* + * \brief Function that performs the hypotheses verification, needs to be implemented in the subclasses + * This function modifies the values of mask_ and needs to be called after both scene and model have been added + */ + + virtual void + verify ()=0; + }; + +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/hv/occlusion_reasoning.h b/deps_install/include/pcl-1.10/pcl/recognition/hv/occlusion_reasoning.h new file mode 100755 index 0000000..43f0654 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/hv/occlusion_reasoning.h @@ -0,0 +1,215 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + + namespace occlusion_reasoning + { + /** + * \brief Class to reason about occlusions + * \author Aitor Aldoma + */ + + template + class ZBuffering + { + private: + float f_; + int cx_, cy_; + float * depth_; + + public: + + ZBuffering (); + ZBuffering (int resx, int resy, float f); + ~ZBuffering (); + void + computeDepthMap (typename pcl::PointCloud::ConstPtr & scene, bool compute_focal = false, bool smooth = false, int wsize = 3); + void + filter (typename pcl::PointCloud::ConstPtr & model, typename pcl::PointCloud::Ptr & filtered, float thres = 0.01); + void filter (typename pcl::PointCloud::ConstPtr & model, std::vector & indices, float thres = 0.01); + }; + + template typename pcl::PointCloud::Ptr + filter (typename pcl::PointCloud::ConstPtr & organized_cloud, typename pcl::PointCloud::ConstPtr & to_be_filtered, float f, + float threshold) + { + float cx = (static_cast (organized_cloud->width) / 2.f - 0.5f); + float cy = (static_cast (organized_cloud->height) / 2.f - 0.5f); + typename pcl::PointCloud::Ptr filtered (new pcl::PointCloud ()); + + std::vector indices_to_keep; + indices_to_keep.resize (to_be_filtered->points.size ()); + + int keep = 0; + for (std::size_t i = 0; i < to_be_filtered->points.size (); i++) + { + float x = to_be_filtered->points[i].x; + float y = to_be_filtered->points[i].y; + float z = to_be_filtered->points[i].z; + int u = static_cast (f * x / z + cx); + int v = static_cast (f * y / z + cy); + + //Not out of bounds + if ((u >= static_cast (organized_cloud->width)) || (v >= static_cast (organized_cloud->height)) || (u < 0) || (v < 0)) + continue; + + //Check for invalid depth + if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y) + || !std::isfinite (organized_cloud->at (u, v).z)) + continue; + + float z_oc = organized_cloud->at (u, v).z; + + //Check if point depth (distance to camera) is greater than the (u,v) + if ((z - z_oc) > threshold) + continue; + + indices_to_keep[keep] = static_cast (i); + keep++; + } + + indices_to_keep.resize (keep); + pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered); + return filtered; + } + + template typename pcl::PointCloud::Ptr + filter (typename pcl::PointCloud::Ptr & organized_cloud, typename pcl::PointCloud::Ptr & to_be_filtered, float f, + float threshold, bool check_invalid_depth = true) + { + float cx = (static_cast (organized_cloud->width) / 2.f - 0.5f); + float cy = (static_cast (organized_cloud->height) / 2.f - 0.5f); + typename pcl::PointCloud::Ptr filtered (new pcl::PointCloud ()); + + std::vector indices_to_keep; + indices_to_keep.resize (to_be_filtered->points.size ()); + + int keep = 0; + for (std::size_t i = 0; i < to_be_filtered->points.size (); i++) + { + float x = to_be_filtered->points[i].x; + float y = to_be_filtered->points[i].y; + float z = to_be_filtered->points[i].z; + int u = static_cast (f * x / z + cx); + int v = static_cast (f * y / z + cy); + + //Not out of bounds + if ((u >= static_cast (organized_cloud->width)) || (v >= static_cast (organized_cloud->height)) || (u < 0) || (v < 0)) + continue; + + //Check for invalid depth + if (check_invalid_depth) + { + if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y) + || !std::isfinite (organized_cloud->at (u, v).z)) + continue; + } + + float z_oc = organized_cloud->at (u, v).z; + + //Check if point depth (distance to camera) is greater than the (u,v) + if ((z - z_oc) > threshold) + continue; + + indices_to_keep[keep] = static_cast (i); + keep++; + } + + indices_to_keep.resize (keep); + pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered); + return filtered; + } + + template typename pcl::PointCloud::Ptr + getOccludedCloud (typename pcl::PointCloud::Ptr & organized_cloud, typename pcl::PointCloud::Ptr & to_be_filtered, float f, + float threshold, bool check_invalid_depth = true) + { + float cx = (static_cast (organized_cloud->width) / 2.f - 0.5f); + float cy = (static_cast (organized_cloud->height) / 2.f - 0.5f); + typename pcl::PointCloud::Ptr filtered (new pcl::PointCloud ()); + + std::vector indices_to_keep; + indices_to_keep.resize (to_be_filtered->points.size ()); + + int keep = 0; + for (std::size_t i = 0; i < to_be_filtered->points.size (); i++) + { + float x = to_be_filtered->points[i].x; + float y = to_be_filtered->points[i].y; + float z = to_be_filtered->points[i].z; + int u = static_cast (f * x / z + cx); + int v = static_cast (f * y / z + cy); + + //Out of bounds + if ((u >= static_cast (organized_cloud->width)) || (v >= static_cast (organized_cloud->height)) || (u < 0) || (v < 0)) + continue; + + //Check for invalid depth + if (check_invalid_depth) + { + if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y) + || !std::isfinite (organized_cloud->at (u, v).z)) + continue; + } + + float z_oc = organized_cloud->at (u, v).z; + + //Check if point depth (distance to camera) is greater than the (u,v) + if ((z - z_oc) > threshold) + { + indices_to_keep[keep] = static_cast (i); + keep++; + } + } + + indices_to_keep.resize (keep); + pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered); + return filtered; + } + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/recognition/hypothesis.h b/deps_install/include/pcl-1.10/pcl/recognition/hypothesis.h new file mode 100755 index 0000000..e4dc596 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/hypothesis.h @@ -0,0 +1,157 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * hypothesis.h + * + * Created on: Mar 12, 2013 + * Author: papazov + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace recognition + { + class HypothesisBase + { + public: + HypothesisBase (const ModelLibrary::Model* obj_model) + : obj_model_ (obj_model) + {} + + HypothesisBase (const ModelLibrary::Model* obj_model, const float* rigid_transform) + : obj_model_ (obj_model) + { + memcpy (rigid_transform_, rigid_transform, 12*sizeof (float)); + } + + virtual ~HypothesisBase (){} + + void + setModel (const ModelLibrary::Model* model) + { + obj_model_ = model; + } + + public: + float rigid_transform_[12]; + const ModelLibrary::Model* obj_model_; + }; + + class Hypothesis: public HypothesisBase + { + public: + Hypothesis (const ModelLibrary::Model* obj_model = nullptr) + : HypothesisBase (obj_model), + match_confidence_ (-1.0f), + linear_id_ (-1) + { + } + + Hypothesis (const Hypothesis& src) + : HypothesisBase (src.obj_model_, src.rigid_transform_), + match_confidence_ (src.match_confidence_), + explained_pixels_ (src.explained_pixels_) + { + } + + ~Hypothesis (){} + + const Hypothesis& + operator =(const Hypothesis& src) + { + memcpy (this->rigid_transform_, src.rigid_transform_, 12*sizeof (float)); + this->obj_model_ = src.obj_model_; + this->match_confidence_ = src.match_confidence_; + this->explained_pixels_ = src.explained_pixels_; + + return *this; + } + + void + setLinearId (int id) + { + linear_id_ = id; + } + + int + getLinearId () const + { + return (linear_id_); + } + + void + computeBounds (float bounds[6]) const + { + const float *b = obj_model_->getBoundsOfOctreePoints (); + float p[3]; + + // Initialize 'bounds' + aux::transform (rigid_transform_, b[0], b[2], b[4], p); + bounds[0] = bounds[1] = p[0]; + bounds[2] = bounds[3] = p[1]; + bounds[4] = bounds[5] = p[2]; + + // Expand 'bounds' to contain the other 7 points of the octree bounding box + aux::transform (rigid_transform_, b[0], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[0], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[0], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[2], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + } + + void + computeCenterOfMass (float center_of_mass[3]) const + { + aux::transform (rigid_transform_, obj_model_->getOctreeCenterOfMass (), center_of_mass); + } + + public: + float match_confidence_; + std::set explained_pixels_; + int linear_id_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/correspondence_grouping.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/correspondence_grouping.hpp new file mode 100755 index 0000000..8df7502 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/correspondence_grouping.hpp @@ -0,0 +1,60 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_ +#define PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_ + +template void +pcl::CorrespondenceGrouping::cluster (std::vector &clustered_corrs) +{ + clustered_corrs.clear (); + corr_group_scale_.clear (); + + if (!initCompute ()) + { + return; + } + + //Perform the actual clustering + clusterCorrespondences (clustered_corrs); + + deinitCompute (); +} + +#endif // PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/geometric_consistency.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/geometric_consistency.hpp new file mode 100755 index 0000000..a181296 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/geometric_consistency.hpp @@ -0,0 +1,183 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_ +#define PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +inline bool +gcCorrespSorter (pcl::Correspondence i, pcl::Correspondence j) +{ + return (i.distance < j.distance); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GeometricConsistencyGrouping::clusterCorrespondences (std::vector &model_instances) +{ + model_instances.clear (); + found_transformations_.clear (); + + if (!model_scene_corrs_) + { + PCL_ERROR( + "[pcl::GeometricConsistencyGrouping::clusterCorrespondences()] Error! Correspondences not set, please set them before calling again this function.\n"); + return; + } + + CorrespondencesPtr sorted_corrs (new Correspondences (*model_scene_corrs_)); + + std::sort (sorted_corrs->begin (), sorted_corrs->end (), gcCorrespSorter); + + model_scene_corrs_ = sorted_corrs; + + std::vector consensus_set; + std::vector taken_corresps (model_scene_corrs_->size (), false); + + Eigen::Vector3f dist_ref, dist_trg; + + //temp copy of scene cloud with the type cast to ModelT in order to use Ransac + PointCloudPtr temp_scene_cloud_ptr (new PointCloud ()); + pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr); + + pcl::registration::CorrespondenceRejectorSampleConsensus corr_rejector; + corr_rejector.setMaximumIterations (10000); + corr_rejector.setInlierThreshold (gc_size_); + corr_rejector.setInputSource(input_); + corr_rejector.setInputTarget (temp_scene_cloud_ptr); + + for (std::size_t i = 0; i < model_scene_corrs_->size (); ++i) + { + if (taken_corresps[i]) + continue; + + consensus_set.clear (); + consensus_set.push_back (static_cast (i)); + + for (std::size_t j = 0; j < model_scene_corrs_->size (); ++j) + { + if ( j != i && !taken_corresps[j]) + { + //Let's check if j fits into the current consensus set + bool is_a_good_candidate = true; + for (const int &k : consensus_set) + { + int scene_index_k = model_scene_corrs_->at (k).index_match; + int model_index_k = model_scene_corrs_->at (k).index_query; + int scene_index_j = model_scene_corrs_->at (j).index_match; + int model_index_j = model_scene_corrs_->at (j).index_query; + + const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap (); + const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap (); + const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap (); + const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap (); + + dist_ref = scene_point_k - scene_point_j; + dist_trg = model_point_k - model_point_j; + + double distance = std::abs (dist_ref.norm () - dist_trg.norm ()); + + if (distance > gc_size_) + { + is_a_good_candidate = false; + break; + } + } + + if (is_a_good_candidate) + consensus_set.push_back (static_cast (j)); + } + } + + if (static_cast (consensus_set.size ()) > gc_threshold_) + { + Correspondences temp_corrs, filtered_corrs; + for (const int &j : consensus_set) + { + temp_corrs.push_back (model_scene_corrs_->at (j)); + taken_corresps[ j ] = true; + } + //ransac filtering + corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs); + //save transformations for recognize + found_transformations_.push_back (corr_rejector.getBestTransformation ()); + + model_instances.push_back (filtered_corrs); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::GeometricConsistencyGrouping::recognize ( + std::vector > &transformations) +{ + std::vector model_instances; + return (this->recognize (transformations, model_instances)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::GeometricConsistencyGrouping::recognize ( + std::vector > &transformations, std::vector &clustered_corrs) +{ + transformations.clear (); + if (!this->initCompute ()) + { + PCL_ERROR( + "[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n"); + return (false); + } + + clusterCorrespondences (clustered_corrs); + + transformations = found_transformations_; + + this->deinitCompute (); + return (true); +} + +#define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping; + +#endif // PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/hough_3d.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/hough_3d.hpp new file mode 100755 index 0000000..ce0d4b6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/cg/hough_3d.hpp @@ -0,0 +1,374 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id:$ + * + */ + +#ifndef PCL_RECOGNITION_HOUGH_3D_IMPL_H_ +#define PCL_RECOGNITION_HOUGH_3D_IMPL_H_ + +#include +#include +#include +//#include +//#include +#include +#include + + +template +template void +pcl::Hough3DGrouping::computeRf (const typename pcl::PointCloud::ConstPtr &input, pcl::PointCloud &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 (hough_bin_size_); + } + pcl::PointCloud::Ptr normal_cloud (new pcl::PointCloud ()); + NormalEstimation 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 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); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Hough3DGrouping::train () +{ + if (!input_) + { + PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud not set.\n"); + return (false); + } + + if (!input_rf_) + { + ModelRfCloudPtr new_input_rf (new ModelRfCloud ()); + computeRf (input_, *new_input_rf); + input_rf_ = new_input_rf; + //PCL_ERROR( + // "[pcl::Hough3DGrouping::train()] Error! Input reference frame not set.\n"); + //return (false); + } + + if (input_->size () != input_rf_->size ()) + { + PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud size != Input RF cloud size.\n"); + return (false); + } + + model_votes_.clear (); + model_votes_.resize (input_->size ()); + + // compute model centroid + Eigen::Vector3f centroid (0, 0, 0); + for (std::size_t i = 0; i < input_->size (); ++i) + { + centroid += input_->at (i).getVector3fMap (); + } + centroid /= static_cast (input_->size ()); + + // compute model votes + for (std::size_t i = 0; i < input_->size (); ++i) + { + Eigen::Vector3f x_ax ((*input_rf_)[i].x_axis[0], (*input_rf_)[i].x_axis[1], (*input_rf_)[i].x_axis[2]); + Eigen::Vector3f y_ax ((*input_rf_)[i].y_axis[0], (*input_rf_)[i].y_axis[1], (*input_rf_)[i].y_axis[2]); + Eigen::Vector3f z_ax ((*input_rf_)[i].z_axis[0], (*input_rf_)[i].z_axis[1], (*input_rf_)[i].z_axis[2]); + + model_votes_[i].x () = x_ax.dot (centroid - input_->at (i).getVector3fMap ()); + model_votes_[i].y () = y_ax.dot (centroid - input_->at (i).getVector3fMap ()); + model_votes_[i].z () = z_ax.dot (centroid - input_->at (i).getVector3fMap ()); + } + + needs_training_ = false; + return (true); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Hough3DGrouping::houghVoting () +{ + if (needs_training_) + { + if (!train ())//checks input and input_rf + return (false); + } + + //if (!scene_) + //{ + // PCL_ERROR( + // "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud not set.\n"); + // return (false); + //} + + if (!scene_rf_) + { + ModelRfCloudPtr new_scene_rf (new ModelRfCloud ()); + computeRf (scene_, *new_scene_rf); + scene_rf_ = new_scene_rf; + //PCL_ERROR( + // "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene reference frame not set.\n"); + //return (false); + } + + if (scene_->size () != scene_rf_->size ()) + { + PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud size != Scene RF cloud size.\n"); + return (false); + } + + if (!model_scene_corrs_) + { + PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Correspondences not set, please set them before calling again this function.\n"); + return (false); + } + + int n_matches = static_cast (model_scene_corrs_->size ()); + if (n_matches == 0) + { + return (false); + } + + std::vector > scene_votes (n_matches); + Eigen::Vector3d d_min, d_max, bin_size; + + d_min.setConstant (std::numeric_limits::max ()); + d_max.setConstant (-std::numeric_limits::max ()); + bin_size.setConstant (hough_bin_size_); + + float max_distance = -std::numeric_limits::max (); + + // Calculating 3D Hough space dimensions and vote position for each match + for (int i=0; i< n_matches; ++i) + { + int scene_index = model_scene_corrs_->at (i).index_match; + int model_index = model_scene_corrs_->at (i).index_query; + + const Eigen::Vector3f& scene_point = scene_->at (scene_index).getVector3fMap (); + const PointSceneRfT& scene_point_rf = scene_rf_->at (scene_index); + + Eigen::Vector3f scene_point_rf_x (scene_point_rf.x_axis[0], scene_point_rf.x_axis[1], scene_point_rf.x_axis[2]); + Eigen::Vector3f scene_point_rf_y (scene_point_rf.y_axis[0], scene_point_rf.y_axis[1], scene_point_rf.y_axis[2]); + Eigen::Vector3f scene_point_rf_z (scene_point_rf.z_axis[0], scene_point_rf.z_axis[1], scene_point_rf.z_axis[2]); + + //const Eigen::Vector3f& model_point = input_->at (model_index).getVector3fMap (); + const Eigen::Vector3f& model_point_vote = model_votes_[model_index]; + + scene_votes[i].x () = scene_point_rf_x[0] * model_point_vote.x () + scene_point_rf_y[0] * model_point_vote.y () + scene_point_rf_z[0] * model_point_vote.z () + scene_point.x (); + scene_votes[i].y () = scene_point_rf_x[1] * model_point_vote.x () + scene_point_rf_y[1] * model_point_vote.y () + scene_point_rf_z[1] * model_point_vote.z () + scene_point.y (); + scene_votes[i].z () = scene_point_rf_x[2] * model_point_vote.x () + scene_point_rf_y[2] * model_point_vote.y () + scene_point_rf_z[2] * model_point_vote.z () + scene_point.z (); + + if (scene_votes[i].x () < d_min.x ()) + d_min.x () = scene_votes[i].x (); + if (scene_votes[i].x () > d_max.x ()) + d_max.x () = scene_votes[i].x (); + + if (scene_votes[i].y () < d_min.y ()) + d_min.y () = scene_votes[i].y (); + if (scene_votes[i].y () > d_max.y ()) + d_max.y () = scene_votes[i].y (); + + if (scene_votes[i].z () < d_min.z ()) + d_min.z () = scene_votes[i].z (); + if (scene_votes[i].z () > d_max.z ()) + d_max.z () = scene_votes[i].z (); + + // Calculate max distance for interpolated votes + if (use_interpolation_ && max_distance < model_scene_corrs_->at (i).distance) + { + max_distance = model_scene_corrs_->at (i).distance; + } + } + + // Hough Voting + hough_space_.reset (new pcl::recognition::HoughSpace3D (d_min, bin_size, d_max)); + + for (int i = 0; i < n_matches; ++i) + { + double weight = 1.0; + if (use_distance_weight_ && max_distance != 0) + { + weight = 1.0 - (model_scene_corrs_->at (i).distance / max_distance); + } + if (use_interpolation_) + { + hough_space_->voteInt (scene_votes[i], weight, i); + } + else + { + hough_space_->vote (scene_votes[i], weight, i); + } + } + + hough_space_initialized_ = true; + + return (true); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::Hough3DGrouping::clusterCorrespondences (std::vector &model_instances) +{ + model_instances.clear (); + found_transformations_.clear (); + + if (!hough_space_initialized_ && !houghVoting ()) + { + return; + } + + // Finding max bins and voters + std::vector max_values; + std::vector > max_ids; + + hough_space_->findMaxima (hough_threshold_, max_values, max_ids); + + // Insert maximas into result vector, after Ransac correspondence rejection + // Temp copy of scene cloud with the type cast to ModelT in order to use Ransac + PointCloudPtr temp_scene_cloud_ptr (new PointCloud); + pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr); + + pcl::registration::CorrespondenceRejectorSampleConsensus corr_rejector; + corr_rejector.setMaximumIterations (10000); + corr_rejector.setInlierThreshold (hough_bin_size_); + corr_rejector.setInputSource (input_); + corr_rejector.setInputTarget (temp_scene_cloud_ptr); + + for (std::size_t j = 0; j < max_values.size (); ++j) + { + Correspondences temp_corrs, filtered_corrs; + for (const int &i : max_ids[j]) + { + temp_corrs.push_back (model_scene_corrs_->at (i)); + } + // RANSAC filtering + corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs); + // Save transformations for recognize + found_transformations_.push_back (corr_rejector.getBestTransformation ()); + + model_instances.push_back (filtered_corrs); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//template bool +//pcl::Hough3DGrouping::getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform) +//{ +// std::vector model_indices; +// std::vector scene_indices; +// pcl::registration::getQueryIndices (corrs, model_indices); +// pcl::registration::getMatchIndices (corrs, scene_indices); +// +// typename pcl::SampleConsensusModelRegistration::Ptr model (new pcl::SampleConsensusModelRegistration (input_, model_indices)); +// model->setInputTarget (scene_cloud, scene_indices); +// +// pcl::RandomSampleConsensus ransac (model); +// ransac.setDistanceThreshold (hough_bin_size_); +// ransac.setMaxIterations (10000); +// if (!ransac.computeModel ()) +// return (false); +// +// // Transform model coefficients from vectorXf to matrix4f +// Eigen::VectorXf coeffs; +// ransac.getModelCoefficients (coeffs); +// +// transform.row (0) = coeffs.segment<4> (0); +// transform.row (1) = coeffs.segment<4> (4); +// transform.row (2) = coeffs.segment<4> (8); +// transform.row (3) = coeffs.segment<4> (12); +// +// return (true); +//} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Hough3DGrouping::recognize ( + std::vector > &transformations) +{ + std::vector model_instances; + return (this->recognize (transformations, model_instances)); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Hough3DGrouping::recognize ( + std::vector > &transformations, std::vector &clustered_corrs) +{ + transformations.clear (); + if (!this->initCompute ()) + { + PCL_ERROR ("[pcl::Hough3DGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n"); + return (false); + } + + clusterCorrespondences (clustered_corrs); + + transformations = found_transformations_; + + //// Temp copy of scene cloud with the type cast to ModelT in order to use Ransac + //PointCloudPtr temp_scene_cloud_ptr (new PointCloud); + //pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr); + + //for (std::size_t i = 0; i < model_instances.size (); ++i) + //{ + // Eigen::Matrix4f curr_transf; + // if (getTransformMatrix (temp_scene_cloud_ptr, model_instances[i], curr_transf)) + // transformations.push_back (curr_transf); + //} + + this->deinitCompute (); + return (transformations.size ()); +} + + +#define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping; + +#endif // PCL_RECOGNITION_HOUGH_3D_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/greedy_verification.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/greedy_verification.hpp new file mode 100755 index 0000000..7f3e49c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/greedy_verification.hpp @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once +#include + +template + void + pcl::GreedyVerification::initialize () + { + //clear stuff + recognition_models_.clear (); + points_explained_by_rm_.clear (); + + // initialize mask... + mask_.resize (visible_models_.size ()); + for (std::size_t i = 0; i < visible_models_.size (); i++) + mask_[i] = false; + + // initialize explained_by_RM + points_explained_by_rm_.resize (scene_cloud_downsampled_->points.size ()); + + // initialize model + for (std::size_t m = 0; m < visible_models_.size (); m++) + { + RecognitionModelPtr recog_model (new RecognitionModel); + // voxelize model cloud + recog_model->cloud_.reset (new pcl::PointCloud); + recog_model->id_ = static_cast (m); + + pcl::VoxelGrid voxel_grid; + voxel_grid.setInputCloud (visible_models_[m]); + voxel_grid.setLeafSize (resolution_, resolution_, resolution_); + voxel_grid.filter (*(recog_model->cloud_)); + + std::vector explained_indices; + std::vector outliers; + std::vector nn_indices; + std::vector nn_distances; + + for (std::size_t i = 0; i < recog_model->cloud_->points.size (); i++) + { + if (!scene_downsampled_tree_->radiusSearch (recog_model->cloud_->points[i], inliers_threshold_, nn_indices, nn_distances, + std::numeric_limits::max ())) + { + outliers.push_back (static_cast (i)); + } + else + { + for (std::size_t k = 0; k < nn_distances.size (); k++) + { + explained_indices.push_back (nn_indices[k]); //nn_indices[k] points to the scene + } + } + } + + std::sort (explained_indices.begin (), explained_indices.end ()); + explained_indices.erase (std::unique (explained_indices.begin (), explained_indices.end ()), explained_indices.end ()); + + recog_model->bad_information_ = static_cast (outliers.size ()); + recog_model->explained_ = explained_indices; + recog_model->good_information_ = static_cast (explained_indices.size ()); + recog_model->regularizer_ = regularizer_; + + recognition_models_.push_back (recog_model); + + for (const int &explained_index : explained_indices) + { + points_explained_by_rm_[explained_index].push_back (recog_model); + } + } + + sortModels (); + } + +template + void + pcl::GreedyVerification::verify () + { + initialize (); + + std::vector best_solution_; + best_solution_.resize (recognition_models_.size ()); + + for (std::size_t i = 0; i < recognition_models_.size (); i++) + { + if (static_cast (recognition_models_[i]->good_information_) > (regularizer_ + * static_cast (recognition_models_[i]->bad_information_))) + { + best_solution_[i] = true; + updateGoodInformation (static_cast (i)); + } + else + best_solution_[i] = false; + } + + for (std::size_t i = 0; i < best_solution_.size (); i++) + { + if (best_solution_[i]) + { + mask_[indices_models_[i].index_] = true; + } + else + { + mask_[indices_models_[i].index_] = false; + } + } + } + +#define PCL_INSTANTIATE_GreedyVerification(T1,T2) template class PCL_EXPORTS pcl::GreedyVerification; diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/hv_go.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/hv_go.hpp new file mode 100755 index 0000000..143fe57 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/hv_go.hpp @@ -0,0 +1,745 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012 Aitor Aldoma, Federico Tombari + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_RECOGNITION_IMPL_HV_GO_HPP_ +#define PCL_RECOGNITION_IMPL_HV_GO_HPP_ + +#include +#include +#include + +#include +#include + +template +inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud &cloud, const typename pcl::PointCloud &normals, float tolerance, + const typename pcl::search::Search::Ptr &tree, std::vector &clusters, double eps_angle, float curvature_threshold, + unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()) +{ + + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset\n"); + return; + } + if (cloud.points.size () != normals.points.size ()) + { + PCL_ERROR("[pcl::extractEuclideanClusters] Number of points in the input point cloud different than normals!\n"); + return; + } + + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + // Process all points in the indices vector + int size = static_cast (cloud.points.size ()); + for (int i = 0; i < size; ++i) + { + if (processed[i]) + continue; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (i); + + processed[i] = true; + + while (sq_idx < static_cast (seed_queue.size ())) + { + + if (normals.points[seed_queue[sq_idx]].curvature > curvature_threshold) + { + sq_idx++; + continue; + } + + // Search for sq_idx + if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) + { + sq_idx++; + continue; + } + + for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + if (normals.points[nn_indices[j]].curvature > curvature_threshold) + { + continue; + } + + //processed[nn_indices[j]] = true; + // [-1;1] + + double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0] + + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] + + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2]; + + if (std::abs (std::acos (dot_p)) < eps_angle) + { + processed[nn_indices[j]] = true; + seed_queue.push_back (nn_indices[j]); + } + } + + sq_idx++; + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + pcl::PointIndices r; + r.indices.resize (seed_queue.size ()); + for (std::size_t j = 0; j < seed_queue.size (); ++j) + r.indices[j] = seed_queue[j]; + + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + r.header = cloud.header; + clusters.push_back (r); // We could avoid a copy by working directly in the vector + } + } +} + +template +mets::gol_type pcl::GlobalHypothesesVerification::evaluateSolution(const std::vector & active, int changed) +{ + float sign = 1.f; + //update explained_by_RM + if (active[changed]) + { + //it has been activated + updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_, + explained_by_RM_distance_weighted, 1.f); + updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights, + unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, 1.f); + updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, 1.f); + } else + { + //it has been deactivated + updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_, + explained_by_RM_distance_weighted, -1.f); + updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights, + unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, -1.f); + updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, -1.f); + sign = -1.f; + } + + int duplicity = getDuplicity (); + float good_info = getExplainedValue (); + + float unexplained_info = getPreviousUnexplainedValue (); + float bad_info = static_cast (getPreviousBadInfo ()) + + (recognition_models_[changed]->outliers_weight_ * static_cast (recognition_models_[changed]->bad_information_)) * sign; + + setPreviousBadInfo (bad_info); + + int n_active_hyp = 0; + for(const bool &i : active) { + if(i) + n_active_hyp++; + } + + float duplicity_cm = static_cast (getDuplicityCM ()) * w_occupied_multiple_cm_; + return static_cast ((good_info - bad_info - static_cast (duplicity) - unexplained_info - duplicity_cm - static_cast (n_active_hyp)) * -1.f); //return the dual to our max problem +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::GlobalHypothesesVerification::initialize() +{ + //clear stuff + recognition_models_.clear (); + unexplained_by_RM_neighboorhods.clear (); + explained_by_RM_distance_weighted.clear (); + explained_by_RM_.clear (); + mask_.clear (); + indices_.clear (), + complete_cloud_occupancy_by_RM_.clear (); + + // initialize mask to false + mask_.resize (complete_models_.size ()); + for (std::size_t i = 0; i < complete_models_.size (); i++) + mask_[i] = false; + + indices_.resize (complete_models_.size ()); + + NormalEstimator_ n3d; + scene_normals_.reset (new pcl::PointCloud ()); + + typename pcl::search::KdTree::Ptr normals_tree (new pcl::search::KdTree); + normals_tree->setInputCloud (scene_cloud_downsampled_); + + n3d.setRadiusSearch (radius_normals_); + n3d.setSearchMethod (normals_tree); + n3d.setInputCloud (scene_cloud_downsampled_); + n3d.compute (*scene_normals_); + + //check nans... + int j = 0; + for (std::size_t i = 0; i < scene_normals_->points.size (); ++i) + { + if (!std::isfinite (scene_normals_->points[i].normal_x) || !std::isfinite (scene_normals_->points[i].normal_y) + || !std::isfinite (scene_normals_->points[i].normal_z)) + continue; + + scene_normals_->points[j] = scene_normals_->points[i]; + scene_cloud_downsampled_->points[j] = scene_cloud_downsampled_->points[i]; + + j++; + } + + scene_normals_->points.resize (j); + scene_normals_->width = j; + scene_normals_->height = 1; + + scene_cloud_downsampled_->points.resize (j); + scene_cloud_downsampled_->width = j; + scene_cloud_downsampled_->height = 1; + + explained_by_RM_.resize (scene_cloud_downsampled_->points.size (), 0); + explained_by_RM_distance_weighted.resize (scene_cloud_downsampled_->points.size (), 0.f); + unexplained_by_RM_neighboorhods.resize (scene_cloud_downsampled_->points.size (), 0.f); + + //compute segmentation of the scene if detect_clutter_ + if (detect_clutter_) + { + //initialize kdtree for search + scene_downsampled_tree_.reset (new pcl::search::KdTree); + scene_downsampled_tree_->setInputCloud (scene_cloud_downsampled_); + + std::vector clusters; + double eps_angle_threshold = 0.2; + int min_points = 20; + float curvature_threshold = 0.045f; + + extractEuclideanClustersSmooth (*scene_cloud_downsampled_, *scene_normals_, inliers_threshold_ * 2.f, scene_downsampled_tree_, + clusters, eps_angle_threshold, curvature_threshold, min_points); + + clusters_cloud_.reset (new pcl::PointCloud); + clusters_cloud_->points.resize (scene_cloud_downsampled_->points.size ()); + clusters_cloud_->width = scene_cloud_downsampled_->width; + clusters_cloud_->height = 1; + + for (std::size_t i = 0; i < scene_cloud_downsampled_->points.size (); i++) + { + pcl::PointXYZI p; + p.getVector3fMap () = scene_cloud_downsampled_->points[i].getVector3fMap (); + p.intensity = 0.f; + clusters_cloud_->points[i] = p; + } + + float intens_incr = 100.f / static_cast (clusters.size ()); + float intens = intens_incr; + for (const auto &cluster : clusters) + { + for (const auto &vertex : cluster.indices) + { + clusters_cloud_->points[vertex].intensity = intens; + } + + intens += intens_incr; + } + } + + //compute cues + { + pcl::ScopeTime tcues ("Computing cues"); + recognition_models_.resize (complete_models_.size ()); + int valid = 0; + for (int i = 0; i < static_cast (complete_models_.size ()); i++) + { + //create recognition model + recognition_models_[valid].reset (new RecognitionModel ()); + if(addModel (visible_models_[i], complete_models_[i], recognition_models_[valid])) { + indices_[valid] = i; + valid++; + } + } + + recognition_models_.resize(valid); + indices_.resize(valid); + } + + //compute the bounding boxes for the models + ModelT min_pt_all, max_pt_all; + min_pt_all.x = min_pt_all.y = min_pt_all.z = std::numeric_limits::max (); + max_pt_all.x = max_pt_all.y = max_pt_all.z = (std::numeric_limits::max () - 0.001f) * -1; + + for (std::size_t i = 0; i < recognition_models_.size (); i++) + { + ModelT min_pt, max_pt; + pcl::getMinMax3D (*complete_models_[indices_[i]], min_pt, max_pt); + if (min_pt.x < min_pt_all.x) + min_pt_all.x = min_pt.x; + + if (min_pt.y < min_pt_all.y) + min_pt_all.y = min_pt.y; + + if (min_pt.z < min_pt_all.z) + min_pt_all.z = min_pt.z; + + if (max_pt.x > max_pt_all.x) + max_pt_all.x = max_pt.x; + + if (max_pt.y > max_pt_all.y) + max_pt_all.y = max_pt.y; + + if (max_pt.z > max_pt_all.z) + max_pt_all.z = max_pt.z; + } + + int size_x, size_y, size_z; + size_x = static_cast (std::ceil (std::abs (max_pt_all.x - min_pt_all.x) / res_occupancy_grid_)) + 1; + size_y = static_cast (std::ceil (std::abs (max_pt_all.y - min_pt_all.y) / res_occupancy_grid_)) + 1; + size_z = static_cast (std::ceil (std::abs (max_pt_all.z - min_pt_all.z) / res_occupancy_grid_)) + 1; + + complete_cloud_occupancy_by_RM_.resize (size_x * size_y * size_z, 0); + + for (std::size_t i = 0; i < recognition_models_.size (); i++) + { + + std::map banned; + std::map::iterator banned_it; + + for (std::size_t j = 0; j < complete_models_[indices_[i]]->points.size (); j++) + { + int pos_x, pos_y, pos_z; + pos_x = static_cast (std::floor ((complete_models_[indices_[i]]->points[j].x - min_pt_all.x) / res_occupancy_grid_)); + pos_y = static_cast (std::floor ((complete_models_[indices_[i]]->points[j].y - min_pt_all.y) / res_occupancy_grid_)); + pos_z = static_cast (std::floor ((complete_models_[indices_[i]]->points[j].z - min_pt_all.z) / res_occupancy_grid_)); + + int idx = pos_z * size_x * size_y + pos_y * size_x + pos_x; + banned_it = banned.find (idx); + if (banned_it == banned.end ()) + { + complete_cloud_occupancy_by_RM_[idx]++; + recognition_models_[i]->complete_cloud_occupancy_indices_.push_back (idx); + banned[idx] = true; + } + } + } + + { + pcl::ScopeTime tcues ("Computing clutter cues"); +#pragma omp parallel for schedule(dynamic, 4) num_threads(omp_get_num_procs()) + for (int j = 0; j < static_cast (recognition_models_.size ()); j++) + computeClutterCue (recognition_models_[j]); + } + + cc_.clear (); + n_cc_ = 1; + cc_.resize (n_cc_); + for (std::size_t i = 0; i < recognition_models_.size (); i++) + cc_[0].push_back (static_cast (i)); + +} + +template +void pcl::GlobalHypothesesVerification::SAOptimize(std::vector & cc_indices, std::vector & initial_solution) +{ + + //temporal copy of recogniton_models_ + std::vector recognition_models_copy; + recognition_models_copy = recognition_models_; + + recognition_models_.clear (); + + for (const int &cc_index : cc_indices) + { + recognition_models_.push_back (recognition_models_copy[cc_index]); + } + + for (std::size_t j = 0; j < recognition_models_.size (); j++) + { + RecognitionModelPtr recog_model = recognition_models_[j]; + for (std::size_t i = 0; i < recog_model->explained_.size (); i++) + { + explained_by_RM_[recog_model->explained_[i]]++; + explained_by_RM_distance_weighted[recog_model->explained_[i]] += recog_model->explained_distances_[i]; + } + + if (detect_clutter_) + { + for (std::size_t i = 0; i < recog_model->unexplained_in_neighborhood.size (); i++) + { + unexplained_by_RM_neighboorhods[recog_model->unexplained_in_neighborhood[i]] += recog_model->unexplained_in_neighborhood_weights[i]; + } + } + } + + int occupied_multiple = 0; + for(std::size_t i=0; i < complete_cloud_occupancy_by_RM_.size(); i++) { + if(complete_cloud_occupancy_by_RM_[i] > 1) { + occupied_multiple+=complete_cloud_occupancy_by_RM_[i]; + } + } + + setPreviousDuplicityCM(occupied_multiple); + //do optimization + //Define model SAModel, initial solution is all models activated + + int duplicity; + float good_information_ = getTotalExplainedInformation (explained_by_RM_, explained_by_RM_distance_weighted, &duplicity); + float bad_information_ = 0; + float unexplained_in_neighboorhod = getUnexplainedInformationInNeighborhood (unexplained_by_RM_neighboorhods, explained_by_RM_); + + for (std::size_t i = 0; i < initial_solution.size (); i++) + { + if (initial_solution[i]) + bad_information_ += recognition_models_[i]->outliers_weight_ * static_cast (recognition_models_[i]->bad_information_); + } + + setPreviousExplainedValue (good_information_); + setPreviousDuplicity (duplicity); + setPreviousBadInfo (bad_information_); + setPreviousUnexplainedValue (unexplained_in_neighboorhod); + + SAModel model; + model.cost_ = static_cast ((good_information_ - bad_information_ + - static_cast (duplicity) + - static_cast (occupied_multiple) * w_occupied_multiple_cm_ + - static_cast (recognition_models_.size ()) + - unexplained_in_neighboorhod) * -1.f); + + model.setSolution (initial_solution); + model.setOptimizer (this); + SAModel best (model); + + move_manager neigh (static_cast (cc_indices.size ())); + + mets::best_ever_solution best_recorder (best); + mets::noimprove_termination_criteria noimprove (max_iterations_); + mets::linear_cooling linear_cooling; + mets::simulated_annealing sa (model, best_recorder, neigh, noimprove, linear_cooling, initial_temp_, 1e-7, 2); + sa.setApplyAndEvaluate(true); + + { + pcl::ScopeTime t ("SA search..."); + sa.search (); + } + + best_seen_ = static_cast (best_recorder.best_seen ()); + for (std::size_t i = 0; i < best_seen_.solution_.size (); i++) + { + initial_solution[i] = best_seen_.solution_[i]; + } + + recognition_models_ = recognition_models_copy; + +} + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::GlobalHypothesesVerification::verify() +{ + initialize (); + + //for each connected component, find the optimal solution + for (int c = 0; c < n_cc_; c++) + { + //TODO: Check for trivial case... + //TODO: Check also the number of hypotheses and use exhaustive enumeration if smaller than 10 + std::vector subsolution (cc_[c].size (), true); + SAOptimize (cc_[c], subsolution); + for (std::size_t i = 0; i < subsolution.size (); i++) + { + mask_[indices_[cc_[c][i]]] = (subsolution[i]); + } + } +} + +template +bool pcl::GlobalHypothesesVerification::addModel(typename pcl::PointCloud::ConstPtr & model, + typename pcl::PointCloud::ConstPtr & complete_model, RecognitionModelPtr & recog_model) +{ + //voxelize model cloud + recog_model->cloud_.reset (new pcl::PointCloud ()); + recog_model->complete_cloud_.reset (new pcl::PointCloud ()); + + float size_model = resolution_; + pcl::VoxelGrid voxel_grid; + voxel_grid.setInputCloud (model); + voxel_grid.setLeafSize (size_model, size_model, size_model); + voxel_grid.filter (*(recog_model->cloud_)); + + pcl::VoxelGrid voxel_grid2; + voxel_grid2.setInputCloud (complete_model); + voxel_grid2.setLeafSize (size_model, size_model, size_model); + voxel_grid2.filter (*(recog_model->complete_cloud_)); + + { + //check nans... + int j = 0; + for (std::size_t i = 0; i < recog_model->cloud_->points.size (); ++i) + { + if (!std::isfinite (recog_model->cloud_->points[i].x) || !std::isfinite (recog_model->cloud_->points[i].y) + || !std::isfinite (recog_model->cloud_->points[i].z)) + continue; + + recog_model->cloud_->points[j] = recog_model->cloud_->points[i]; + j++; + } + + recog_model->cloud_->points.resize (j); + recog_model->cloud_->width = j; + recog_model->cloud_->height = 1; + } + + if (recog_model->cloud_->points.empty ()) + { + PCL_WARN("The model cloud has no points..\n"); + return false; + } + + //compute normals unless given (now do it always...) + typename pcl::search::KdTree::Ptr normals_tree (new pcl::search::KdTree); + pcl::NormalEstimation n3d; + recog_model->normals_.reset (new pcl::PointCloud ()); + normals_tree->setInputCloud (recog_model->cloud_); + n3d.setRadiusSearch (radius_normals_); + n3d.setSearchMethod (normals_tree); + n3d.setInputCloud ((recog_model->cloud_)); + n3d.compute (*(recog_model->normals_)); + + //check nans... + int j = 0; + for (std::size_t i = 0; i < recog_model->normals_->points.size (); ++i) + { + if (!std::isfinite (recog_model->normals_->points[i].normal_x) || !std::isfinite (recog_model->normals_->points[i].normal_y) + || !std::isfinite (recog_model->normals_->points[i].normal_z)) + continue; + + recog_model->normals_->points[j] = recog_model->normals_->points[i]; + recog_model->cloud_->points[j] = recog_model->cloud_->points[i]; + j++; + } + + recog_model->normals_->points.resize (j); + recog_model->normals_->width = j; + recog_model->normals_->height = 1; + + recog_model->cloud_->points.resize (j); + recog_model->cloud_->width = j; + recog_model->cloud_->height = 1; + + std::vector explained_indices; + std::vector outliers_weight; + std::vector explained_indices_distances; + + std::vector nn_indices; + std::vector nn_distances; + + std::map>>> model_explains_scene_points; //which point i from the scene is explained by a points j_k with dist d_k from the model + + outliers_weight.resize (recog_model->cloud_->points.size ()); + recog_model->outlier_indices_.resize (recog_model->cloud_->points.size ()); + + std::size_t o = 0; + for (std::size_t i = 0; i < recog_model->cloud_->points.size (); i++) + { + if (!scene_downsampled_tree_->radiusSearch (recog_model->cloud_->points[i], inliers_threshold_, nn_indices, nn_distances, std::numeric_limits::max ())) + { + //outlier + outliers_weight[o] = regularizer_; + recog_model->outlier_indices_[o] = static_cast (i); + o++; + } else + { + for (std::size_t k = 0; k < nn_distances.size (); k++) + { + std::pair pair = std::make_pair (i, nn_distances[k]); //i is a index to a model point and then distance + auto it = model_explains_scene_points.find (nn_indices[k]); + if (it == model_explains_scene_points.end ()) + { + std::shared_ptr>> vec (new std::vector> ()); + vec->push_back (pair); + model_explains_scene_points[nn_indices[k]] = vec; + } else + { + it->second->push_back (pair); + } + } + } + } + + outliers_weight.resize (o); + recog_model->outlier_indices_.resize (o); + + recog_model->outliers_weight_ = (std::accumulate (outliers_weight.begin (), outliers_weight.end (), 0.f) / static_cast (outliers_weight.size ())); + if (outliers_weight.empty ()) + recog_model->outliers_weight_ = 1.f; + + pcl::IndicesPtr indices_scene (new std::vector); + //go through the map and keep the closest model point in case that several model points explain a scene point + + int p = 0; + + for (auto it = model_explains_scene_points.cbegin (); it != model_explains_scene_points.cend (); it++, p++) + { + std::size_t closest = 0; + float min_d = std::numeric_limits::min (); + for (std::size_t i = 0; i < it->second->size (); i++) + { + if (it->second->at (i).second > min_d) + { + min_d = it->second->at (i).second; + closest = i; + } + } + + float d = it->second->at (closest).second; + float d_weight = -(d * d / (inliers_threshold_)) + 1; + + //it->first is index to scene point + //using normals to weight inliers + Eigen::Vector3f scene_p_normal = scene_normals_->points[it->first].getNormalVector3fMap (); + Eigen::Vector3f model_p_normal = recog_model->normals_->points[it->second->at (closest).first].getNormalVector3fMap (); + float dotp = scene_p_normal.dot (model_p_normal) * 1.f; //[-1,1] from antiparallel trough perpendicular to parallel + + if (dotp < 0.f) + dotp = 0.f; + + explained_indices.push_back (it->first); + explained_indices_distances.push_back (d_weight * dotp); + + } + + recog_model->bad_information_ = static_cast (recog_model->outlier_indices_.size ()); + recog_model->explained_ = explained_indices; + recog_model->explained_distances_ = explained_indices_distances; + + return true; +} + +template +void pcl::GlobalHypothesesVerification::computeClutterCue(RecognitionModelPtr & recog_model) +{ + if (detect_clutter_) + { + + float rn_sqr = radius_neighborhood_GO_ * radius_neighborhood_GO_; + std::vector nn_indices; + std::vector nn_distances; + + std::vector < std::pair > neighborhood_indices; //first is indices to scene point and second is indices to explained_ scene points + for (int i = 0; i < static_cast (recog_model->explained_.size ()); i++) + { + if (scene_downsampled_tree_->radiusSearch (scene_cloud_downsampled_->points[recog_model->explained_[i]], radius_neighborhood_GO_, nn_indices, + nn_distances, std::numeric_limits::max ())) + { + for (std::size_t k = 0; k < nn_distances.size (); k++) + { + if (nn_indices[k] != i) + neighborhood_indices.emplace_back (nn_indices[k], i); + } + } + } + + //sort neighborhood indices by id + std::sort (neighborhood_indices.begin (), neighborhood_indices.end (), + [] (const auto& p1, const auto& p2) { return p1.first < p2.first; }); + + //erase duplicated unexplained points + neighborhood_indices.erase ( + std::unique (neighborhood_indices.begin (), neighborhood_indices.end (), + [] (const auto& p1, const auto& p2) { return p1.first == p2.first; }), neighborhood_indices.end ()); + + //sort explained points + std::vector exp_idces (recog_model->explained_); + std::sort (exp_idces.begin (), exp_idces.end ()); + + recog_model->unexplained_in_neighborhood.resize (neighborhood_indices.size ()); + recog_model->unexplained_in_neighborhood_weights.resize (neighborhood_indices.size ()); + + std::size_t p = 0; + std::size_t j = 0; + for (const auto &neighborhood_index : neighborhood_indices) + { + if ((j < exp_idces.size ()) && (neighborhood_index.first == exp_idces[j])) + { + //this index is explained by the hypothesis so ignore it, advance j + j++; + } else + { + //indices_in_nb[i] < exp_idces[j] + //recog_model->unexplained_in_neighborhood.push_back(neighborhood_indices[i]); + recog_model->unexplained_in_neighborhood[p] = neighborhood_index.first; + + if (clusters_cloud_->points[recog_model->explained_[neighborhood_index.second]].intensity != 0.f + && (clusters_cloud_->points[recog_model->explained_[neighborhood_index.second]].intensity + == clusters_cloud_->points[neighborhood_index.first].intensity)) + { + + recog_model->unexplained_in_neighborhood_weights[p] = clutter_regularizer_; + + } else + { + //neighborhood_indices[i].first gives the index to the scene point and second to the explained scene point by the model causing this... + //calculate weight of this clutter point based on the distance of the scene point and the model point causing it + float d = static_cast (pow ( + (scene_cloud_downsampled_->points[recog_model->explained_[neighborhood_index.second]].getVector3fMap () + - scene_cloud_downsampled_->points[neighborhood_index.first].getVector3fMap ()).norm (), 2)); + float d_weight = -(d / rn_sqr) + 1; //points that are close have a strong weight*/ + + //using normals to weight clutter points + Eigen::Vector3f scene_p_normal = scene_normals_->points[neighborhood_index.first].getNormalVector3fMap (); + Eigen::Vector3f model_p_normal = scene_normals_->points[recog_model->explained_[neighborhood_index.second]].getNormalVector3fMap (); + float dotp = scene_p_normal.dot (model_p_normal); //[-1,1] from antiparallel trough perpendicular to parallel + + if (dotp < 0) + dotp = 0.f; + + recog_model->unexplained_in_neighborhood_weights[p] = d_weight * dotp; + } + p++; + } + } + + recog_model->unexplained_in_neighborhood_weights.resize (p); + recog_model->unexplained_in_neighborhood.resize (p); + } +} + +#define PCL_INSTANTIATE_GoHV(T1,T2) template class PCL_EXPORTS pcl::GlobalHypothesesVerification; + +#endif /* PCL_RECOGNITION_IMPL_HV_GO_HPP_ */ + diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/hv_papazov.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/hv_papazov.hpp new file mode 100755 index 0000000..387ff4e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/hv_papazov.hpp @@ -0,0 +1,233 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once +#include + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template + void + pcl::PapazovHV::initialize () + { + + //clear stuff + recognition_models_.clear (); + graph_id_model_map_.clear (); + conflict_graph_.clear (); + explained_by_RM_.clear (); + points_explained_by_rm_.clear (); + + // initialize mask... + mask_.resize (complete_models_.size ()); + for (std::size_t i = 0; i < complete_models_.size (); i++) + mask_[i] = true; + + // initialize explained_by_RM + explained_by_RM_.resize (scene_cloud_downsampled_->points.size ()); + points_explained_by_rm_.resize (scene_cloud_downsampled_->points.size ()); + + // initialize model + for (std::size_t m = 0; m < complete_models_.size (); m++) + { + RecognitionModelPtr recog_model (new RecognitionModel); + // voxelize model cloud + recog_model->cloud_.reset (new pcl::PointCloud); + recog_model->complete_cloud_.reset (new pcl::PointCloud); + recog_model->id_ = static_cast (m); + + pcl::VoxelGrid voxel_grid; + voxel_grid.setInputCloud (visible_models_[m]); + voxel_grid.setLeafSize (resolution_, resolution_, resolution_); + voxel_grid.filter (*(recog_model->cloud_)); + + pcl::VoxelGrid voxel_grid_complete; + voxel_grid_complete.setInputCloud (complete_models_[m]); + voxel_grid_complete.setLeafSize (resolution_, resolution_, resolution_); + voxel_grid_complete.filter (*(recog_model->complete_cloud_)); + + std::vector explained_indices; + std::vector outliers; + std::vector nn_indices; + std::vector nn_distances; + + for (std::size_t i = 0; i < recog_model->cloud_->points.size (); i++) + { + if (!scene_downsampled_tree_->radiusSearch (recog_model->cloud_->points[i], inliers_threshold_, nn_indices, nn_distances, + std::numeric_limits::max ())) + { + outliers.push_back (static_cast (i)); + } + else + { + for (std::size_t k = 0; k < nn_distances.size (); k++) + { + explained_indices.push_back (nn_indices[k]); //nn_indices[k] points to the scene + } + } + } + + std::sort (explained_indices.begin (), explained_indices.end ()); + explained_indices.erase (std::unique (explained_indices.begin (), explained_indices.end ()), explained_indices.end ()); + + recog_model->bad_information_ = static_cast (outliers.size ()); + + if ((static_cast (recog_model->bad_information_) / static_cast (recog_model->complete_cloud_->points.size ())) + <= penalty_threshold_ && (static_cast (explained_indices.size ()) + / static_cast (recog_model->complete_cloud_->points.size ())) >= support_threshold_) + { + recog_model->explained_ = explained_indices; + recognition_models_.push_back (recog_model); + + // update explained_by_RM_, add 1 + for (const int &explained_index : explained_indices) + { + explained_by_RM_[explained_index]++; + points_explained_by_rm_[explained_index].push_back (recog_model); + } + } + else + { + mask_[m] = false; // the model didn't survive the sequential check... + } + } + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template + void + pcl::PapazovHV::nonMaximaSuppresion () + { + // iterate over all vertices of the graph and check if they have a better neighbour, then remove that vertex + using VertexIterator = typename boost::graph_traits::vertex_iterator; + VertexIterator vi, vi_end; + boost::tie (vi, vi_end) = boost::vertices (conflict_graph_); + + for (auto next = vi; next != vi_end; next++) + { + const typename Graph::vertex_descriptor v = boost::vertex (*next, conflict_graph_); + typename boost::graph_traits::adjacency_iterator ai; + typename boost::graph_traits::adjacency_iterator ai_end; + + auto current = std::static_pointer_cast (graph_id_model_map_[int (v)]); + + bool a_better_one = false; + for (boost::tie (ai, ai_end) = boost::adjacent_vertices (v, conflict_graph_); (ai != ai_end) && !a_better_one; ++ai) + { + auto neighbour = std::static_pointer_cast (graph_id_model_map_[int (*ai)]); + if ((neighbour->explained_.size () >= current->explained_.size ()) && mask_[neighbour->id_]) + { + a_better_one = true; + } + } + + if (a_better_one) + { + mask_[current->id_] = false; + } + } + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template + void + pcl::PapazovHV::buildConflictGraph () + { + // create vertices for the graph + for (std::size_t i = 0; i < (recognition_models_.size ()); i++) + { + const typename Graph::vertex_descriptor v = boost::add_vertex (recognition_models_[i], conflict_graph_); + graph_id_model_map_[int (v)] = std::static_pointer_cast (recognition_models_[i]); + } + + // iterate over the remaining models and check for each one if there is a conflict with another one + for (std::size_t i = 0; i < recognition_models_.size (); i++) + { + for (std::size_t j = i; j < recognition_models_.size (); j++) + { + if (i != j) + { + float n_conflicts = 0.f; + // count scene points explained by both models + for (std::size_t k = 0; k < explained_by_RM_.size (); k++) + { + if (explained_by_RM_[k] > 1) + { + // this point could be a conflict + bool i_found = false; + bool j_found = false; + bool both_found = false; + for (std::size_t kk = 0; (kk < points_explained_by_rm_[k].size ()) && !both_found; kk++) + { + if (points_explained_by_rm_[k][kk]->id_ == recognition_models_[i]->id_) + i_found = true; + + if (points_explained_by_rm_[k][kk]->id_ == recognition_models_[j]->id_) + j_found = true; + + if (i_found && j_found) + both_found = true; + } + + if (both_found) + n_conflicts += 1.f; + } + } + + // check if number of points is big enough to create a conflict + bool add_conflict = false; + add_conflict = ((n_conflicts / static_cast (recognition_models_[i]->complete_cloud_->points.size ())) > conflict_threshold_size_) + || ((n_conflicts / static_cast (recognition_models_[j]->complete_cloud_->points.size ())) > conflict_threshold_size_); + + if (add_conflict) + { + boost::add_edge (i, j, conflict_graph_); + } + } + } + } + } + +/////////////////////////////////////////////////////////////////////////////////////////////////// +template + void + pcl::PapazovHV::verify () + { + initialize (); + buildConflictGraph (); + nonMaximaSuppresion (); + } + +#define PCL_INSTANTIATE_PapazovHV(T1,T2) template class PCL_EXPORTS pcl::PapazovHV; diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/occlusion_reasoning.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/occlusion_reasoning.hpp new file mode 100755 index 0000000..575e3a8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/hv/occlusion_reasoning.hpp @@ -0,0 +1,199 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_RECOGNITION_OCCLUSION_REASONING_HPP_ +#define PCL_RECOGNITION_OCCLUSION_REASONING_HPP_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::occlusion_reasoning::ZBuffering::ZBuffering (int resx, int resy, float f) : + f_ (f), cx_ (resx), cy_ (resy), depth_ (nullptr) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::occlusion_reasoning::ZBuffering::ZBuffering () : + f_ (), cx_ (), cy_ (), depth_ (nullptr) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::occlusion_reasoning::ZBuffering::~ZBuffering () +{ + delete[] depth_; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::occlusion_reasoning::ZBuffering::filter (typename pcl::PointCloud::ConstPtr & model, + typename pcl::PointCloud::Ptr & filtered, float thres) +{ + std::vector indices_to_keep; + filter(model, indices_to_keep, thres); + pcl::copyPointCloud (*model, indices_to_keep, *filtered); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::occlusion_reasoning::ZBuffering::filter (typename pcl::PointCloud::ConstPtr & model, + std::vector & indices_to_keep, float thres) +{ + + float cx, cy; + cx = static_cast (cx_) / 2.f - 0.5f; + cy = static_cast (cy_) / 2.f - 0.5f; + + indices_to_keep.resize (model->points.size ()); + int keep = 0; + for (std::size_t i = 0; i < model->points.size (); i++) + { + float x = model->points[i].x; + float y = model->points[i].y; + float z = model->points[i].z; + int u = static_cast (f_ * x / z + cx); + int v = static_cast (f_ * y / z + cy); + + if (u >= cx_ || v >= cy_ || u < 0 || v < 0) + continue; + + //Check if point depth (distance to camera) is greater than the (u,v) meaning that the point is not visible + if ((z - thres) > depth_[u * cy_ + v] || !std::isfinite(depth_[u * cy_ + v])) + continue; + + indices_to_keep[keep] = static_cast (i); + keep++; + } + + indices_to_keep.resize (keep); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::occlusion_reasoning::ZBuffering::computeDepthMap (typename pcl::PointCloud::ConstPtr & scene, bool compute_focal, + bool smooth, int wsize) +{ + float cx, cy; + cx = static_cast (cx_) / 2.f - 0.5f; + cy = static_cast (cy_) / 2.f - 0.5f; + + //compute the focal length + if (compute_focal) + { + + float max_u, max_v, min_u, min_v; + max_u = max_v = std::numeric_limits::max () * -1; + min_u = min_v = std::numeric_limits::max (); + + for (std::size_t i = 0; i < scene->points.size (); i++) + { + float b_x = scene->points[i].x / scene->points[i].z; + if (b_x > max_u) + max_u = b_x; + if (b_x < min_u) + min_u = b_x; + + float b_y = scene->points[i].y / scene->points[i].z; + if (b_y > max_v) + max_v = b_y; + if (b_y < min_v) + min_v = b_y; + } + + float maxC = std::max (std::max (std::abs (max_u), std::abs (max_v)), std::max (std::abs (min_u), std::abs (min_v))); + f_ = (cx) / maxC; + } + + depth_ = new float[cx_ * cy_]; + for (int i = 0; i < (cx_ * cy_); i++) + depth_[i] = std::numeric_limits::quiet_NaN (); + + for (std::size_t i = 0; i < scene->points.size (); i++) + { + float x = scene->points[i].x; + float y = scene->points[i].y; + float z = scene->points[i].z; + int u = static_cast (f_ * x / z + cx); + int v = static_cast (f_ * y / z + cy); + + if (u >= cx_ || v >= cy_ || u < 0 || v < 0) + continue; + + if ((z < depth_[u * cy_ + v]) || (!std::isfinite(depth_[u * cy_ + v]))) + depth_[u * cx_ + v] = z; + } + + if (smooth) + { + //Dilate and smooth the depth map + int ws = wsize; + int ws2 = int (std::floor (static_cast (ws) / 2.f)); + float * depth_smooth = new float[cx_ * cy_]; + for (int i = 0; i < (cx_ * cy_); i++) + depth_smooth[i] = std::numeric_limits::quiet_NaN (); + + for (int u = ws2; u < (cx_ - ws2); u++) + { + for (int v = ws2; v < (cy_ - ws2); v++) + { + float min = std::numeric_limits::max (); + for (int j = (u - ws2); j <= (u + ws2); j++) + { + for (int i = (v - ws2); i <= (v + ws2); i++) + { + if (std::isfinite(depth_[j * cx_ + i]) && (depth_[j * cx_ + i] < min)) + { + min = depth_[j * cx_ + i]; + } + } + } + + if (min < (std::numeric_limits::max () - 0.1)) + { + depth_smooth[u * cx_ + v] = min; + } + } + } + + memcpy (depth_, depth_smooth, sizeof(float) * cx_ * cy_); + delete[] depth_smooth; + } +} + +#endif // PCL_RECOGNITION_OCCLUSION_REASONING_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/implicit_shape_model.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/implicit_shape_model.hpp new file mode 100755 index 0000000..8a06e68 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/implicit_shape_model.hpp @@ -0,0 +1,1529 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * Implementation of the ISM algorithm described in "Hough Transforms and 3D SURF for robust three dimensional classication" + * by Jan Knopp, Mukta Prasad, Geert Willems, Radu Timofte, and Luc Van Gool + * + * Authors: Roman Shapovalov, Alexander Velizhev, Sergey Ushakov + */ + +#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_ +#define PCL_IMPLICIT_SHAPE_MODEL_HPP_ + +#include "../implicit_shape_model.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::features::ISMVoteList::ISMVoteList () : + votes_ (new pcl::PointCloud ()), + tree_is_valid_ (false), + votes_origins_ (new pcl::PointCloud ()), + votes_class_ (0), + k_ind_ (0), + k_sqr_dist_ (0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::features::ISMVoteList::~ISMVoteList () +{ + votes_class_.clear (); + votes_origins_.reset (); + votes_.reset (); + k_ind_.clear (); + k_sqr_dist_.clear (); + tree_.reset (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::features::ISMVoteList::addVote ( + pcl::InterestPoint& vote, const PointT &vote_origin, int votes_class) +{ + tree_is_valid_ = false; + votes_->points.insert (votes_->points.end (), vote);// TODO: adjust height and width + + votes_origins_->points.push_back (vote_origin); + votes_class_.push_back (votes_class); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::PointCloud::Ptr +pcl::features::ISMVoteList::getColoredCloud (typename pcl::PointCloud::Ptr cloud) +{ + pcl::PointXYZRGB point; + pcl::PointCloud::Ptr colored_cloud = (new pcl::PointCloud)->makeShared (); + colored_cloud->height = 0; + colored_cloud->width = 1; + + if (cloud != nullptr) + { + colored_cloud->height += static_cast (cloud->points.size ()); + point.r = 255; + point.g = 255; + point.b = 255; + for (std::size_t i_point = 0; i_point < cloud->points.size (); i_point++) + { + point.x = cloud->points[i_point].x; + point.y = cloud->points[i_point].y; + point.z = cloud->points[i_point].z; + colored_cloud->points.push_back (point); + } + } + + point.r = 0; + point.g = 0; + point.b = 255; + for (const auto &i_vote : votes_->points) + { + point.x = i_vote.x; + point.y = i_vote.y; + point.z = i_vote.z; + colored_cloud->points.push_back (point); + } + colored_cloud->height += static_cast (votes_->points.size ()); + + return (colored_cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::features::ISMVoteList::findStrongestPeaks ( + std::vector > &out_peaks, + int in_class_id, + double in_non_maxima_radius, + double in_sigma) +{ + validateTree (); + + const std::size_t n_vote_classes = votes_class_.size (); + if (n_vote_classes == 0) + return; + for (std::size_t i = 0; i < n_vote_classes ; i++) + assert ( votes_class_[i] == in_class_id ); + + // heuristic: start from NUM_INIT_PTS different locations selected uniformly + // on the votes. Intuitively, it is likely to get a good location in dense regions. + const int NUM_INIT_PTS = 100; + double SIGMA_DIST = in_sigma;// rule of thumb: 10% of the object radius + const double FINAL_EPS = SIGMA_DIST / 100;// another heuristic + + std::vector > peaks (NUM_INIT_PTS); + std::vector peak_densities (NUM_INIT_PTS); + double max_density = -1.0; + for (int i = 0; i < NUM_INIT_PTS; i++) + { + Eigen::Vector3f old_center; + Eigen::Vector3f curr_center; + curr_center (0) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].x; + curr_center (1) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].y; + curr_center (2) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].z; + + do + { + old_center = curr_center; + curr_center = shiftMean (old_center, SIGMA_DIST); + } while ((old_center - curr_center).norm () > FINAL_EPS); + + pcl::PointXYZ point; + point.x = curr_center (0); + point.y = curr_center (1); + point.z = curr_center (2); + double curr_density = getDensityAtPoint (point, SIGMA_DIST); + assert (curr_density >= 0.0); + + peaks[i] = curr_center; + peak_densities[i] = curr_density; + + if ( max_density < curr_density ) + max_density = curr_density; + } + + //extract peaks + std::vector peak_flag (NUM_INIT_PTS, true); + for (int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++) + { + // find best peak with taking into consideration peak flags + double best_density = -1.0; + Eigen::Vector3f strongest_peak; + int best_peak_ind (-1); + int peak_counter (0); + for (int i = 0; i < NUM_INIT_PTS; i++) + { + if ( !peak_flag[i] ) + continue; + + if ( peak_densities[i] > best_density) + { + best_density = peak_densities[i]; + strongest_peak = peaks[i]; + best_peak_ind = i; + } + ++peak_counter; + } + + if( peak_counter == 0 ) + break;// no peaks + + pcl::ISMPeak peak; + peak.x = strongest_peak(0); + peak.y = strongest_peak(1); + peak.z = strongest_peak(2); + peak.density = best_density; + peak.class_id = in_class_id; + out_peaks.push_back ( peak ); + + // mark best peaks and all its neighbors + peak_flag[best_peak_ind] = false; + for (int i = 0; i < NUM_INIT_PTS; i++) + { + // compute distance between best peak and all unmarked peaks + if ( !peak_flag[i] ) + continue; + + double dist = (strongest_peak - peaks[i]).norm (); + if ( dist < in_non_maxima_radius ) + peak_flag[i] = false; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::features::ISMVoteList::validateTree () +{ + if (!tree_is_valid_) + { + if (tree_ == nullptr) + tree_.reset (new pcl::KdTreeFLANN); + tree_->setInputCloud (votes_); + k_ind_.resize ( votes_->points.size (), -1 ); + k_sqr_dist_.resize ( votes_->points.size (), 0.0f ); + tree_is_valid_ = true; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::Vector3f +pcl::features::ISMVoteList::shiftMean (const Eigen::Vector3f& snap_pt, const double in_sigma_dist) +{ + validateTree (); + + Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0); + double denom = 0.0; + + pcl::InterestPoint pt; + pt.x = snap_pt[0]; + pt.y = snap_pt[1]; + pt.z = snap_pt[2]; + std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_); + + for (std::size_t j = 0; j < n_pts; j++) + { + double kernel = votes_->points[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist)); + Eigen::Vector3f vote_vec (votes_->points[k_ind_[j]].x, votes_->points[k_ind_[j]].y, votes_->points[k_ind_[j]].z); + wgh_sum += vote_vec * static_cast (kernel); + denom += kernel; + } + assert (denom > 0.0); // at least one point is close. In fact, this case should be handled too + + return (wgh_sum / static_cast (denom)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::features::ISMVoteList::getDensityAtPoint ( + const PointT &point, double sigma_dist) +{ + validateTree (); + + const std::size_t n_vote_classes = votes_class_.size (); + if (n_vote_classes == 0) + return (0.0); + + double sum_vote = 0.0; + + pcl::InterestPoint pt; + pt.x = point.x; + pt.y = point.y; + pt.z = point.z; + std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_); + + for (std::size_t j = 0; j < num_of_pts; j++) + sum_vote += votes_->points[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist)); + + return (sum_vote); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::features::ISMVoteList::getNumberOfVotes () +{ + return (static_cast (votes_->points.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +pcl::features::ISMModel::ISMModel () : + statistical_weights_ (0), + learned_weights_ (0), + classes_ (0), + sigmas_ (0), + clusters_ (0), + number_of_classes_ (0), + number_of_visual_words_ (0), + number_of_clusters_ (0), + descriptors_dimension_ (0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +pcl::features::ISMModel::ISMModel (ISMModel const & copy) +{ + reset (); + + this->number_of_classes_ = copy.number_of_classes_; + this->number_of_visual_words_ = copy.number_of_visual_words_; + this->number_of_clusters_ = copy.number_of_clusters_; + this->descriptors_dimension_ = copy.descriptors_dimension_; + + std::vector vec; + vec.resize (this->number_of_clusters_, 0.0f); + this->statistical_weights_.resize (this->number_of_classes_, vec); + for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) + for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) + this->statistical_weights_[i_class][i_cluster] = copy.statistical_weights_[i_class][i_cluster]; + + this->learned_weights_.resize (this->number_of_visual_words_, 0.0f); + for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) + this->learned_weights_[i_visual_word] = copy.learned_weights_[i_visual_word]; + + this->classes_.resize (this->number_of_visual_words_, 0); + for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) + this->classes_[i_visual_word] = copy.classes_[i_visual_word]; + + this->sigmas_.resize (this->number_of_classes_, 0.0f); + for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) + this->sigmas_[i_class] = copy.sigmas_[i_class]; + + this->directions_to_center_.resize (this->number_of_visual_words_, 3); + for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) + for (unsigned int i_dim = 0; i_dim < 3; i_dim++) + this->directions_to_center_ (i_visual_word, i_dim) = copy.directions_to_center_ (i_visual_word, i_dim); + + this->clusters_centers_.resize (this->number_of_clusters_, 3); + for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) + for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++) + this->clusters_centers_ (i_cluster, i_dim) = copy.clusters_centers_ (i_cluster, i_dim); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +pcl::features::ISMModel::~ISMModel () +{ + reset (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::features::ISMModel::saveModelToFile (std::string& file_name) +{ + std::ofstream output_file (file_name.c_str (), std::ios::trunc); + if (!output_file) + { + output_file.close (); + return (false); + } + + output_file << number_of_classes_ << " "; + output_file << number_of_visual_words_ << " "; + output_file << number_of_clusters_ << " "; + output_file << descriptors_dimension_ << " "; + + //write statistical weights + for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + output_file << statistical_weights_[i_class][i_cluster] << " "; + + //write learned weights + for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) + output_file << learned_weights_[i_visual_word] << " "; + + //write classes + for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) + output_file << classes_[i_visual_word] << " "; + + //write sigmas + for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) + output_file << sigmas_[i_class] << " "; + + //write directions to centers + for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) + for (unsigned int i_dim = 0; i_dim < 3; i_dim++) + output_file << directions_to_center_ (i_visual_word, i_dim) << " "; + + //write clusters centers + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++) + output_file << clusters_centers_ (i_cluster, i_dim) << " "; + + //write clusters + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + { + output_file << static_cast (clusters_[i_cluster].size ()) << " "; + for (const unsigned int &visual_word : clusters_[i_cluster]) + output_file << visual_word << " "; + } + + output_file.close (); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::features::ISMModel::loadModelFromfile (std::string& file_name) +{ + reset (); + std::ifstream input_file (file_name.c_str ()); + if (!input_file) + { + input_file.close (); + return (false); + } + + char line[256]; + + input_file.getline (line, 256, ' '); + number_of_classes_ = static_cast (strtol (line, nullptr, 10)); + input_file.getline (line, 256, ' '); number_of_visual_words_ = atoi (line); + input_file.getline (line, 256, ' '); number_of_clusters_ = atoi (line); + input_file.getline (line, 256, ' '); descriptors_dimension_ = atoi (line); + + //read statistical weights + std::vector vec; + vec.resize (number_of_clusters_, 0.0f); + statistical_weights_.resize (number_of_classes_, vec); + for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + input_file >> statistical_weights_[i_class][i_cluster]; + + //read learned weights + learned_weights_.resize (number_of_visual_words_, 0.0f); + for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) + input_file >> learned_weights_[i_visual_word]; + + //read classes + classes_.resize (number_of_visual_words_, 0); + for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) + input_file >> classes_[i_visual_word]; + + //read sigmas + sigmas_.resize (number_of_classes_, 0.0f); + for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++) + input_file >> sigmas_[i_class]; + + //read directions to centers + directions_to_center_.resize (number_of_visual_words_, 3); + for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++) + for (unsigned int i_dim = 0; i_dim < 3; i_dim++) + input_file >> directions_to_center_ (i_visual_word, i_dim); + + //read clusters centers + clusters_centers_.resize (number_of_clusters_, descriptors_dimension_); + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++) + input_file >> clusters_centers_ (i_cluster, i_dim); + + //read clusters + std::vector vect; + clusters_.resize (number_of_clusters_, vect); + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + { + unsigned int size_of_current_cluster = 0; + input_file >> size_of_current_cluster; + clusters_[i_cluster].resize (size_of_current_cluster, 0); + for (unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++) + input_file >> clusters_[i_cluster][i_visual_word]; + } + + input_file.close (); + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::features::ISMModel::reset () +{ + statistical_weights_.clear (); + learned_weights_.clear (); + classes_.clear (); + sigmas_.clear (); + directions_to_center_.resize (0, 0); + clusters_centers_.resize (0, 0); + clusters_.clear (); + number_of_classes_ = 0; + number_of_visual_words_ = 0; + number_of_clusters_ = 0; + descriptors_dimension_ = 0; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +pcl::features::ISMModel& +pcl::features::ISMModel::operator = (const pcl::features::ISMModel& other) +{ + if (this != &other) + { + this->reset (); + + this->number_of_classes_ = other.number_of_classes_; + this->number_of_visual_words_ = other.number_of_visual_words_; + this->number_of_clusters_ = other.number_of_clusters_; + this->descriptors_dimension_ = other.descriptors_dimension_; + + std::vector vec; + vec.resize (number_of_clusters_, 0.0f); + this->statistical_weights_.resize (this->number_of_classes_, vec); + for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) + for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) + this->statistical_weights_[i_class][i_cluster] = other.statistical_weights_[i_class][i_cluster]; + + this->learned_weights_.resize (this->number_of_visual_words_, 0.0f); + for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) + this->learned_weights_[i_visual_word] = other.learned_weights_[i_visual_word]; + + this->classes_.resize (this->number_of_visual_words_, 0); + for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) + this->classes_[i_visual_word] = other.classes_[i_visual_word]; + + this->sigmas_.resize (this->number_of_classes_, 0.0f); + for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++) + this->sigmas_[i_class] = other.sigmas_[i_class]; + + this->directions_to_center_.resize (this->number_of_visual_words_, 3); + for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++) + for (unsigned int i_dim = 0; i_dim < 3; i_dim++) + this->directions_to_center_ (i_visual_word, i_dim) = other.directions_to_center_ (i_visual_word, i_dim); + + this->clusters_centers_.resize (this->number_of_clusters_, 3); + for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++) + for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++) + this->clusters_centers_ (i_cluster, i_dim) = other.clusters_centers_ (i_cluster, i_dim); + } + return (*this); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ism::ImplicitShapeModelEstimation::ImplicitShapeModelEstimation () : + training_clouds_ (0), + training_classes_ (0), + training_normals_ (0), + training_sigmas_ (0), + sampling_size_ (0.1f), + feature_estimator_ (), + number_of_clusters_ (184), + n_vot_ON_ (true) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ism::ImplicitShapeModelEstimation::~ImplicitShapeModelEstimation () +{ + training_clouds_.clear (); + training_classes_.clear (); + training_normals_.clear (); + training_sigmas_.clear (); + feature_estimator_.reset (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector::Ptr> +pcl::ism::ImplicitShapeModelEstimation::getTrainingClouds () +{ + return (training_clouds_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setTrainingClouds ( + const std::vector< typename pcl::PointCloud::Ptr >& training_clouds) +{ + training_clouds_.clear (); + std::vector::Ptr > clouds ( training_clouds.begin (), training_clouds.end () ); + training_clouds_.swap (clouds); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector +pcl::ism::ImplicitShapeModelEstimation::getTrainingClasses () +{ + return (training_classes_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setTrainingClasses (const std::vector& training_classes) +{ + training_classes_.clear (); + std::vector classes ( training_classes.begin (), training_classes.end () ); + training_classes_.swap (classes); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector::Ptr> +pcl::ism::ImplicitShapeModelEstimation::getTrainingNormals () +{ + return (training_normals_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setTrainingNormals ( + const std::vector< typename pcl::PointCloud::Ptr >& training_normals) +{ + training_normals_.clear (); + std::vector::Ptr > normals ( training_normals.begin (), training_normals.end () ); + training_normals_.swap (normals); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::ism::ImplicitShapeModelEstimation::getSamplingSize () +{ + return (sampling_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setSamplingSize (float sampling_size) +{ + if (sampling_size >= std::numeric_limits::epsilon ()) + sampling_size_ = sampling_size; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::ism::ImplicitShapeModelEstimation::FeaturePtr +pcl::ism::ImplicitShapeModelEstimation::getFeatureEstimator () +{ + return (feature_estimator_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setFeatureEstimator (FeaturePtr feature) +{ + feature_estimator_ = feature; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::ism::ImplicitShapeModelEstimation::getNumberOfClusters () +{ + return (number_of_clusters_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setNumberOfClusters (unsigned int num_of_clusters) +{ + if (num_of_clusters > 0) + number_of_clusters_ = num_of_clusters; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector +pcl::ism::ImplicitShapeModelEstimation::getSigmaDists () +{ + return (training_sigmas_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setSigmaDists (const std::vector& training_sigmas) +{ + training_sigmas_.clear (); + std::vector sigmas ( training_sigmas.begin (), training_sigmas.end () ); + training_sigmas_.swap (sigmas); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ism::ImplicitShapeModelEstimation::getNVotState () +{ + return (n_vot_ON_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::setNVotState (bool state) +{ + n_vot_ON_ = state; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ism::ImplicitShapeModelEstimation::trainISM (ISMModelPtr& trained_model) +{ + bool success = true; + + if (trained_model == nullptr) + return (false); + trained_model->reset (); + + std::vector > histograms; + std::vector > locations; + success = extractDescriptors (histograms, locations); + if (!success) + return (false); + + Eigen::MatrixXi labels; + success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_); + if (!success) + return (false); + + std::vector vec; + trained_model->clusters_.resize (number_of_clusters_, vec); + for (std::size_t i_label = 0; i_label < locations.size (); i_label++) + trained_model->clusters_[labels (i_label)].push_back (i_label); + + calculateSigmas (trained_model->sigmas_); + + calculateWeights( + locations, + labels, + trained_model->sigmas_, + trained_model->clusters_, + trained_model->statistical_weights_, + trained_model->learned_weights_); + + trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1; + trained_model->number_of_visual_words_ = static_cast (histograms.size ()); + trained_model->number_of_clusters_ = number_of_clusters_; + trained_model->descriptors_dimension_ = FeatureSize; + + trained_model->directions_to_center_.resize (locations.size (), 3); + trained_model->classes_.resize (locations.size ()); + for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++) + { + trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x; + trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y; + trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z; + trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_]; + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::features::ISMVoteList::Ptr +pcl::ism::ImplicitShapeModelEstimation::findObjects ( + ISMModelPtr model, + typename pcl::PointCloud::Ptr in_cloud, + typename pcl::PointCloud::Ptr in_normals, + int in_class_of_interest) +{ + typename pcl::features::ISMVoteList::Ptr out_votes (new pcl::features::ISMVoteList ()); + + if (in_cloud->points.empty ()) + return (out_votes); + + typename pcl::PointCloud::Ptr sampled_point_cloud (new pcl::PointCloud ()); + typename pcl::PointCloud::Ptr sampled_normal_cloud (new pcl::PointCloud ()); + simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud); + if (sampled_point_cloud->points.empty ()) + return (out_votes); + + typename pcl::PointCloud >::Ptr feature_cloud (new pcl::PointCloud > ()); + estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud); + + //find nearest cluster + const unsigned int n_key_points = static_cast (sampled_point_cloud->size ()); + std::vector min_dist_inds (n_key_points, -1); + for (unsigned int i_point = 0; i_point < n_key_points; i_point++) + { + Eigen::VectorXf curr_descriptor (FeatureSize); + for (int i_dim = 0; i_dim < FeatureSize; i_dim++) + curr_descriptor (i_dim) = feature_cloud->points[i_point].histogram[i_dim]; + + float descriptor_sum = curr_descriptor.sum (); + if (descriptor_sum < std::numeric_limits::epsilon ()) + continue; + + unsigned int min_dist_idx = 0; + Eigen::VectorXf clusters_center (FeatureSize); + for (int i_dim = 0; i_dim < FeatureSize; i_dim++) + clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim); + + float best_dist = computeDistance (curr_descriptor, clusters_center); + for (unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++) + { + for (int i_dim = 0; i_dim < FeatureSize; i_dim++) + clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim); + float curr_dist = computeDistance (clusters_center, curr_descriptor); + if (curr_dist < best_dist) + { + min_dist_idx = i_clust_cent; + best_dist = curr_dist; + } + } + min_dist_inds[i_point] = min_dist_idx; + }//next keypoint + + for (std::size_t i_point = 0; i_point < n_key_points; i_point++) + { + int min_dist_idx = min_dist_inds[i_point]; + if (min_dist_idx == -1) + continue; + + const unsigned int n_words = static_cast (model->clusters_[min_dist_idx].size ()); + //compute coord system transform + Eigen::Matrix3f transform = alignYCoordWithNormal (sampled_normal_cloud->points[i_point]); + for (unsigned int i_word = 0; i_word < n_words; i_word++) + { + unsigned int index = model->clusters_[min_dist_idx][i_word]; + unsigned int i_class = model->classes_[index]; + if (static_cast (i_class) != in_class_of_interest) + continue;//skip this class + + //rotate dir to center as needed + Eigen::Vector3f direction ( + model->directions_to_center_(index, 0), + model->directions_to_center_(index, 1), + model->directions_to_center_(index, 2)); + applyTransform (direction, transform.transpose ()); + + pcl::InterestPoint vote; + Eigen::Vector3f vote_pos = sampled_point_cloud->points[i_point].getVector3fMap () + direction; + vote.x = vote_pos[0]; + vote.y = vote_pos[1]; + vote.z = vote_pos[2]; + float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx]; + float learned_weight = model->learned_weights_[index]; + float power = statistical_weight * learned_weight; + vote.strength = power; + if (vote.strength > std::numeric_limits::epsilon ()) + out_votes->addVote (vote, sampled_point_cloud->points[i_point], i_class); + } + }//next point + + return (out_votes); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ism::ImplicitShapeModelEstimation::extractDescriptors ( + std::vector< pcl::Histogram >& histograms, + std::vector< LocationInfo, Eigen::aligned_allocator >& locations) +{ + histograms.clear (); + locations.clear (); + + int n_key_points = 0; + + if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ == nullptr) + return (false); + + for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++) + { + //compute the center of the training object + Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f); + const std::size_t num_of_points = training_clouds_[i_cloud]->points.size (); + for (auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++) + models_center += point_j->getVector3fMap (); + models_center /= static_cast (num_of_points); + + //downsample the cloud + typename pcl::PointCloud::Ptr sampled_point_cloud (new pcl::PointCloud ()); + typename pcl::PointCloud::Ptr sampled_normal_cloud (new pcl::PointCloud ()); + simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud); + if (sampled_point_cloud->points.empty ()) + continue; + + shiftCloud (training_clouds_[i_cloud], models_center); + shiftCloud (sampled_point_cloud, models_center); + + n_key_points += static_cast (sampled_point_cloud->size ()); + + typename pcl::PointCloud >::Ptr feature_cloud (new pcl::PointCloud > ()); + estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud); + + int point_index = 0; + for (auto point_i = sampled_point_cloud->points.cbegin (); point_i != sampled_point_cloud->points.cend (); point_i++, point_index++) + { + float descriptor_sum = Eigen::VectorXf::Map (feature_cloud->points[point_index].histogram, FeatureSize).sum (); + if (descriptor_sum < std::numeric_limits::epsilon ()) + continue; + + histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 ); + + int dist = static_cast (std::distance (sampled_point_cloud->points.cbegin (), point_i)); + Eigen::Matrix3f new_basis = alignYCoordWithNormal (sampled_normal_cloud->points[dist]); + Eigen::Vector3f zero; + zero (0) = 0.0; + zero (1) = 0.0; + zero (2) = 0.0; + Eigen::Vector3f new_dir = zero - point_i->getVector3fMap (); + applyTransform (new_dir, new_basis); + + PointT point (new_dir[0], new_dir[1], new_dir[2]); + LocationInfo info (static_cast (i_cloud), point, *point_i, sampled_normal_cloud->points[dist]); + locations.insert(locations.end (), info); + } + }//next training cloud + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ism::ImplicitShapeModelEstimation::clusterDescriptors ( + std::vector< pcl::Histogram >& histograms, + Eigen::MatrixXi& labels, + Eigen::MatrixXf& clusters_centers) +{ + Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize); + + for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++) + for (int i_dim = 0; i_dim < FeatureSize; i_dim++) + points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim]; + + labels.resize (histograms.size(), 1); + computeKMeansClustering ( + points_to_cluster, + number_of_clusters_, + labels, + TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000 + 5, + PP_CENTERS, + clusters_centers); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::calculateSigmas (std::vector& sigmas) +{ + if (!training_sigmas_.empty ()) + { + sigmas.resize (training_sigmas_.size (), 0.0f); + for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++) + sigmas[i_sigma] = training_sigmas_[i_sigma]; + return; + } + + sigmas.clear (); + unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1; + sigmas.resize (number_of_classes, 0.0f); + + std::vector vec; + std::vector > objects_sigmas; + objects_sigmas.resize (number_of_classes, vec); + + unsigned int number_of_objects = static_cast (training_clouds_.size ()); + for (unsigned int i_object = 0; i_object < number_of_objects; i_object++) + { + float max_distance = 0.0f; + unsigned int number_of_points = static_cast (training_clouds_[i_object]->points.size ()); + for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++) + for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++) + { + float curr_distance = 0.0f; + curr_distance += training_clouds_[i_object]->points[i_point].x * training_clouds_[i_object]->points[j_point].x; + curr_distance += training_clouds_[i_object]->points[i_point].y * training_clouds_[i_object]->points[j_point].y; + curr_distance += training_clouds_[i_object]->points[i_point].z * training_clouds_[i_object]->points[j_point].z; + if (curr_distance > max_distance) + max_distance = curr_distance; + } + max_distance = static_cast (sqrt (max_distance)); + unsigned int i_class = training_classes_[i_object]; + objects_sigmas[i_class].push_back (max_distance); + } + + for (unsigned int i_class = 0; i_class < number_of_classes; i_class++) + { + float sig = 0.0f; + unsigned int number_of_objects_in_class = static_cast (objects_sigmas[i_class].size ()); + for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++) + sig += objects_sigmas[i_class][i_object]; + sig /= (static_cast (number_of_objects_in_class) * 10.0f); + sigmas[i_class] = sig; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::calculateWeights ( + const std::vector< LocationInfo, Eigen::aligned_allocator >& locations, + const Eigen::MatrixXi &labels, + std::vector& sigmas, + std::vector >& clusters, + std::vector >& statistical_weights, + std::vector& learned_weights) +{ + unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1; + //Temporary variable + std::vector vec; + vec.resize (number_of_clusters_, 0.0f); + statistical_weights.clear (); + learned_weights.clear (); + statistical_weights.resize (number_of_classes, vec); + learned_weights.resize (locations.size (), 0.0f); + + //Temporary variable + std::vector vect; + vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0); + + //Number of features from which c_i was learned + std::vector n_ftr; + + //Total number of votes from visual word v_j + std::vector n_vot; + + //Number of visual words that vote for class c_i + std::vector n_vw; + + //Number of votes for class c_i from v_j + std::vector > n_vot_2; + + n_vot_2.resize (number_of_clusters_, vect); + n_vot.resize (number_of_clusters_, 0); + n_ftr.resize (number_of_classes, 0); + for (std::size_t i_location = 0; i_location < locations.size (); i_location++) + { + int i_class = training_classes_[locations[i_location].model_num_]; + int i_cluster = labels (i_location); + n_vot_2[i_cluster][i_class] += 1; + n_vot[i_cluster] += 1; + n_ftr[i_class] += 1; + } + + n_vw.resize (number_of_classes, 0); + for (unsigned int i_class = 0; i_class < number_of_classes; i_class++) + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + if (n_vot_2[i_cluster][i_class] > 0) + n_vw[i_class] += 1; + + //computing learned weights + learned_weights.resize (locations.size (), 0.0); + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + { + unsigned int number_of_words_in_cluster = static_cast (clusters[i_cluster].size ()); + for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++) + { + unsigned int i_index = clusters[i_cluster][i_visual_word]; + int i_class = training_classes_[locations[i_index].model_num_]; + float square_sigma_dist = sigmas[i_class] * sigmas[i_class]; + if (square_sigma_dist < std::numeric_limits::epsilon ()) + { + std::vector calculated_sigmas; + calculateSigmas (calculated_sigmas); + square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class]; + if (square_sigma_dist < std::numeric_limits::epsilon ()) + continue; + } + Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_); + Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap (); + applyTransform (direction, transform); + Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction; + + //collect gaussian weighted distances + std::vector gauss_dists; + for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++) + { + unsigned int j_index = clusters[i_cluster][j_visual_word]; + int j_class = training_classes_[locations[j_index].model_num_]; + if (i_class != j_class) + continue; + //predict center + Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_); + Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap (); + applyTransform (direction_2, transform_2); + Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2; + float residual = (predicted_center - actual_center).norm (); + float value = -residual * residual / square_sigma_dist; + gauss_dists.push_back (static_cast (std::exp (value))); + }//next word + //find median gaussian weighted distance + std::size_t mid_elem = (gauss_dists.size () - 1) / 2; + std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ()); + learned_weights[i_index] = *(gauss_dists.begin () + mid_elem); + }//next word + }//next cluster + + //computing statistical weights + for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++) + { + for (unsigned int i_class = 0; i_class < number_of_classes; i_class++) + { + if (n_vot_2[i_cluster][i_class] == 0) + continue;//no votes per class of interest in this cluster + if (n_vw[i_class] == 0) + continue;//there were no objects of this class in the training dataset + if (n_vot[i_cluster] == 0) + continue;//this cluster has never been used + if (n_ftr[i_class] == 0) + continue;//there were no objects of this class in the training dataset + float part_1 = static_cast (n_vw[i_class]); + float part_2 = static_cast (n_vot[i_cluster]); + float part_3 = static_cast (n_vot_2[i_cluster][i_class]) / static_cast (n_ftr[i_class]); + float part_4 = 0.0f; + + if (!n_vot_ON_) + part_2 = 1.0f; + + for (unsigned int j_class = 0; j_class < number_of_classes; j_class++) + if (n_ftr[j_class] != 0) + part_4 += static_cast (n_vot_2[i_cluster][j_class]) / static_cast (n_ftr[j_class]); + + statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4; + } + }//next cluster +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::simplifyCloud ( + typename pcl::PointCloud::ConstPtr in_point_cloud, + typename pcl::PointCloud::ConstPtr in_normal_cloud, + typename pcl::PointCloud::Ptr out_sampled_point_cloud, + typename pcl::PointCloud::Ptr out_sampled_normal_cloud) +{ + //create voxel grid + pcl::VoxelGrid grid; + grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_); + grid.setSaveLeafLayout (true); + grid.setInputCloud (in_point_cloud); + + pcl::PointCloud temp_cloud; + grid.filter (temp_cloud); + + //extract indices of points from source cloud which are closest to grid points + const float max_value = std::numeric_limits::max (); + + const std::size_t num_source_points = in_point_cloud->points.size (); + const std::size_t num_sample_points = temp_cloud.points.size (); + + std::vector dist_to_grid_center (num_sample_points, max_value); + std::vector sampling_indices (num_sample_points, -1); + + for (std::size_t i_point = 0; i_point < num_source_points; i_point++) + { + int index = grid.getCentroidIndex (in_point_cloud->points[i_point]); + if (index == -1) + continue; + + PointT pt_1 = in_point_cloud->points[i_point]; + PointT pt_2 = temp_cloud.points[index]; + + float distance = (pt_1.x - pt_2.x) * (pt_1.x - pt_2.x) + (pt_1.y - pt_2.y) * (pt_1.y - pt_2.y) + (pt_1.z - pt_2.z) * (pt_1.z - pt_2.z); + if (distance < dist_to_grid_center[index]) + { + dist_to_grid_center[index] = distance; + sampling_indices[index] = static_cast (i_point); + } + } + + //extract source points + pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ()); + pcl::ExtractIndices extract_points; + pcl::ExtractIndices extract_normals; + + final_inliers_indices->indices.reserve (num_sample_points); + for (std::size_t i_point = 0; i_point < num_sample_points; i_point++) + { + if (sampling_indices[i_point] != -1) + final_inliers_indices->indices.push_back ( sampling_indices[i_point] ); + } + + extract_points.setInputCloud (in_point_cloud); + extract_points.setIndices (final_inliers_indices); + extract_points.filter (*out_sampled_point_cloud); + + extract_normals.setInputCloud (in_normal_cloud); + extract_normals.setIndices (final_inliers_indices); + extract_normals.filter (*out_sampled_normal_cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::shiftCloud ( + typename pcl::PointCloud::Ptr in_cloud, + Eigen::Vector3f shift_point) +{ + for (auto point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++) + { + point_it->x -= shift_point.x (); + point_it->y -= shift_point.y (); + point_it->z -= shift_point.z (); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::Matrix3f +pcl::ism::ImplicitShapeModelEstimation::alignYCoordWithNormal (const NormalT& in_normal) +{ + Eigen::Matrix3f result; + Eigen::Matrix3f rotation_matrix_X; + Eigen::Matrix3f rotation_matrix_Z; + + float A = 0.0f; + float B = 0.0f; + float sign = -1.0f; + + float denom_X = static_cast (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y)); + A = in_normal.normal_y / denom_X; + B = sign * in_normal.normal_z / denom_X; + rotation_matrix_X << 1.0f, 0.0f, 0.0f, + 0.0f, A, -B, + 0.0f, B, A; + + float denom_Z = static_cast (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y)); + A = in_normal.normal_y / denom_Z; + B = sign * in_normal.normal_x / denom_Z; + rotation_matrix_Z << A, -B, 0.0f, + B, A, 0.0f, + 0.0f, 0.0f, 1.0f; + + result = rotation_matrix_X * rotation_matrix_Z; + + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform) +{ + io_vec = in_transform * io_vec; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::estimateFeatures ( + typename pcl::PointCloud::Ptr sampled_point_cloud, + typename pcl::PointCloud::Ptr normal_cloud, + typename pcl::PointCloud >::Ptr feature_cloud) +{ + typename pcl::search::Search::Ptr tree (new pcl::search::KdTree); +// tree->setInputCloud (point_cloud); + + feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ()); +// feature_estimator_->setSearchSurface (point_cloud->makeShared ()); + feature_estimator_->setSearchMethod (tree); + +// typename pcl::SpinImageEstimation >::Ptr feat_est_norm = +// boost::dynamic_pointer_cast > > (feature_estimator_); +// feat_est_norm->setInputNormals (normal_cloud); + + typename pcl::FeatureFromNormals >::Ptr feat_est_norm = + boost::dynamic_pointer_cast > > (feature_estimator_); + feat_est_norm->setInputNormals (normal_cloud); + + feature_estimator_->compute (*feature_cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::ism::ImplicitShapeModelEstimation::computeKMeansClustering ( + const Eigen::MatrixXf& points_to_cluster, + int number_of_clusters, + Eigen::MatrixXi& io_labels, + TermCriteria criteria, + int attempts, + int flags, + Eigen::MatrixXf& cluster_centers) +{ + const int spp_trials = 3; + std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols (); + int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1; + + attempts = std::max (attempts, 1); + srand (static_cast (time (nullptr))); + + Eigen::MatrixXi labels (number_of_points, 1); + + if (flags & USE_INITIAL_LABELS) + labels = io_labels; + else + labels.setZero (); + + Eigen::MatrixXf centers (number_of_clusters, feature_dimension); + Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension); + std::vector counters (number_of_clusters); + std::vector > boxes (feature_dimension); + Eigen::Vector2f* box = &boxes[0]; + + double best_compactness = std::numeric_limits::max (); + double compactness = 0.0; + + if (criteria.type_ & TermCriteria::EPS) + criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f); + else + criteria.epsilon_ = std::numeric_limits::epsilon (); + + criteria.epsilon_ *= criteria.epsilon_; + + if (criteria.type_ & TermCriteria::COUNT) + criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100); + else + criteria.max_count_ = 100; + + if (number_of_clusters == 1) + { + attempts = 1; + criteria.max_count_ = 2; + } + + for (int i_dim = 0; i_dim < feature_dimension; i_dim++) + box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim)); + + for (std::size_t i_point = 0; i_point < number_of_points; i_point++) + for (int i_dim = 0; i_dim < feature_dimension; i_dim++) + { + float v = points_to_cluster (i_point, i_dim); + box[i_dim] (0) = std::min (box[i_dim] (0), v); + box[i_dim] (1) = std::max (box[i_dim] (1), v); + } + + for (int i_attempt = 0; i_attempt < attempts; i_attempt++) + { + float max_center_shift = std::numeric_limits::max (); + for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++) + { + Eigen::MatrixXf temp (centers.rows (), centers.cols ()); + temp = centers; + centers = old_centers; + old_centers = temp; + + if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) ) + { + if (flags & PP_CENTERS) + generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials); + else + { + for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++) + { + Eigen::VectorXf center (feature_dimension); + generateRandomCenter (boxes, center); + for (int i_dim = 0; i_dim < feature_dimension; i_dim++) + centers (i_cl_center, i_dim) = center (i_dim); + }//generate center for next cluster + }//end if-else random or PP centers + } + else + { + centers.setZero (); + for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) + counters[i_cluster] = 0; + for (std::size_t i_point = 0; i_point < number_of_points; i_point++) + { + int i_label = labels (i_point, 0); + for (int i_dim = 0; i_dim < feature_dimension; i_dim++) + centers (i_label, i_dim) += points_to_cluster (i_point, i_dim); + counters[i_label]++; + } + if (iter > 0) + max_center_shift = 0.0f; + for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++) + { + if (counters[i_cl_center] != 0) + { + float scale = 1.0f / static_cast (counters[i_cl_center]); + for (int i_dim = 0; i_dim < feature_dimension; i_dim++) + centers (i_cl_center, i_dim) *= scale; + } + else + { + Eigen::VectorXf center (feature_dimension); + generateRandomCenter (boxes, center); + for(int i_dim = 0; i_dim < feature_dimension; i_dim++) + centers (i_cl_center, i_dim) = center (i_dim); + } + + if (iter > 0) + { + float dist = 0.0f; + for (int i_dim = 0; i_dim < feature_dimension; i_dim++) + { + float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim); + dist += diff * diff; + } + max_center_shift = std::max (max_center_shift, dist); + } + } + } + compactness = 0.0f; + for (std::size_t i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::VectorXf sample (feature_dimension); + sample = points_to_cluster.row (i_point); + + int k_best = 0; + float min_dist = std::numeric_limits::max (); + + for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) + { + Eigen::VectorXf center (feature_dimension); + center = centers.row (i_cluster); + float dist = computeDistance (sample, center); + if (min_dist > dist) + { + min_dist = dist; + k_best = i_cluster; + } + } + compactness += min_dist; + labels (i_point, 0) = k_best; + } + }//next iteration + + if (compactness < best_compactness) + { + best_compactness = compactness; + cluster_centers = centers; + io_labels = labels; + } + }//next attempt + + return (best_compactness); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::generateCentersPP ( + const Eigen::MatrixXf& data, + Eigen::MatrixXf& out_centers, + int number_of_clusters, + int trials) +{ + std::size_t dimension = data.cols (); + unsigned int number_of_points = static_cast (data.rows ()); + std::vector centers_vec (number_of_clusters); + int* centers = ¢ers_vec[0]; + std::vector dist (number_of_points); + std::vector tdist (number_of_points); + std::vector tdist2 (number_of_points); + double sum0 = 0.0; + + unsigned int random_unsigned = rand (); + centers[0] = random_unsigned % number_of_points; + + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::VectorXf first (dimension); + Eigen::VectorXf second (dimension); + first = data.row (i_point); + second = data.row (centers[0]); + dist[i_point] = computeDistance (first, second); + sum0 += dist[i_point]; + } + + for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) + { + double best_sum = std::numeric_limits::max (); + int best_center = -1; + for (int i_trials = 0; i_trials < trials; i_trials++) + { + unsigned int random_integer = rand () - 1; + double random_double = static_cast (random_integer) / static_cast (std::numeric_limits::max ()); + double p = random_double * sum0; + + unsigned int i_point; + for (i_point = 0; i_point < number_of_points - 1; i_point++) + if ( (p -= dist[i_point]) <= 0.0) + break; + + int ci = i_point; + + double s = 0.0; + for (unsigned int i_point = 0; i_point < number_of_points; i_point++) + { + Eigen::VectorXf first (dimension); + Eigen::VectorXf second (dimension); + first = data.row (i_point); + second = data.row (ci); + tdist2[i_point] = std::min (static_cast (computeDistance (first, second)), dist[i_point]); + s += tdist2[i_point]; + } + + if (s <= best_sum) + { + best_sum = s; + best_center = ci; + std::swap (tdist, tdist2); + } + } + + centers[i_cluster] = best_center; + sum0 = best_sum; + std::swap (dist, tdist); + } + + for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++) + for (std::size_t i_dim = 0; i_dim < dimension; i_dim++) + out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ism::ImplicitShapeModelEstimation::generateRandomCenter (const std::vector >& boxes, + Eigen::VectorXf& center) +{ + std::size_t dimension = boxes.size (); + float margin = 1.0f / static_cast (dimension); + + for (std::size_t i_dim = 0; i_dim < dimension; i_dim++) + { + unsigned int random_integer = rand () - 1; + float random_float = static_cast (random_integer) / static_cast (std::numeric_limits::max ()); + center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::ism::ImplicitShapeModelEstimation::computeDistance (Eigen::VectorXf& vec_1, Eigen::VectorXf& vec_2) +{ + std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols (); + float distance = 0.0f; + for(std::size_t i_dim = 0; i_dim < dimension; i_dim++) + { + float diff = vec_1 (i_dim) - vec_2 (i_dim); + distance += diff * diff; + } + + return (distance); +} + +#endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/line_rgbd.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/line_rgbd.hpp new file mode 100755 index 0000000..b89f3a3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/line_rgbd.hpp @@ -0,0 +1,979 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ +#define PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ + +//#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::LineRGBD::readLTMHeader (int fd, pcl::io::TARHeader &header) +{ + // Read in the header + int result = static_cast (::read (fd, reinterpret_cast (&header.file_name[0]), 512)); + if (result == -1) + return (false); + + // We only support regular files for now. + // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices, + // directories, and named pipes. + if (header.file_type[0] != '0' && header.file_type[0] != '\0') + return (false); + + // We only support USTAR version 0 files for now + if (std::string (header.ustar).substr (0, 5) != "ustar") + return (false); + + if (header.getFileSize () == 0) + return (false); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::LineRGBD::loadTemplates (const std::string &file_name, const std::size_t object_id) +{ + // Open the file + int ltm_fd = io::raw_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 (::read (ltm_fd, reinterpret_cast (&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 (io::raw_lseek(ltm_fd, ltm_offset, SEEK_SET) < 0) + break; + } + + // Close the file + io::raw_close(ltm_fd); + + // Compute 3D bounding boxes from the template point clouds + bounding_boxes_.resize (template_point_clouds_.size ()); + for (std::size_t i = 0; i < template_point_clouds_.size (); ++i) + { + PointCloud & template_point_cloud = template_point_clouds_[i]; + BoundingBoxXYZ & bb = bounding_boxes_[i]; + bb.x = bb.y = bb.z = std::numeric_limits::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::max (); + float min_y = std::numeric_limits::max (); + float min_z = std::numeric_limits::max (); + float max_x = -std::numeric_limits::max (); + float max_y = -std::numeric_limits::max (); + float max_z = -std::numeric_limits::max (); + std::size_t counter = 0; + for (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + const PointXYZRGBA & p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::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 (counter); + center_y /= static_cast (counter); + center_z /= static_cast (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 (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + PointXYZRGBA p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z)) + continue; + + p.x -= center_x; + p.y -= center_y; + p.z -= center_z; + + template_point_cloud.points[j] = p; + } + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::LineRGBD::createAndAddTemplate ( + pcl::PointCloud & cloud, + const std::size_t object_id, + const MaskMap & mask_xyz, + const MaskMap & mask_rgb, + const RegionXY & region) +{ + // add point cloud + template_point_clouds_.resize (template_point_clouds_.size () + 1); + pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]); + + // add template + object_ids_.push_back (object_id); + + // Compute 3D bounding boxes from the template point clouds + bounding_boxes_.resize (template_point_clouds_.size ()); + { + const std::size_t i = template_point_clouds_.size () - 1; + + PointCloud & template_point_cloud = template_point_clouds_[i]; + BoundingBoxXYZ & bb = bounding_boxes_[i]; + bb.x = bb.y = bb.z = std::numeric_limits::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::max (); + float min_y = std::numeric_limits::max (); + float min_z = std::numeric_limits::max (); + float max_x = -std::numeric_limits::max (); + float max_y = -std::numeric_limits::max (); + float max_z = -std::numeric_limits::max (); + std::size_t counter = 0; + for (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + const PointXYZRGBA & p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::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 (counter); + center_y /= static_cast (counter); + center_z /= static_cast (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 (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + PointXYZRGBA p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z)) + continue; + + p.x -= center_x; + p.y -= center_y; + p.z -= center_z; + + template_point_cloud.points[j] = p; + } + } + + std::vector modalities; + modalities.push_back (&color_gradient_mod_); + modalities.push_back (&surface_normal_mod_); + + std::vector masks; + masks.push_back (const_cast (&mask_rgb)); + masks.push_back (const_cast (&mask_xyz)); + + return (linemod_.createAndAddTemplate (modalities, masks, region)); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::LineRGBD::addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud & cloud, std::size_t object_id) +{ + // add point cloud + template_point_clouds_.resize (template_point_clouds_.size () + 1); + pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]); + + // add template + linemod_.addTemplate (sqmmt); + object_ids_.push_back (object_id); + + // Compute 3D bounding boxes from the template point clouds + bounding_boxes_.resize (template_point_clouds_.size ()); + { + const std::size_t i = template_point_clouds_.size () - 1; + + PointCloud & template_point_cloud = template_point_clouds_[i]; + BoundingBoxXYZ & bb = bounding_boxes_[i]; + bb.x = bb.y = bb.z = std::numeric_limits::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::max (); + float min_y = std::numeric_limits::max (); + float min_z = std::numeric_limits::max (); + float max_x = -std::numeric_limits::max (); + float max_y = -std::numeric_limits::max (); + float max_z = -std::numeric_limits::max (); + std::size_t counter = 0; + for (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + const PointXYZRGBA & p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::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 (counter); + center_y /= static_cast (counter); + center_z /= static_cast (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 (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + PointXYZRGBA p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z)) + continue; + + p.x -= center_x; + p.y -= center_y; + p.z -= center_z; + + template_point_cloud.points[j] = p; + } + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::detect ( + std::vector::Detection> & detections) +{ + std::vector modalities; + modalities.push_back (&color_gradient_mod_); + modalities.push_back (&surface_normal_mod_); + + std::vector linemod_detections; + linemod_.detectTemplates (modalities, linemod_detections); + + detections_.clear (); + detections_.reserve (linemod_detections.size ()); + detections.clear (); + detections.reserve (linemod_detections.size ()); + for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id) + { + pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id]; + + typename pcl::LineRGBD::Detection detection; + detection.template_id = linemod_detection.template_id; + detection.object_id = object_ids_[linemod_detection.template_id]; + detection.detection_id = detection_id; + detection.response = linemod_detection.score; + + // compute bounding box: + // we assume that the bounding boxes of the templates are relative to the center of mass + // of the template points; so we also compute the center of mass of the points + // covered by the + + const pcl::SparseQuantizedMultiModTemplate & linemod_template = + linemod_.getTemplate (linemod_detection.template_id); + + const std::size_t start_x = std::max (linemod_detection.x, 0); + const std::size_t start_y = std::max (linemod_detection.y, 0); + const std::size_t end_x = std::min (static_cast (start_x + linemod_template.region.width), + static_cast (cloud_xyz_->width)); + const std::size_t end_y = std::min (static_cast (start_y + linemod_template.region.height), + static_cast (cloud_xyz_->height)); + + detection.region.x = linemod_detection.x; + detection.region.y = linemod_detection.y; + detection.region.width = linemod_template.region.width; + detection.region.height = linemod_template.region.height; + + //std::cerr << "detection region: " << linemod_detection.x << ", " + // << linemod_detection.y << ", " + // << linemod_template.region.width << ", " + // << linemod_template.region.height << std::endl; + + float center_x = 0.0f; + float center_y = 0.0f; + float center_z = 0.0f; + std::size_t counter = 0; + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z)) + { + center_x += point.x; + center_y += point.y; + center_z += point.z; + ++counter; + } + } + } + const float inv_counter = 1.0f / static_cast (counter); + center_x *= inv_counter; + center_y *= inv_counter; + center_z *= inv_counter; + + pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id]; + + detection.bounding_box = template_bounding_box; + detection.bounding_box.x += center_x; + detection.bounding_box.y += center_y; + detection.bounding_box.z += center_z; + + detections_.push_back (detection); + } + + // refine detections along depth + refineDetectionsAlongDepth (); + //applyprojectivedepthicpondetections(); + + // remove overlaps + removeOverlappingDetections (); + + for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index) + { + detections.push_back (detections_[detection_index]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::detectSemiScaleInvariant ( + std::vector::Detection> & detections, + const float min_scale, + const float max_scale, + const float scale_multiplier) +{ + std::vector modalities; + modalities.push_back (&color_gradient_mod_); + modalities.push_back (&surface_normal_mod_); + + std::vector linemod_detections; + linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier); + + detections_.clear (); + detections_.reserve (linemod_detections.size ()); + detections.clear (); + detections.reserve (linemod_detections.size ()); + for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id) + { + pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id]; + + typename pcl::LineRGBD::Detection detection; + detection.template_id = linemod_detection.template_id; + detection.object_id = object_ids_[linemod_detection.template_id]; + detection.detection_id = detection_id; + detection.response = linemod_detection.score; + + // compute bounding box: + // we assume that the bounding boxes of the templates are relative to the center of mass + // of the template points; so we also compute the center of mass of the points + // covered by the + + const pcl::SparseQuantizedMultiModTemplate & linemod_template = + linemod_.getTemplate (linemod_detection.template_id); + + const std::size_t start_x = std::max (linemod_detection.x, 0); + const std::size_t start_y = std::max (linemod_detection.y, 0); + const std::size_t end_x = std::min (static_cast (start_x + linemod_template.region.width * linemod_detection.scale), + static_cast (cloud_xyz_->width)); + const std::size_t end_y = std::min (static_cast (start_y + linemod_template.region.height * linemod_detection.scale), + static_cast (cloud_xyz_->height)); + + detection.region.x = linemod_detection.x; + detection.region.y = linemod_detection.y; + detection.region.width = linemod_template.region.width * linemod_detection.scale; + detection.region.height = linemod_template.region.height * linemod_detection.scale; + + //std::cerr << "detection region: " << linemod_detection.x << ", " + // << linemod_detection.y << ", " + // << linemod_template.region.width << ", " + // << linemod_template.region.height << std::endl; + + float center_x = 0.0f; + float center_y = 0.0f; + float center_z = 0.0f; + std::size_t counter = 0; + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z)) + { + center_x += point.x; + center_y += point.y; + center_z += point.z; + ++counter; + } + } + } + const float inv_counter = 1.0f / static_cast (counter); + center_x *= inv_counter; + center_y *= inv_counter; + center_z *= inv_counter; + + pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id]; + + detection.bounding_box = template_bounding_box; + detection.bounding_box.x += center_x; + detection.bounding_box.y += center_y; + detection.bounding_box.z += center_z; + + detections_.push_back (detection); + } + + // refine detections along depth + //refineDetectionsAlongDepth (); + //applyProjectiveDepthICPOnDetections(); + + // remove overlaps + removeOverlappingDetections (); + + for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index) + { + detections.push_back (detections_[detection_index]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::computeTransformedTemplatePoints ( + const std::size_t detection_id, pcl::PointCloud &cloud) +{ + if (detection_id >= detections_.size ()) + PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); + + const std::size_t template_id = detections_[detection_id].template_id; + const pcl::PointCloud & template_point_cloud = template_point_clouds_[template_id]; + + const pcl::BoundingBoxXYZ & template_bounding_box = bounding_boxes_[template_id]; + const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box; + + //std::cerr << "detection: " + // << detection_bounding_box.x << ", " + // << detection_bounding_box.y << ", " + // << detection_bounding_box.z << std::endl; + //std::cerr << "template: " + // << template_bounding_box.x << ", " + // << template_bounding_box.y << ", " + // << template_bounding_box.z << std::endl; + const float translation_x = detection_bounding_box.x - template_bounding_box.x; + const float translation_y = detection_bounding_box.y - template_bounding_box.y; + const float translation_z = detection_bounding_box.z - template_bounding_box.z; + + //std::cerr << "translation: " + // << translation_x << ", " + // << translation_y << ", " + // << translation_z << std::endl; + + const std::size_t nr_points = template_point_cloud.size (); + cloud.resize (nr_points); + cloud.width = template_point_cloud.width; + cloud.height = template_point_cloud.height; + for (std::size_t point_index = 0; point_index < nr_points; ++point_index) + { + pcl::PointXYZRGBA point = template_point_cloud.points[point_index]; + + point.x += translation_x; + point.y += translation_y; + point.z += translation_z; + + cloud.points[point_index] = point; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::refineDetectionsAlongDepth () +{ + const std::size_t nr_detections = detections_.size (); + for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) + { + typename LineRGBD::Detection & detection = detections_[detection_index]; + + // find depth with most valid points + const std::size_t start_x = std::max (detection.region.x, 0); + const std::size_t start_y = std::max (detection.region.y, 0); + const std::size_t end_x = std::min (static_cast (detection.region.x + detection.region.width), + static_cast (cloud_xyz_->width)); + const std::size_t end_y = std::min (static_cast (detection.region.y + detection.region.height), + static_cast (cloud_xyz_->height)); + + + float min_depth = std::numeric_limits::max (); + float max_depth = -std::numeric_limits::max (); + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (/*std::isfinite (point.x) && std::isfinite (point.y) && */std::isfinite (point.z)) + { + min_depth = std::min (min_depth, point.z); + max_depth = std::max (max_depth, point.z); + } + } + } + + const std::size_t nr_bins = 1000; + const float step_size = (max_depth - min_depth) / static_cast (nr_bins-1); + std::vector depth_bins (nr_bins, 0); + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (/*std::isfinite (point.x) && std::isfinite (point.y) && */std::isfinite (point.z)) + { + const std::size_t bin_index = static_cast ((point.z - min_depth) / step_size); + ++depth_bins[bin_index]; + } + } + } + + std::vector integral_depth_bins (nr_bins, 0); + + integral_depth_bins[0] = depth_bins[0]; + for (std::size_t bin_index = 1; bin_index < nr_bins; ++bin_index) + { + integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1]; + } + + const std::size_t bb_depth_range = static_cast (detection.bounding_box.depth / step_size); + + std::size_t max_nr_points = 0; + std::size_t max_index = 0; + for (std::size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index) + { + const std::size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index]; + + if (nr_points_in_range > max_nr_points) + { + max_nr_points = nr_points_in_range; + max_index = bin_index; + } + } + + const float z = static_cast (max_index) * step_size + min_depth; + + detection.bounding_box.z = z; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::applyProjectiveDepthICPOnDetections () +{ + const std::size_t nr_detections = detections_.size (); + for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) + { + typename pcl::LineRGBD::Detection & detection = detections_[detection_index]; + + const std::size_t template_id = detection.template_id; + pcl::PointCloud & point_cloud = template_point_clouds_[template_id]; + + const std::size_t start_x = detection.region.x; + const std::size_t start_y = detection.region.y; + const std::size_t pc_width = point_cloud.width; + const std::size_t pc_height = point_cloud.height; + + std::vector > depth_matches; + for (std::size_t row_index = 0; row_index < pc_height; ++row_index) + { + for (std::size_t col_index = 0; col_index < pc_width; ++col_index) + { + const pcl::PointXYZRGBA & point_template = point_cloud (col_index, row_index); + const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y); + + if (!std::isfinite (point_template.z) || !std::isfinite (point_input.z)) + continue; + + depth_matches.push_back (std::make_pair (point_template.z, point_input.z)); + } + } + + // apply ransac on matches + const std::size_t nr_matches = depth_matches.size (); + const std::size_t nr_iterations = 100; // todo: should be a parameter... + const float inlier_threshold = 0.01f; // 5cm // todo: should be a parameter... + std::size_t best_nr_inliers = 0; + float best_z_translation = 0.0f; + for (std::size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index) + { + const std::size_t rand_index = (rand () * nr_matches) / RAND_MAX; + + const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first; + + std::size_t nr_inliers = 0; + for (std::size_t match_index = 0; match_index < nr_matches; ++match_index) + { + const float error = std::abs (depth_matches[match_index].first + z_translation - depth_matches[match_index].second); + + if (error <= inlier_threshold) + { + ++nr_inliers; + } + } + + if (nr_inliers > best_nr_inliers) + { + best_nr_inliers = nr_inliers; + best_z_translation = z_translation; + } + } + + float average_depth = 0.0f; + std::size_t average_counter = 0; + for (std::size_t match_index = 0; match_index < nr_matches; ++match_index) + { + const float error = std::abs (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second); + + if (error <= inlier_threshold) + { + //average_depth += depth_matches[match_index].second; + average_depth += depth_matches[match_index].second - depth_matches[match_index].first; + ++average_counter; + } + } + average_depth /= static_cast (average_counter); + + detection.bounding_box.z = bounding_boxes_[template_id].z + average_depth;// - detection.bounding_box.depth/2.0f; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::removeOverlappingDetections () +{ + // compute overlap between each detection + const std::size_t nr_detections = detections_.size (); + Eigen::MatrixXf overlaps (nr_detections, nr_detections); + for (std::size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1) + { + for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2) + { + const float bounding_box_volume = detections_[detection_index_1].bounding_box.width + * detections_[detection_index_1].bounding_box.height + * detections_[detection_index_1].bounding_box.depth; + + if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id) + overlaps (detection_index_1, detection_index_2) = 0.0f; + else + overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume ( + detections_[detection_index_1].bounding_box, + detections_[detection_index_2].bounding_box) / bounding_box_volume; + } + } + + // create clusters of detections + std::vector detection_to_cluster_mapping (nr_detections, -1); + std::vector > clusters; + for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) + { + if (detection_to_cluster_mapping[detection_index] != -1) + continue; // already part of a cluster + + std::vector cluster; + const std::size_t cluster_id = clusters.size (); + + cluster.push_back (detection_index); + detection_to_cluster_mapping[detection_index] = static_cast (cluster_id); + + // check for detections which have sufficient overlap with a detection in the cluster + for (std::size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index) + { + const std::size_t detection_index_1 = cluster[cluster_index]; + + for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2) + { + if (detection_to_cluster_mapping[detection_index_2] != -1) + continue; // already part of a cluster + + if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_) + continue; // not enough overlap + + cluster.push_back (detection_index_2); + detection_to_cluster_mapping[detection_index_2] = static_cast (cluster_id); + } + } + + clusters.push_back (cluster); + } + + // compute detection representatives for every cluster + std::vector::Detection> clustered_detections; + + const std::size_t nr_clusters = clusters.size (); + for (std::size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id) + { + std::vector & cluster = clusters[cluster_id]; + + float average_center_x = 0.0f; + float average_center_y = 0.0f; + float average_center_z = 0.0f; + float weight_sum = 0.0f; + + float best_response = 0.0f; + std::size_t best_detection_id = 0; + + float average_region_x = 0.0f; + float average_region_y = 0.0f; + + const std::size_t elements_in_cluster = cluster.size (); + for (std::size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index) + { + const std::size_t detection_id = cluster[cluster_index]; + + const float weight = detections_[detection_id].response; + + if (weight > best_response) + { + best_response = weight; + best_detection_id = detection_id; + } + + const Detection & d = detections_[detection_id]; + const float p_center_x = d.bounding_box.x + d.bounding_box.width / 2.0f; + const float p_center_y = d.bounding_box.y + d.bounding_box.height / 2.0f; + const float p_center_z = d.bounding_box.z + d.bounding_box.depth / 2.0f; + + average_center_x += p_center_x * weight; + average_center_y += p_center_y * weight; + average_center_z += p_center_z * weight; + weight_sum += weight; + + average_region_x += float (detections_[detection_id].region.x) * weight; + average_region_y += float (detections_[detection_id].region.y) * weight; + } + + typename LineRGBD::Detection detection; + detection.template_id = detections_[best_detection_id].template_id; + detection.object_id = detections_[best_detection_id].object_id; + detection.detection_id = cluster_id; + detection.response = best_response; + + const float inv_weight_sum = 1.0f / weight_sum; + const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width; + const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height; + const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth; + + detection.bounding_box.x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f; + detection.bounding_box.y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f; + detection.bounding_box.z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f; + detection.bounding_box.width = best_detection_bb_width; + detection.bounding_box.height = best_detection_bb_height; + detection.bounding_box.depth = best_detection_bb_depth; + + detection.region.x = int (average_region_x * inv_weight_sum); + detection.region.y = int (average_region_y * inv_weight_sum); + detection.region.width = detections_[best_detection_id].region.width; + detection.region.height = detections_[best_detection_id].region.height; + + clustered_detections.push_back (detection); + } + + detections_ = clustered_detections; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::LineRGBD::computeBoundingBoxIntersectionVolume ( + const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2) +{ + const float x1_min = box1.x; + const float y1_min = box1.y; + const float z1_min = box1.z; + const float x1_max = box1.x + box1.width; + const float y1_max = box1.y + box1.height; + const float z1_max = box1.z + box1.depth; + + const float x2_min = box2.x; + const float y2_min = box2.y; + const float z2_min = box2.z; + const float x2_max = box2.x + box2.width; + const float y2_max = box2.y + box2.height; + const float z2_max = box2.z + box2.depth; + + const float xi_min = std::max (x1_min, x2_min); + const float yi_min = std::max (y1_min, y2_min); + const float zi_min = std::max (z1_min, z2_min); + + const float xi_max = std::min (x1_max, x2_max); + const float yi_max = std::min (y1_max, y2_max); + const float zi_max = std::min (z1_max, z2_max); + + const float intersection_width = xi_max - xi_min; + const float intersection_height = yi_max - yi_min; + const float intersection_depth = zi_max - zi_min; + + if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f) + return 0.0f; + + const float intersection_volume = intersection_width * intersection_height * intersection_depth; + + return (intersection_volume); +} + + +#endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/linemod/line_rgbd.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/linemod/line_rgbd.hpp new file mode 100755 index 0000000..b89f3a3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/linemod/line_rgbd.hpp @@ -0,0 +1,979 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ +#define PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ + +//#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::LineRGBD::readLTMHeader (int fd, pcl::io::TARHeader &header) +{ + // Read in the header + int result = static_cast (::read (fd, reinterpret_cast (&header.file_name[0]), 512)); + if (result == -1) + return (false); + + // We only support regular files for now. + // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices, + // directories, and named pipes. + if (header.file_type[0] != '0' && header.file_type[0] != '\0') + return (false); + + // We only support USTAR version 0 files for now + if (std::string (header.ustar).substr (0, 5) != "ustar") + return (false); + + if (header.getFileSize () == 0) + return (false); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::LineRGBD::loadTemplates (const std::string &file_name, const std::size_t object_id) +{ + // Open the file + int ltm_fd = io::raw_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 (::read (ltm_fd, reinterpret_cast (&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 (io::raw_lseek(ltm_fd, ltm_offset, SEEK_SET) < 0) + break; + } + + // Close the file + io::raw_close(ltm_fd); + + // Compute 3D bounding boxes from the template point clouds + bounding_boxes_.resize (template_point_clouds_.size ()); + for (std::size_t i = 0; i < template_point_clouds_.size (); ++i) + { + PointCloud & template_point_cloud = template_point_clouds_[i]; + BoundingBoxXYZ & bb = bounding_boxes_[i]; + bb.x = bb.y = bb.z = std::numeric_limits::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::max (); + float min_y = std::numeric_limits::max (); + float min_z = std::numeric_limits::max (); + float max_x = -std::numeric_limits::max (); + float max_y = -std::numeric_limits::max (); + float max_z = -std::numeric_limits::max (); + std::size_t counter = 0; + for (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + const PointXYZRGBA & p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::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 (counter); + center_y /= static_cast (counter); + center_z /= static_cast (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 (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + PointXYZRGBA p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z)) + continue; + + p.x -= center_x; + p.y -= center_y; + p.z -= center_z; + + template_point_cloud.points[j] = p; + } + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::LineRGBD::createAndAddTemplate ( + pcl::PointCloud & cloud, + const std::size_t object_id, + const MaskMap & mask_xyz, + const MaskMap & mask_rgb, + const RegionXY & region) +{ + // add point cloud + template_point_clouds_.resize (template_point_clouds_.size () + 1); + pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]); + + // add template + object_ids_.push_back (object_id); + + // Compute 3D bounding boxes from the template point clouds + bounding_boxes_.resize (template_point_clouds_.size ()); + { + const std::size_t i = template_point_clouds_.size () - 1; + + PointCloud & template_point_cloud = template_point_clouds_[i]; + BoundingBoxXYZ & bb = bounding_boxes_[i]; + bb.x = bb.y = bb.z = std::numeric_limits::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::max (); + float min_y = std::numeric_limits::max (); + float min_z = std::numeric_limits::max (); + float max_x = -std::numeric_limits::max (); + float max_y = -std::numeric_limits::max (); + float max_z = -std::numeric_limits::max (); + std::size_t counter = 0; + for (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + const PointXYZRGBA & p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::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 (counter); + center_y /= static_cast (counter); + center_z /= static_cast (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 (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + PointXYZRGBA p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z)) + continue; + + p.x -= center_x; + p.y -= center_y; + p.z -= center_z; + + template_point_cloud.points[j] = p; + } + } + + std::vector modalities; + modalities.push_back (&color_gradient_mod_); + modalities.push_back (&surface_normal_mod_); + + std::vector masks; + masks.push_back (const_cast (&mask_rgb)); + masks.push_back (const_cast (&mask_xyz)); + + return (linemod_.createAndAddTemplate (modalities, masks, region)); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::LineRGBD::addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud & cloud, std::size_t object_id) +{ + // add point cloud + template_point_clouds_.resize (template_point_clouds_.size () + 1); + pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]); + + // add template + linemod_.addTemplate (sqmmt); + object_ids_.push_back (object_id); + + // Compute 3D bounding boxes from the template point clouds + bounding_boxes_.resize (template_point_clouds_.size ()); + { + const std::size_t i = template_point_clouds_.size () - 1; + + PointCloud & template_point_cloud = template_point_clouds_[i]; + BoundingBoxXYZ & bb = bounding_boxes_[i]; + bb.x = bb.y = bb.z = std::numeric_limits::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::max (); + float min_y = std::numeric_limits::max (); + float min_z = std::numeric_limits::max (); + float max_x = -std::numeric_limits::max (); + float max_y = -std::numeric_limits::max (); + float max_z = -std::numeric_limits::max (); + std::size_t counter = 0; + for (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + const PointXYZRGBA & p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::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 (counter); + center_y /= static_cast (counter); + center_z /= static_cast (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 (std::size_t j = 0; j < template_point_cloud.size (); ++j) + { + PointXYZRGBA p = template_point_cloud.points[j]; + + if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z)) + continue; + + p.x -= center_x; + p.y -= center_y; + p.z -= center_z; + + template_point_cloud.points[j] = p; + } + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::detect ( + std::vector::Detection> & detections) +{ + std::vector modalities; + modalities.push_back (&color_gradient_mod_); + modalities.push_back (&surface_normal_mod_); + + std::vector linemod_detections; + linemod_.detectTemplates (modalities, linemod_detections); + + detections_.clear (); + detections_.reserve (linemod_detections.size ()); + detections.clear (); + detections.reserve (linemod_detections.size ()); + for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id) + { + pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id]; + + typename pcl::LineRGBD::Detection detection; + detection.template_id = linemod_detection.template_id; + detection.object_id = object_ids_[linemod_detection.template_id]; + detection.detection_id = detection_id; + detection.response = linemod_detection.score; + + // compute bounding box: + // we assume that the bounding boxes of the templates are relative to the center of mass + // of the template points; so we also compute the center of mass of the points + // covered by the + + const pcl::SparseQuantizedMultiModTemplate & linemod_template = + linemod_.getTemplate (linemod_detection.template_id); + + const std::size_t start_x = std::max (linemod_detection.x, 0); + const std::size_t start_y = std::max (linemod_detection.y, 0); + const std::size_t end_x = std::min (static_cast (start_x + linemod_template.region.width), + static_cast (cloud_xyz_->width)); + const std::size_t end_y = std::min (static_cast (start_y + linemod_template.region.height), + static_cast (cloud_xyz_->height)); + + detection.region.x = linemod_detection.x; + detection.region.y = linemod_detection.y; + detection.region.width = linemod_template.region.width; + detection.region.height = linemod_template.region.height; + + //std::cerr << "detection region: " << linemod_detection.x << ", " + // << linemod_detection.y << ", " + // << linemod_template.region.width << ", " + // << linemod_template.region.height << std::endl; + + float center_x = 0.0f; + float center_y = 0.0f; + float center_z = 0.0f; + std::size_t counter = 0; + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z)) + { + center_x += point.x; + center_y += point.y; + center_z += point.z; + ++counter; + } + } + } + const float inv_counter = 1.0f / static_cast (counter); + center_x *= inv_counter; + center_y *= inv_counter; + center_z *= inv_counter; + + pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id]; + + detection.bounding_box = template_bounding_box; + detection.bounding_box.x += center_x; + detection.bounding_box.y += center_y; + detection.bounding_box.z += center_z; + + detections_.push_back (detection); + } + + // refine detections along depth + refineDetectionsAlongDepth (); + //applyprojectivedepthicpondetections(); + + // remove overlaps + removeOverlappingDetections (); + + for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index) + { + detections.push_back (detections_[detection_index]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::detectSemiScaleInvariant ( + std::vector::Detection> & detections, + const float min_scale, + const float max_scale, + const float scale_multiplier) +{ + std::vector modalities; + modalities.push_back (&color_gradient_mod_); + modalities.push_back (&surface_normal_mod_); + + std::vector linemod_detections; + linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier); + + detections_.clear (); + detections_.reserve (linemod_detections.size ()); + detections.clear (); + detections.reserve (linemod_detections.size ()); + for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id) + { + pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id]; + + typename pcl::LineRGBD::Detection detection; + detection.template_id = linemod_detection.template_id; + detection.object_id = object_ids_[linemod_detection.template_id]; + detection.detection_id = detection_id; + detection.response = linemod_detection.score; + + // compute bounding box: + // we assume that the bounding boxes of the templates are relative to the center of mass + // of the template points; so we also compute the center of mass of the points + // covered by the + + const pcl::SparseQuantizedMultiModTemplate & linemod_template = + linemod_.getTemplate (linemod_detection.template_id); + + const std::size_t start_x = std::max (linemod_detection.x, 0); + const std::size_t start_y = std::max (linemod_detection.y, 0); + const std::size_t end_x = std::min (static_cast (start_x + linemod_template.region.width * linemod_detection.scale), + static_cast (cloud_xyz_->width)); + const std::size_t end_y = std::min (static_cast (start_y + linemod_template.region.height * linemod_detection.scale), + static_cast (cloud_xyz_->height)); + + detection.region.x = linemod_detection.x; + detection.region.y = linemod_detection.y; + detection.region.width = linemod_template.region.width * linemod_detection.scale; + detection.region.height = linemod_template.region.height * linemod_detection.scale; + + //std::cerr << "detection region: " << linemod_detection.x << ", " + // << linemod_detection.y << ", " + // << linemod_template.region.width << ", " + // << linemod_template.region.height << std::endl; + + float center_x = 0.0f; + float center_y = 0.0f; + float center_z = 0.0f; + std::size_t counter = 0; + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z)) + { + center_x += point.x; + center_y += point.y; + center_z += point.z; + ++counter; + } + } + } + const float inv_counter = 1.0f / static_cast (counter); + center_x *= inv_counter; + center_y *= inv_counter; + center_z *= inv_counter; + + pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id]; + + detection.bounding_box = template_bounding_box; + detection.bounding_box.x += center_x; + detection.bounding_box.y += center_y; + detection.bounding_box.z += center_z; + + detections_.push_back (detection); + } + + // refine detections along depth + //refineDetectionsAlongDepth (); + //applyProjectiveDepthICPOnDetections(); + + // remove overlaps + removeOverlappingDetections (); + + for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index) + { + detections.push_back (detections_[detection_index]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::computeTransformedTemplatePoints ( + const std::size_t detection_id, pcl::PointCloud &cloud) +{ + if (detection_id >= detections_.size ()) + PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); + + const std::size_t template_id = detections_[detection_id].template_id; + const pcl::PointCloud & template_point_cloud = template_point_clouds_[template_id]; + + const pcl::BoundingBoxXYZ & template_bounding_box = bounding_boxes_[template_id]; + const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box; + + //std::cerr << "detection: " + // << detection_bounding_box.x << ", " + // << detection_bounding_box.y << ", " + // << detection_bounding_box.z << std::endl; + //std::cerr << "template: " + // << template_bounding_box.x << ", " + // << template_bounding_box.y << ", " + // << template_bounding_box.z << std::endl; + const float translation_x = detection_bounding_box.x - template_bounding_box.x; + const float translation_y = detection_bounding_box.y - template_bounding_box.y; + const float translation_z = detection_bounding_box.z - template_bounding_box.z; + + //std::cerr << "translation: " + // << translation_x << ", " + // << translation_y << ", " + // << translation_z << std::endl; + + const std::size_t nr_points = template_point_cloud.size (); + cloud.resize (nr_points); + cloud.width = template_point_cloud.width; + cloud.height = template_point_cloud.height; + for (std::size_t point_index = 0; point_index < nr_points; ++point_index) + { + pcl::PointXYZRGBA point = template_point_cloud.points[point_index]; + + point.x += translation_x; + point.y += translation_y; + point.z += translation_z; + + cloud.points[point_index] = point; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::refineDetectionsAlongDepth () +{ + const std::size_t nr_detections = detections_.size (); + for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) + { + typename LineRGBD::Detection & detection = detections_[detection_index]; + + // find depth with most valid points + const std::size_t start_x = std::max (detection.region.x, 0); + const std::size_t start_y = std::max (detection.region.y, 0); + const std::size_t end_x = std::min (static_cast (detection.region.x + detection.region.width), + static_cast (cloud_xyz_->width)); + const std::size_t end_y = std::min (static_cast (detection.region.y + detection.region.height), + static_cast (cloud_xyz_->height)); + + + float min_depth = std::numeric_limits::max (); + float max_depth = -std::numeric_limits::max (); + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (/*std::isfinite (point.x) && std::isfinite (point.y) && */std::isfinite (point.z)) + { + min_depth = std::min (min_depth, point.z); + max_depth = std::max (max_depth, point.z); + } + } + } + + const std::size_t nr_bins = 1000; + const float step_size = (max_depth - min_depth) / static_cast (nr_bins-1); + std::vector depth_bins (nr_bins, 0); + for (std::size_t row_index = start_y; row_index < end_y; ++row_index) + { + for (std::size_t col_index = start_x; col_index < end_x; ++col_index) + { + const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); + + if (/*std::isfinite (point.x) && std::isfinite (point.y) && */std::isfinite (point.z)) + { + const std::size_t bin_index = static_cast ((point.z - min_depth) / step_size); + ++depth_bins[bin_index]; + } + } + } + + std::vector integral_depth_bins (nr_bins, 0); + + integral_depth_bins[0] = depth_bins[0]; + for (std::size_t bin_index = 1; bin_index < nr_bins; ++bin_index) + { + integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1]; + } + + const std::size_t bb_depth_range = static_cast (detection.bounding_box.depth / step_size); + + std::size_t max_nr_points = 0; + std::size_t max_index = 0; + for (std::size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index) + { + const std::size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index]; + + if (nr_points_in_range > max_nr_points) + { + max_nr_points = nr_points_in_range; + max_index = bin_index; + } + } + + const float z = static_cast (max_index) * step_size + min_depth; + + detection.bounding_box.z = z; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::applyProjectiveDepthICPOnDetections () +{ + const std::size_t nr_detections = detections_.size (); + for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) + { + typename pcl::LineRGBD::Detection & detection = detections_[detection_index]; + + const std::size_t template_id = detection.template_id; + pcl::PointCloud & point_cloud = template_point_clouds_[template_id]; + + const std::size_t start_x = detection.region.x; + const std::size_t start_y = detection.region.y; + const std::size_t pc_width = point_cloud.width; + const std::size_t pc_height = point_cloud.height; + + std::vector > depth_matches; + for (std::size_t row_index = 0; row_index < pc_height; ++row_index) + { + for (std::size_t col_index = 0; col_index < pc_width; ++col_index) + { + const pcl::PointXYZRGBA & point_template = point_cloud (col_index, row_index); + const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y); + + if (!std::isfinite (point_template.z) || !std::isfinite (point_input.z)) + continue; + + depth_matches.push_back (std::make_pair (point_template.z, point_input.z)); + } + } + + // apply ransac on matches + const std::size_t nr_matches = depth_matches.size (); + const std::size_t nr_iterations = 100; // todo: should be a parameter... + const float inlier_threshold = 0.01f; // 5cm // todo: should be a parameter... + std::size_t best_nr_inliers = 0; + float best_z_translation = 0.0f; + for (std::size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index) + { + const std::size_t rand_index = (rand () * nr_matches) / RAND_MAX; + + const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first; + + std::size_t nr_inliers = 0; + for (std::size_t match_index = 0; match_index < nr_matches; ++match_index) + { + const float error = std::abs (depth_matches[match_index].first + z_translation - depth_matches[match_index].second); + + if (error <= inlier_threshold) + { + ++nr_inliers; + } + } + + if (nr_inliers > best_nr_inliers) + { + best_nr_inliers = nr_inliers; + best_z_translation = z_translation; + } + } + + float average_depth = 0.0f; + std::size_t average_counter = 0; + for (std::size_t match_index = 0; match_index < nr_matches; ++match_index) + { + const float error = std::abs (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second); + + if (error <= inlier_threshold) + { + //average_depth += depth_matches[match_index].second; + average_depth += depth_matches[match_index].second - depth_matches[match_index].first; + ++average_counter; + } + } + average_depth /= static_cast (average_counter); + + detection.bounding_box.z = bounding_boxes_[template_id].z + average_depth;// - detection.bounding_box.depth/2.0f; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::LineRGBD::removeOverlappingDetections () +{ + // compute overlap between each detection + const std::size_t nr_detections = detections_.size (); + Eigen::MatrixXf overlaps (nr_detections, nr_detections); + for (std::size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1) + { + for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2) + { + const float bounding_box_volume = detections_[detection_index_1].bounding_box.width + * detections_[detection_index_1].bounding_box.height + * detections_[detection_index_1].bounding_box.depth; + + if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id) + overlaps (detection_index_1, detection_index_2) = 0.0f; + else + overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume ( + detections_[detection_index_1].bounding_box, + detections_[detection_index_2].bounding_box) / bounding_box_volume; + } + } + + // create clusters of detections + std::vector detection_to_cluster_mapping (nr_detections, -1); + std::vector > clusters; + for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) + { + if (detection_to_cluster_mapping[detection_index] != -1) + continue; // already part of a cluster + + std::vector cluster; + const std::size_t cluster_id = clusters.size (); + + cluster.push_back (detection_index); + detection_to_cluster_mapping[detection_index] = static_cast (cluster_id); + + // check for detections which have sufficient overlap with a detection in the cluster + for (std::size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index) + { + const std::size_t detection_index_1 = cluster[cluster_index]; + + for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2) + { + if (detection_to_cluster_mapping[detection_index_2] != -1) + continue; // already part of a cluster + + if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_) + continue; // not enough overlap + + cluster.push_back (detection_index_2); + detection_to_cluster_mapping[detection_index_2] = static_cast (cluster_id); + } + } + + clusters.push_back (cluster); + } + + // compute detection representatives for every cluster + std::vector::Detection> clustered_detections; + + const std::size_t nr_clusters = clusters.size (); + for (std::size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id) + { + std::vector & cluster = clusters[cluster_id]; + + float average_center_x = 0.0f; + float average_center_y = 0.0f; + float average_center_z = 0.0f; + float weight_sum = 0.0f; + + float best_response = 0.0f; + std::size_t best_detection_id = 0; + + float average_region_x = 0.0f; + float average_region_y = 0.0f; + + const std::size_t elements_in_cluster = cluster.size (); + for (std::size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index) + { + const std::size_t detection_id = cluster[cluster_index]; + + const float weight = detections_[detection_id].response; + + if (weight > best_response) + { + best_response = weight; + best_detection_id = detection_id; + } + + const Detection & d = detections_[detection_id]; + const float p_center_x = d.bounding_box.x + d.bounding_box.width / 2.0f; + const float p_center_y = d.bounding_box.y + d.bounding_box.height / 2.0f; + const float p_center_z = d.bounding_box.z + d.bounding_box.depth / 2.0f; + + average_center_x += p_center_x * weight; + average_center_y += p_center_y * weight; + average_center_z += p_center_z * weight; + weight_sum += weight; + + average_region_x += float (detections_[detection_id].region.x) * weight; + average_region_y += float (detections_[detection_id].region.y) * weight; + } + + typename LineRGBD::Detection detection; + detection.template_id = detections_[best_detection_id].template_id; + detection.object_id = detections_[best_detection_id].object_id; + detection.detection_id = cluster_id; + detection.response = best_response; + + const float inv_weight_sum = 1.0f / weight_sum; + const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width; + const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height; + const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth; + + detection.bounding_box.x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f; + detection.bounding_box.y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f; + detection.bounding_box.z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f; + detection.bounding_box.width = best_detection_bb_width; + detection.bounding_box.height = best_detection_bb_height; + detection.bounding_box.depth = best_detection_bb_depth; + + detection.region.x = int (average_region_x * inv_weight_sum); + detection.region.y = int (average_region_y * inv_weight_sum); + detection.region.width = detections_[best_detection_id].region.width; + detection.region.height = detections_[best_detection_id].region.height; + + clustered_detections.push_back (detection); + } + + detections_ = clustered_detections; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::LineRGBD::computeBoundingBoxIntersectionVolume ( + const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2) +{ + const float x1_min = box1.x; + const float y1_min = box1.y; + const float z1_min = box1.z; + const float x1_max = box1.x + box1.width; + const float y1_max = box1.y + box1.height; + const float z1_max = box1.z + box1.depth; + + const float x2_min = box2.x; + const float y2_min = box2.y; + const float z2_min = box2.z; + const float x2_max = box2.x + box2.width; + const float y2_max = box2.y + box2.height; + const float z2_max = box2.z + box2.depth; + + const float xi_min = std::max (x1_min, x2_min); + const float yi_min = std::max (y1_min, y2_min); + const float zi_min = std::max (z1_min, z2_min); + + const float xi_max = std::min (x1_max, x2_max); + const float yi_max = std::min (y1_max, y2_max); + const float zi_max = std::min (z1_max, z2_max); + + const float intersection_width = xi_max - xi_min; + const float intersection_height = yi_max - yi_min; + const float intersection_depth = zi_max - zi_min; + + if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f) + return 0.0f; + + const float intersection_volume = intersection_width * intersection_height * intersection_depth; + + return (intersection_volume); +} + + +#endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/ransac_based/simple_octree.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/ransac_based/simple_octree.hpp new file mode 100755 index 0000000..89f8386 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/ransac_based/simple_octree.hpp @@ -0,0 +1,402 @@ +/* + * simple_octree.hpp + * + * Created on: Mar 12, 2013 + * Author: papazov + */ + +#ifndef SIMPLE_OCTREE_HPP_ +#define SIMPLE_OCTREE_HPP_ + +#include +#include + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::Node::Node () +: data_ (nullptr), + parent_ (nullptr), + children_(nullptr) +{} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::Node::~Node () +{ + this->deleteChildren (); + this->deleteData (); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::setCenter (const Scalar *c) +{ + center_[0] = c[0]; + center_[1] = c[1]; + center_[2] = c[2]; +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::setBounds (const Scalar *b) +{ + bounds_[0] = b[0]; + bounds_[1] = b[1]; + bounds_[2] = b[2]; + bounds_[3] = b[3]; + bounds_[4] = b[4]; + bounds_[5] = b[5]; +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::computeRadius () +{ + Scalar v[3] = {static_cast (0.5)*(bounds_[1]-bounds_[0]), + static_cast (0.5)*(bounds_[3]-bounds_[2]), + static_cast (0.5)*(bounds_[5]-bounds_[4])}; + + radius_ = static_cast (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); +} + +//=============================================================================================================================== + +template inline bool +pcl::recognition::SimpleOctree::Node::createChildren () +{ + if ( children_ ) + return (false); + + Scalar bounds[6], center[3], childside = static_cast (0.5)*(bounds_[1]-bounds_[0]); + children_ = new Node[8]; + + // Compute bounds and center for child 0, i.e., for (0,0,0) + bounds[0] = bounds_[0]; bounds[1] = center_[0]; + bounds[2] = bounds_[2]; bounds[3] = center_[1]; + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Compute the center of the new child + center[0] = 0.5f*(bounds[0] + bounds[1]); + center[1] = 0.5f*(bounds[2] + bounds[3]); + center[2] = 0.5f*(bounds[4] + bounds[5]); + // Save the results + children_[0].setBounds(bounds); + children_[0].setCenter(center); + + // Compute bounds and center for child 1, i.e., for (0,0,1) + bounds[4] = center_[2]; bounds[5] = bounds_[5]; + // Update the center + center[2] += childside; + // Save the results + children_[1].setBounds(bounds); + children_[1].setCenter(center); + + // Compute bounds and center for child 3, i.e., for (0,1,1) + bounds[2] = center_[1]; bounds[3] = bounds_[3]; + // Update the center + center[1] += childside; + // Save the results + children_[3].setBounds(bounds); + children_[3].setCenter(center); + + // Compute bounds and center for child 2, i.e., for (0,1,0) + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Update the center + center[2] -= childside; + // Save the results + children_[2].setBounds(bounds); + children_[2].setCenter(center); + + // Compute bounds and center for child 6, i.e., for (1,1,0) + bounds[0] = center_[0]; bounds[1] = bounds_[1]; + // Update the center + center[0] += childside; + // Save the results + children_[6].setBounds(bounds); + children_[6].setCenter(center); + + // Compute bounds and center for child 7, i.e., for (1,1,1) + bounds[4] = center_[2]; bounds[5] = bounds_[5]; + // Update the center + center[2] += childside; + // Save the results + children_[7].setBounds(bounds); + children_[7].setCenter(center); + + // Compute bounds and center for child 5, i.e., for (1,0,1) + bounds[2] = bounds_[2]; bounds[3] = center_[1]; + // Update the center + center[1] -= childside; + // Save the results + children_[5].setBounds(bounds); + children_[5].setCenter(center); + + // Compute bounds and center for child 4, i.e., for (1,0,0) + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Update the center + center[2] -= childside; + // Save the results + children_[4].setBounds(bounds); + children_[4].setCenter(center); + + for ( int i = 0 ; i < 8 ; ++i ) + { + children_[i].computeRadius(); + children_[i].setParent(this); + } + + return (true); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::deleteChildren () +{ + if ( children_ ) + { + delete[] children_; + children_ = nullptr; + } +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::deleteData () +{ + if ( data_ ) + { + delete data_; + data_ = nullptr; + } +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::makeNeighbors (Node* node) +{ + if ( !this->hasData () || !node->hasData () ) + return; + + this->full_leaf_neighbors_.insert (node); + node->full_leaf_neighbors_.insert (this); +} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::SimpleOctree () +: tree_levels_ (0), + root_ (nullptr) +{ +} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::~SimpleOctree () +{ + this->clear (); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::clear () +{ + if ( root_ ) + { + delete root_; + root_ = nullptr; + } + + full_leaves_.clear(); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::build (const Scalar* bounds, Scalar voxel_size, + NodeDataCreator* node_data_creator) +{ + if ( voxel_size <= 0 ) + return; + + this->clear(); + + voxel_size_ = voxel_size; + node_data_creator_ = node_data_creator; + + Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]); + Scalar center[3] = {static_cast (0.5)*(bounds[0]+bounds[1]), + static_cast (0.5)*(bounds[2]+bounds[3]), + static_cast (0.5)*(bounds[4]+bounds[5])}; + + Scalar arg = extent/voxel_size; + + // Compute the number of tree levels + if ( arg > 1 ) + tree_levels_ = static_cast (std::ceil (std::log (arg)/std::log (2.0)) + 0.5); + else + tree_levels_ = 0; + + // Compute the number of octree levels and the bounds of the root + Scalar half_root_side = static_cast (0.5f*pow (2.0, tree_levels_)*voxel_size); + + // Determine the bounding box of the octree + bounds_[0] = center[0] - half_root_side; + bounds_[1] = center[0] + half_root_side; + bounds_[2] = center[1] - half_root_side; + bounds_[3] = center[1] + half_root_side; + bounds_[4] = center[2] - half_root_side; + bounds_[5] = center[2] + half_root_side; + + // Create and initialize the root + root_ = new Node (); + root_->setCenter (center); + root_->setBounds (bounds_); + root_->setParent (nullptr); + root_->computeRadius (); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::createLeaf (Scalar x, Scalar y, Scalar z) +{ + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + Node* node = root_; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + node->createChildren (); + const Scalar *c = node->getCenter (); + int id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->hasData () ) + { + node->setData (node_data_creator_->create (node)); + this->insertNeighbors (node); + full_leaves_.push_back (node); + } + + return (node); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::getFullLeaf (int i, int j, int k) +{ + Scalar offset = 0.5f*voxel_size_; + Scalar p[3] = {bounds_[0] + offset + static_cast (i)*voxel_size_, + bounds_[2] + offset + static_cast (j)*voxel_size_, + bounds_[4] + offset + static_cast (k)*voxel_size_}; + + return (this->getFullLeaf (p[0], p[1], p[2])); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::getFullLeaf (Scalar x, Scalar y, Scalar z) +{ + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + Node* node = root_; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + if ( !node->hasChildren () ) + return (nullptr); + + const Scalar *c = node->getCenter (); + int id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->hasData () ) + return (nullptr); + + return (node); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::insertNeighbors (Node* node) +{ + const Scalar* c = node->getCenter (); + Scalar s = static_cast (0.5)*voxel_size_; + Node *neigh; + + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); +//neigh = this->getFullLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); +} + +//=============================================================================================================================== + +#endif /* SIMPLE_OCTREE_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/ransac_based/voxel_structure.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/ransac_based/voxel_structure.hpp new file mode 100755 index 0000000..93a00fc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/ransac_based/voxel_structure.hpp @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ +#define PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ + +template inline void +pcl::recognition::VoxelStructure::build (const REAL bounds[6], int num_of_voxels[3]) +{ + this->clear(); + + // Copy the bounds + bounds_[0] = bounds[0]; + bounds_[1] = bounds[1]; + bounds_[2] = bounds[2]; + bounds_[3] = bounds[3]; + bounds_[4] = bounds[4]; + bounds_[5] = bounds[5]; + + num_of_voxels_[0] = num_of_voxels[0]; + num_of_voxels_[1] = num_of_voxels[1]; + num_of_voxels_[2] = num_of_voxels[2]; + num_of_voxels_xy_plane_ = num_of_voxels[0]*num_of_voxels[1]; + total_num_of_voxels_ = num_of_voxels_xy_plane_*num_of_voxels[2]; + + // Allocate memory for the voxels + voxels_ = new T[total_num_of_voxels_]; + + // Compute the spacing between the voxels in x, y and z direction + spacing_[0] = (bounds[1]-bounds[0])/static_cast (num_of_voxels[0]); + spacing_[1] = (bounds[3]-bounds[2])/static_cast (num_of_voxels[1]); + spacing_[2] = (bounds[5]-bounds[4])/static_cast (num_of_voxels[2]); + + // Compute the center of the voxel with integer coordinates (0, 0, 0) + min_center_[0] = bounds_[0] + static_cast (0.5)*spacing_[0]; + min_center_[1] = bounds_[2] + static_cast (0.5)*spacing_[1]; + min_center_[2] = bounds_[4] + static_cast (0.5)*spacing_[2]; +} + +//================================================================================================================================ + +template inline T* +pcl::recognition::VoxelStructure::getVoxel (const REAL p[3]) +{ + if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) + return nullptr; + + int x = static_cast ((p[0] - bounds_[0])/spacing_[0]); + int y = static_cast ((p[1] - bounds_[2])/spacing_[1]); + int z = static_cast ((p[2] - bounds_[4])/spacing_[2]); + + return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; +} + +//================================================================================================================================ + +template inline T* +pcl::recognition::VoxelStructure::getVoxel (int x, int y, int z) const +{ + if ( x < 0 || x >= num_of_voxels_[0] ) return nullptr; + if ( y < 0 || y >= num_of_voxels_[1] ) return nullptr; + if ( z < 0 || z >= num_of_voxels_[2] ) return nullptr; + + return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; +} + +//================================================================================================================================ + +template inline int +pcl::recognition::VoxelStructure::getNeighbors (const REAL* p, T **neighs) const +{ + if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) + return 0; + + const int x = static_cast ((p[0] - bounds_[0])/spacing_[0]); + const int y = static_cast ((p[1] - bounds_[2])/spacing_[1]); + const int z = static_cast ((p[2] - bounds_[4])/spacing_[2]); + + const int x_m1 = x-1, x_p1 = x+1; + const int y_m1 = y-1, y_p1 = y+1; + const int z_m1 = z-1, z_p1 = z+1; + + T* voxel; + int num_neighs = 0; + + voxel = this->getVoxel (x_p1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + + voxel = this->getVoxel (x , y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y , z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + + voxel = this->getVoxel (x_m1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + + return num_neighs; +} + +//================================================================================================================================ + +#endif // PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/simple_octree.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/simple_octree.hpp new file mode 100755 index 0000000..89f8386 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/simple_octree.hpp @@ -0,0 +1,402 @@ +/* + * simple_octree.hpp + * + * Created on: Mar 12, 2013 + * Author: papazov + */ + +#ifndef SIMPLE_OCTREE_HPP_ +#define SIMPLE_OCTREE_HPP_ + +#include +#include + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::Node::Node () +: data_ (nullptr), + parent_ (nullptr), + children_(nullptr) +{} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::Node::~Node () +{ + this->deleteChildren (); + this->deleteData (); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::setCenter (const Scalar *c) +{ + center_[0] = c[0]; + center_[1] = c[1]; + center_[2] = c[2]; +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::setBounds (const Scalar *b) +{ + bounds_[0] = b[0]; + bounds_[1] = b[1]; + bounds_[2] = b[2]; + bounds_[3] = b[3]; + bounds_[4] = b[4]; + bounds_[5] = b[5]; +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::computeRadius () +{ + Scalar v[3] = {static_cast (0.5)*(bounds_[1]-bounds_[0]), + static_cast (0.5)*(bounds_[3]-bounds_[2]), + static_cast (0.5)*(bounds_[5]-bounds_[4])}; + + radius_ = static_cast (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); +} + +//=============================================================================================================================== + +template inline bool +pcl::recognition::SimpleOctree::Node::createChildren () +{ + if ( children_ ) + return (false); + + Scalar bounds[6], center[3], childside = static_cast (0.5)*(bounds_[1]-bounds_[0]); + children_ = new Node[8]; + + // Compute bounds and center for child 0, i.e., for (0,0,0) + bounds[0] = bounds_[0]; bounds[1] = center_[0]; + bounds[2] = bounds_[2]; bounds[3] = center_[1]; + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Compute the center of the new child + center[0] = 0.5f*(bounds[0] + bounds[1]); + center[1] = 0.5f*(bounds[2] + bounds[3]); + center[2] = 0.5f*(bounds[4] + bounds[5]); + // Save the results + children_[0].setBounds(bounds); + children_[0].setCenter(center); + + // Compute bounds and center for child 1, i.e., for (0,0,1) + bounds[4] = center_[2]; bounds[5] = bounds_[5]; + // Update the center + center[2] += childside; + // Save the results + children_[1].setBounds(bounds); + children_[1].setCenter(center); + + // Compute bounds and center for child 3, i.e., for (0,1,1) + bounds[2] = center_[1]; bounds[3] = bounds_[3]; + // Update the center + center[1] += childside; + // Save the results + children_[3].setBounds(bounds); + children_[3].setCenter(center); + + // Compute bounds and center for child 2, i.e., for (0,1,0) + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Update the center + center[2] -= childside; + // Save the results + children_[2].setBounds(bounds); + children_[2].setCenter(center); + + // Compute bounds and center for child 6, i.e., for (1,1,0) + bounds[0] = center_[0]; bounds[1] = bounds_[1]; + // Update the center + center[0] += childside; + // Save the results + children_[6].setBounds(bounds); + children_[6].setCenter(center); + + // Compute bounds and center for child 7, i.e., for (1,1,1) + bounds[4] = center_[2]; bounds[5] = bounds_[5]; + // Update the center + center[2] += childside; + // Save the results + children_[7].setBounds(bounds); + children_[7].setCenter(center); + + // Compute bounds and center for child 5, i.e., for (1,0,1) + bounds[2] = bounds_[2]; bounds[3] = center_[1]; + // Update the center + center[1] -= childside; + // Save the results + children_[5].setBounds(bounds); + children_[5].setCenter(center); + + // Compute bounds and center for child 4, i.e., for (1,0,0) + bounds[4] = bounds_[4]; bounds[5] = center_[2]; + // Update the center + center[2] -= childside; + // Save the results + children_[4].setBounds(bounds); + children_[4].setCenter(center); + + for ( int i = 0 ; i < 8 ; ++i ) + { + children_[i].computeRadius(); + children_[i].setParent(this); + } + + return (true); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::deleteChildren () +{ + if ( children_ ) + { + delete[] children_; + children_ = nullptr; + } +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::deleteData () +{ + if ( data_ ) + { + delete data_; + data_ = nullptr; + } +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::Node::makeNeighbors (Node* node) +{ + if ( !this->hasData () || !node->hasData () ) + return; + + this->full_leaf_neighbors_.insert (node); + node->full_leaf_neighbors_.insert (this); +} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::SimpleOctree () +: tree_levels_ (0), + root_ (nullptr) +{ +} + +//=============================================================================================================================== + +template inline +pcl::recognition::SimpleOctree::~SimpleOctree () +{ + this->clear (); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::clear () +{ + if ( root_ ) + { + delete root_; + root_ = nullptr; + } + + full_leaves_.clear(); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::build (const Scalar* bounds, Scalar voxel_size, + NodeDataCreator* node_data_creator) +{ + if ( voxel_size <= 0 ) + return; + + this->clear(); + + voxel_size_ = voxel_size; + node_data_creator_ = node_data_creator; + + Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]); + Scalar center[3] = {static_cast (0.5)*(bounds[0]+bounds[1]), + static_cast (0.5)*(bounds[2]+bounds[3]), + static_cast (0.5)*(bounds[4]+bounds[5])}; + + Scalar arg = extent/voxel_size; + + // Compute the number of tree levels + if ( arg > 1 ) + tree_levels_ = static_cast (std::ceil (std::log (arg)/std::log (2.0)) + 0.5); + else + tree_levels_ = 0; + + // Compute the number of octree levels and the bounds of the root + Scalar half_root_side = static_cast (0.5f*pow (2.0, tree_levels_)*voxel_size); + + // Determine the bounding box of the octree + bounds_[0] = center[0] - half_root_side; + bounds_[1] = center[0] + half_root_side; + bounds_[2] = center[1] - half_root_side; + bounds_[3] = center[1] + half_root_side; + bounds_[4] = center[2] - half_root_side; + bounds_[5] = center[2] + half_root_side; + + // Create and initialize the root + root_ = new Node (); + root_->setCenter (center); + root_->setBounds (bounds_); + root_->setParent (nullptr); + root_->computeRadius (); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::createLeaf (Scalar x, Scalar y, Scalar z) +{ + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + Node* node = root_; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + node->createChildren (); + const Scalar *c = node->getCenter (); + int id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->hasData () ) + { + node->setData (node_data_creator_->create (node)); + this->insertNeighbors (node); + full_leaves_.push_back (node); + } + + return (node); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::getFullLeaf (int i, int j, int k) +{ + Scalar offset = 0.5f*voxel_size_; + Scalar p[3] = {bounds_[0] + offset + static_cast (i)*voxel_size_, + bounds_[2] + offset + static_cast (j)*voxel_size_, + bounds_[4] + offset + static_cast (k)*voxel_size_}; + + return (this->getFullLeaf (p[0], p[1], p[2])); +} + +//=============================================================================================================================== + +template inline +typename pcl::recognition::SimpleOctree::Node* +pcl::recognition::SimpleOctree::getFullLeaf (Scalar x, Scalar y, Scalar z) +{ + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + Node* node = root_; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + if ( !node->hasChildren () ) + return (nullptr); + + const Scalar *c = node->getCenter (); + int id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->hasData () ) + return (nullptr); + + return (node); +} + +//=============================================================================================================================== + +template inline void +pcl::recognition::SimpleOctree::insertNeighbors (Node* node) +{ + const Scalar* c = node->getCenter (); + Scalar s = static_cast (0.5)*voxel_size_; + Node *neigh; + + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); +//neigh = this->getFullLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); +} + +//=============================================================================================================================== + +#endif /* SIMPLE_OCTREE_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/impl/voxel_structure.hpp b/deps_install/include/pcl-1.10/pcl/recognition/impl/voxel_structure.hpp new file mode 100755 index 0000000..93a00fc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/impl/voxel_structure.hpp @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ +#define PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ + +template inline void +pcl::recognition::VoxelStructure::build (const REAL bounds[6], int num_of_voxels[3]) +{ + this->clear(); + + // Copy the bounds + bounds_[0] = bounds[0]; + bounds_[1] = bounds[1]; + bounds_[2] = bounds[2]; + bounds_[3] = bounds[3]; + bounds_[4] = bounds[4]; + bounds_[5] = bounds[5]; + + num_of_voxels_[0] = num_of_voxels[0]; + num_of_voxels_[1] = num_of_voxels[1]; + num_of_voxels_[2] = num_of_voxels[2]; + num_of_voxels_xy_plane_ = num_of_voxels[0]*num_of_voxels[1]; + total_num_of_voxels_ = num_of_voxels_xy_plane_*num_of_voxels[2]; + + // Allocate memory for the voxels + voxels_ = new T[total_num_of_voxels_]; + + // Compute the spacing between the voxels in x, y and z direction + spacing_[0] = (bounds[1]-bounds[0])/static_cast (num_of_voxels[0]); + spacing_[1] = (bounds[3]-bounds[2])/static_cast (num_of_voxels[1]); + spacing_[2] = (bounds[5]-bounds[4])/static_cast (num_of_voxels[2]); + + // Compute the center of the voxel with integer coordinates (0, 0, 0) + min_center_[0] = bounds_[0] + static_cast (0.5)*spacing_[0]; + min_center_[1] = bounds_[2] + static_cast (0.5)*spacing_[1]; + min_center_[2] = bounds_[4] + static_cast (0.5)*spacing_[2]; +} + +//================================================================================================================================ + +template inline T* +pcl::recognition::VoxelStructure::getVoxel (const REAL p[3]) +{ + if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) + return nullptr; + + int x = static_cast ((p[0] - bounds_[0])/spacing_[0]); + int y = static_cast ((p[1] - bounds_[2])/spacing_[1]); + int z = static_cast ((p[2] - bounds_[4])/spacing_[2]); + + return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; +} + +//================================================================================================================================ + +template inline T* +pcl::recognition::VoxelStructure::getVoxel (int x, int y, int z) const +{ + if ( x < 0 || x >= num_of_voxels_[0] ) return nullptr; + if ( y < 0 || y >= num_of_voxels_[1] ) return nullptr; + if ( z < 0 || z >= num_of_voxels_[2] ) return nullptr; + + return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; +} + +//================================================================================================================================ + +template inline int +pcl::recognition::VoxelStructure::getNeighbors (const REAL* p, T **neighs) const +{ + if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) + return 0; + + const int x = static_cast ((p[0] - bounds_[0])/spacing_[0]); + const int y = static_cast ((p[1] - bounds_[2])/spacing_[1]); + const int z = static_cast ((p[2] - bounds_[4])/spacing_[2]); + + const int x_m1 = x-1, x_p1 = x+1; + const int y_m1 = y-1, y_p1 = y+1; + const int z_m1 = z-1, z_p1 = z+1; + + T* voxel; + int num_neighs = 0; + + voxel = this->getVoxel (x_p1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_p1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + + voxel = this->getVoxel (x , y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y , z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x , y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + + voxel = this->getVoxel (x_m1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; + voxel = this->getVoxel (x_m1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; + + return num_neighs; +} + +//================================================================================================================================ + +#endif // PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/recognition/implicit_shape_model.h b/deps_install/include/pcl-1.10/pcl/recognition/implicit_shape_model.h new file mode 100755 index 0000000..55723ae --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/implicit_shape_model.h @@ -0,0 +1,633 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief This struct is used for storing peak. */ + struct EIGEN_ALIGN16 ISMPeak + { + /** \brief Point were this peak is located. */ + PCL_ADD_POINT4D; + + /** \brief Density of this peak. */ + double density; + + /** \brief Determines which class this peak belongs. */ + int class_id; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + namespace features + { + /** \brief This class is used for storing, analyzing and manipulating votes + * obtained from ISM algorithm. */ + template + class PCL_EXPORTS ISMVoteList + { + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Empty constructor with member variables initialization. */ + ISMVoteList (); + + /** \brief virtual descriptor. */ + virtual + ~ISMVoteList (); + + /** \brief This method simply adds another vote to the list. + * \param[in] in_vote vote to add + * \param[in] vote_origin origin of the added vote + * \param[in] in_class class for which this vote is cast + */ + void + addVote (pcl::InterestPoint& in_vote, const PointT &vote_origin, int in_class); + + /** \brief Returns the colored cloud that consists of votes for center (blue points) and + * initial point cloud (if it was passed). + * \param[in] cloud cloud that needs to be merged with votes for visualizing. */ + typename pcl::PointCloud::Ptr + getColoredCloud (typename pcl::PointCloud::Ptr cloud = 0); + + /** \brief This method finds the strongest peaks (points were density has most higher values). + * It is based on the non maxima supression principles. + * \param[out] out_peaks it will contain the strongest peaks + * \param[in] in_class_id class of interest for which peaks are evaluated + * \param[in] in_non_maxima_radius non maxima supression radius. The shapes radius is recommended for this value. + * \param in_sigma + */ + void + findStrongestPeaks (std::vector > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma); + + /** \brief Returns the density at the specified point. + * \param[in] point point of interest + * \param[in] sigma_dist + */ + double + getDensityAtPoint (const PointT &point, double sigma_dist); + + /** \brief This method simply returns the number of votes. */ + unsigned int + getNumberOfVotes (); + + protected: + + /** \brief this method is simply setting up the search tree. */ + void + validateTree (); + + Eigen::Vector3f + shiftMean (const Eigen::Vector3f& snapPt, const double in_dSigmaDist); + + protected: + + /** \brief Stores all votes. */ + pcl::PointCloud::Ptr votes_; + + /** \brief Signalizes if the tree is valid. */ + bool tree_is_valid_; + + /** \brief Stores the origins of the votes. */ + typename pcl::PointCloud::Ptr votes_origins_; + + /** \brief Stores classes for which every single vote was cast. */ + std::vector votes_class_; + + /** \brief Stores the search tree. */ + pcl::KdTreeFLANN::Ptr tree_; + + /** \brief Stores neighbours indices. */ + std::vector k_ind_; + + /** \brief Stores square distances to the corresponding neighbours. */ + std::vector k_sqr_dist_; + }; + + /** \brief The assignment of this structure is to store the statistical/learned weights and other information + * of the trained Implict Shape Model algorithm. + */ + struct PCL_EXPORTS ISMModel + { + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Simple constructor that initializes the structure. */ + ISMModel (); + + /** \brief Copy constructor for deep copy. */ + ISMModel (ISMModel const & copy); + + /** Destructor that frees memory. */ + virtual + ~ISMModel (); + + /** \brief This method simply saves the trained model for later usage. + * \param[in] file_name path to file for saving model + */ + bool + saveModelToFile (std::string& file_name); + + /** \brief This method loads the trained model from file. + * \param[in] file_name path to file which stores trained model + */ + bool + loadModelFromfile (std::string& file_name); + + /** \brief this method resets all variables and frees memory. */ + void + reset (); + + /** Operator overloading for deep copy. */ + ISMModel & operator = (const ISMModel& other); + + /** \brief Stores statistical weights. */ + std::vector > statistical_weights_; + + /** \brief Stores learned weights. */ + std::vector learned_weights_; + + /** \brief Stores the class label for every direction. */ + std::vector classes_; + + /** \brief Stores the sigma value for each class. This values were used to compute the learned weights. */ + std::vector sigmas_; + + /** \brief Stores the directions to objects center for each visual word. */ + Eigen::MatrixXf directions_to_center_; + + /** \brief Stores the centers of the clusters that were obtained during the visual words clusterization. */ + Eigen::MatrixXf clusters_centers_; + + /** \brief This is an array of clusters. Each cluster stores the indices of the visual words that it contains. */ + std::vector > clusters_; + + /** \brief Stores the number of classes. */ + unsigned int number_of_classes_; + + /** \brief Stores the number of visual words. */ + unsigned int number_of_visual_words_; + + /** \brief Stores the number of clusters. */ + unsigned int number_of_clusters_; + + /** \brief Stores descriptors dimension. */ + unsigned int descriptors_dimension_; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } + + namespace ism + { + /** \brief This class implements Implicit Shape Model algorithm described in + * "Hough Transforms and 3D SURF for robust three dimensional classication" + * by Jan Knopp1, Mukta Prasad, Geert Willems1, Radu Timofte, and Luc Van Gool. + * It has two main member functions. One for training, using the data for which we know + * which class it belongs to. And second for investigating a cloud for the presence + * of the class of interest. + * Implementation of the ISM algorithm described in "Hough Transforms and 3D SURF for robust three dimensional classication" + * by Jan Knopp, Mukta Prasad, Geert Willems, Radu Timofte, and Luc Van Gool + * + * Authors: Roman Shapovalov, Alexander Velizhev, Sergey Ushakov + */ + template + class PCL_EXPORTS ImplicitShapeModelEstimation + { + public: + + using ISMModelPtr = pcl::features::ISMModel::Ptr; + using Feature = pcl::Feature>; + using FeaturePtr = typename Feature::Ptr; + + protected: + + /** \brief This structure stores the information about the keypoint. */ + struct PCL_EXPORTS LocationInfo + { + /** \brief Location info constructor. + * \param[in] model_num number of training model. + * \param[in] dir_to_center expected direction to center + * \param[in] origin initial point + * \param[in] normal normal of the initial point + */ + LocationInfo (unsigned int model_num, const PointT& dir_to_center, const PointT& origin, const NormalT& normal) : + model_num_ (model_num), + dir_to_center_ (dir_to_center), + point_ (origin), + normal_ (normal) {}; + + /** \brief Tells from which training model this keypoint was extracted. */ + unsigned int model_num_; + + /** \brief Expected direction to center for this keypoint. */ + PointT dir_to_center_; + + /** \brief Stores the initial point. */ + PointT point_; + + /** \brief Stores the normal of the initial point. */ + NormalT normal_; + }; + + /** \brief This structure is used for determining the end of the + * k-means clustering process. */ + struct PCL_EXPORTS TermCriteria + { + enum + { + COUNT = 1, + EPS = 2 + }; + + /** \brief Termination criteria constructor. + * \param[in] type defines the condition of termination(max iter., desired accuracy) + * \param[in] max_count defines the max number of iterations + * \param[in] epsilon defines the desired accuracy + */ + TermCriteria(int type, int max_count, float epsilon) : + type_ (type), + max_count_ (max_count), + epsilon_ (epsilon) {}; + + /** \brief Flag that determines when the k-means clustering must be stopped. + * If type_ equals COUNT then it must be stopped when the max number of iterations will be + * reached. If type_ eaquals EPS then it must be stopped when the desired accuracy will be reached. + * These flags can be used together, in that case the clustering will be finished when one of these + * conditions will be reached. + */ + int type_; + + /** \brief Defines maximum number of iterations for k-means clustering. */ + int max_count_; + + /** \brief Defines the accuracy for k-means clustering. */ + float epsilon_; + }; + + /** \brief Structure for storing the visual word. */ + struct PCL_EXPORTS VisualWordStat + { + /** \brief Empty constructor with member variables initialization. */ + VisualWordStat () : + class_ (-1), + learned_weight_ (0.0f), + dir_to_center_ (0.0f, 0.0f, 0.0f) {}; + + /** \brief Which class this vote belongs. */ + int class_; + + /** \brief Weight of the vote. */ + float learned_weight_; + + /** \brief Expected direction to center. */ + pcl::PointXYZ dir_to_center_; + }; + + public: + + /** \brief Simple constructor that initializes everything. */ + ImplicitShapeModelEstimation (); + + /** \brief Simple destructor. */ + virtual + ~ImplicitShapeModelEstimation (); + + /** \brief This method simply returns the clouds that were set as the training clouds. */ + std::vector::Ptr> + getTrainingClouds (); + + /** \brief Allows to set clouds for training the ISM model. + * \param[in] training_clouds array of point clouds for training + */ + void + setTrainingClouds (const std::vector< typename pcl::PointCloud::Ptr >& training_clouds); + + /** \brief Returns the array of classes that indicates which class the corresponding training cloud belongs. */ + std::vector + getTrainingClasses (); + + /** \brief Allows to set the class labels for the corresponding training clouds. + * \param[in] training_classes array of class labels + */ + void + setTrainingClasses (const std::vector& training_classes); + + /** \brief This method returns the corresponding cloud of normals for every training point cloud. */ + std::vector::Ptr> + getTrainingNormals (); + + /** \brief Allows to set normals for the training clouds that were passed through setTrainingClouds method. + * \param[in] training_normals array of clouds, each cloud is the cloud of normals + */ + void + setTrainingNormals (const std::vector< typename pcl::PointCloud::Ptr >& training_normals); + + /** \brief Returns the sampling size used for cloud simplification. */ + float + getSamplingSize (); + + /** \brief Changes the sampling size used for cloud simplification. + * \param[in] sampling_size desired size of grid bin + */ + void + setSamplingSize (float sampling_size); + + /** \brief Returns the current feature estimator used for extraction of the descriptors. */ + FeaturePtr + getFeatureEstimator (); + + /** \brief Changes the feature estimator. + * \param[in] feature feature estimator that will be used to extract the descriptors. + * Note that it must be fully initialized and configured. + */ + void + setFeatureEstimator (FeaturePtr feature); + + /** \brief Returns the number of clusters used for descriptor clustering. */ + unsigned int + getNumberOfClusters (); + + /** \brief Changes the number of clusters. + * \param num_of_clusters desired number of clusters + */ + void + setNumberOfClusters (unsigned int num_of_clusters); + + /** \brief Returns the array of sigma values. */ + std::vector + getSigmaDists (); + + /** \brief This method allows to set the value of sigma used for calculating the learned weights for every single class. + * \param[in] training_sigmas new sigmas for every class. If you want these values to be computed automatically, + * just pass the empty array. The automatic regime calculates the maximum distance between the objects points and takes 10% of + * this value as recommended in the article. If there are several objects of the same class, + * then it computes the average maximum distance and takes 10%. Note that each class has its own sigma value. + */ + void + setSigmaDists (const std::vector& training_sigmas); + + /** \brief Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], + * if set to false then coeff is taken as 1.0. It is just a kind of heuristic. + * The default behavior is as in the article. So you can ignore this if you want. + */ + bool + getNVotState (); + + /** \brief Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)]. + * \param[in] state desired state, if false then Nvot is taken as 1.0 + */ + void + setNVotState (bool state); + + /** \brief This method performs training and forms a visual vocabulary. It returns a trained model that + * can be saved to file for later usage. + * \param[out] trained_model trained model + */ + bool + trainISM (ISMModelPtr& trained_model); + + /** \brief This function is searching for the class of interest in a given cloud + * and returns the list of votes. + * \param[in] model trained model which will be used for searching the objects + * \param[in] in_cloud input cloud that need to be investigated + * \param[in] in_normals cloud of normals corresponding to the input cloud + * \param[in] in_class_of_interest class which we are looking for + */ + typename pcl::features::ISMVoteList::Ptr + findObjects (ISMModelPtr model, typename pcl::PointCloud::Ptr in_cloud, typename pcl::PointCloud::Ptr in_normals, int in_class_of_interest); + + protected: + + /** \brief Extracts the descriptors from the input clouds. + * \param[out] histograms it will store the descriptors for each key point + * \param[out] locations it will contain the comprehensive information (such as direction, initial keypoint) + * for the corresponding descriptors + */ + bool + extractDescriptors (std::vector >& histograms, + std::vector >& locations); + + /** \brief This method performs descriptor clustering. + * \param[in] histograms descriptors to cluster + * \param[out] labels it contains labels for each descriptor + * \param[out] clusters_centers stores the centers of clusters + */ + bool + clusterDescriptors (std::vector< pcl::Histogram >& histograms, Eigen::MatrixXi& labels, Eigen::MatrixXf& clusters_centers); + + /** \brief This method calculates the value of sigma used for calculating the learned weights for every single class. + * \param[out] sigmas computed sigmas. + */ + void + calculateSigmas (std::vector& sigmas); + + /** \brief This function forms a visual vocabulary and evaluates weights + * described in [Knopp et al., 2010, (5)]. + * \param[in] locations array containing description of each keypoint: its position, which cloud belongs + * and expected direction to center + * \param[in] labels labels that were obtained during k-means clustering + * \param[in] sigmas array of sigmas for each class + * \param[in] clusters clusters that were obtained during k-means clustering + * \param[out] statistical_weights stores the computed statistical weights + * \param[out] learned_weights stores the computed learned weights + */ + void + calculateWeights (const std::vector< LocationInfo, Eigen::aligned_allocator >& locations, + const Eigen::MatrixXi &labels, + std::vector& sigmas, + std::vector >& clusters, + std::vector >& statistical_weights, + std::vector& learned_weights); + + /** \brief Simplifies the cloud using voxel grid principles. + * \param[in] in_point_cloud cloud that need to be simplified + * \param[in] in_normal_cloud normals of the cloud that need to be simplified + * \param[out] out_sampled_point_cloud simplified cloud + * \param[out] out_sampled_normal_cloud and the corresponding normals + */ + void + simplifyCloud (typename pcl::PointCloud::ConstPtr in_point_cloud, + typename pcl::PointCloud::ConstPtr in_normal_cloud, + typename pcl::PointCloud::Ptr out_sampled_point_cloud, + typename pcl::PointCloud::Ptr out_sampled_normal_cloud); + + /** \brief This method simply shifts the clouds points relative to the passed point. + * \param[in] in_cloud cloud to shift + * \param[in] shift_point point relative to which the cloud will be shifted + */ + void + shiftCloud (typename pcl::PointCloud::Ptr in_cloud, Eigen::Vector3f shift_point); + + /** \brief This method simply computes the rotation matrix, so that the given normal + * would match the Y axis after the transformation. This is done because the algorithm needs to be invariant + * to the affine transformations. + * \param[in] in_normal normal for which the rotation matrix need to be computed + */ + Eigen::Matrix3f + alignYCoordWithNormal (const NormalT& in_normal); + + /** \brief This method applies transform set in in_transform to vector io_vector. + * \param[in] io_vec vector that need to be transformed + * \param[in] in_transform matrix that contains the transformation + */ + void + applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform); + + /** \brief This method estimates features for the given point cloud. + * \param[in] sampled_point_cloud sampled point cloud for which the features must be computed + * \param[in] normal_cloud normals for the original point cloud + * \param[out] feature_cloud it will store the computed histograms (features) for the given cloud + */ + void + estimateFeatures (typename pcl::PointCloud::Ptr sampled_point_cloud, + typename pcl::PointCloud::Ptr normal_cloud, + typename pcl::PointCloud >::Ptr feature_cloud); + + /** \brief Performs K-means clustering. + * \param[in] points_to_cluster points to cluster + * \param[in] number_of_clusters desired number of clusters + * \param[out] io_labels output parameter, which stores the label for each point + * \param[in] criteria defines when the computational process need to be finished. For example if the + * desired accuracy is achieved or the iteration number exceeds given value + * \param[in] attempts number of attempts to compute clustering + * \param[in] flags if set to USE_INITIAL_LABELS then initial approximation of labels is taken from io_labels + * \param[out] cluster_centers it will store the cluster centers + */ + double + computeKMeansClustering (const Eigen::MatrixXf& points_to_cluster, + int number_of_clusters, + Eigen::MatrixXi& io_labels, + TermCriteria criteria, + int attempts, + int flags, + Eigen::MatrixXf& cluster_centers); + + /** \brief Generates centers for clusters as described in + * Arthur, David and Sergei Vassilvitski (2007) k-means++: The Advantages of Careful Seeding. + * \param[in] data points to cluster + * \param[out] out_centers it will contain generated centers + * \param[in] number_of_clusters defines the number of desired cluster centers + * \param[in] trials number of trials to generate a center + */ + void + generateCentersPP (const Eigen::MatrixXf& data, + Eigen::MatrixXf& out_centers, + int number_of_clusters, + int trials); + + /** \brief Generates random center for cluster. + * \param[in] boxes contains min and max values for each dimension + * \param[out] center it will the contain generated center + */ + void + generateRandomCenter (const std::vector >& boxes, Eigen::VectorXf& center); + + /** \brief Computes the square distance between two vectors. + * \param[in] vec_1 first vector + * \param[in] vec_2 second vector + */ + float + computeDistance (Eigen::VectorXf& vec_1, Eigen::VectorXf& vec_2); + + /** \brief Forbids the assignment operator. */ + ImplicitShapeModelEstimation& + operator= (const ImplicitShapeModelEstimation&); + + protected: + + /** \brief Stores the clouds used for training. */ + std::vector::Ptr> training_clouds_; + + /** \brief Stores the class number for each cloud from training_clouds_. */ + std::vector training_classes_; + + /** \brief Stores the normals for each training cloud. */ + std::vector::Ptr> training_normals_; + + /** \brief This array stores the sigma values for each training class. If this array has a size equals 0, then + * sigma values will be calculated automatically. + */ + std::vector training_sigmas_; + + /** \brief This value is used for the simplification. It sets the size of grid bin. */ + float sampling_size_; + + /** \brief Stores the feature estimator. */ + typename Feature::Ptr feature_estimator_; + + /** \brief Number of clusters, is used for clustering descriptors during the training. */ + unsigned int number_of_clusters_; + + /** \brief If set to false then Nvot coeff from [Knopp et al., 2010, (4)] is equal 1.0. */ + bool n_vot_ON_; + + /** \brief This const value is used for indicating that for k-means clustering centers must + * be generated as described in + * Arthur, David and Sergei Vassilvitski (2007) k-means++: The Advantages of Careful Seeding. */ + static const int PP_CENTERS = 2; + + /** \brief This const value is used for indicating that input labels must be taken as the + * initial approximation for k-means clustering. */ + static const int USE_INITIAL_LABELS = 1; + }; + } +} + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ISMPeak, + (float, x, x) + (float, y, y) + (float, z, z) + (float, density, ism_density) + (float, class_id, ism_class_id) +) diff --git a/deps_install/include/pcl-1.10/pcl/recognition/line_rgbd.h b/deps_install/include/pcl-1.10/pcl/recognition/line_rgbd.h new file mode 100755 index 0000000..340b8ce --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/line_rgbd.h @@ -0,0 +1,313 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + + struct BoundingBoxXYZ + { + /** \brief Constructor. */ + BoundingBoxXYZ () : x (0.0f), y (0.0f), z (0.0f), width (0.0f), height (0.0f), depth (0.0f) {} + + /** \brief X-coordinate of the upper left front point */ + float x; + /** \brief Y-coordinate of the upper left front point */ + float y; + /** \brief Z-coordinate of the upper left front point */ + float z; + + /** \brief Width of the bounding box */ + float width; + /** \brief Height of the bounding box */ + float height; + /** \brief Depth of the bounding box */ + float depth; + }; + + /** \brief High-level class for template matching using the LINEMOD approach based on RGB and Depth data. + * \author Stefan Holzer + */ + template + class PCL_EXPORTS LineRGBD + { + public: + + /** \brief A LineRGBD detection. */ + struct Detection + { + /** \brief Constructor. */ + Detection () : template_id (0), object_id (0), detection_id (0), response (0.0f) {} + + /** \brief The ID of the template. */ + std::size_t template_id; + /** \brief The ID of the object corresponding to the template. */ + std::size_t object_id; + /** \brief The ID of this detection. This is only valid for the last call of the method detect (...). */ + std::size_t detection_id; + /** \brief The response of this detection. Responses are between 0 and 1, where 1 is best. */ + float response; + /** \brief The 3D bounding box of the detection. */ + BoundingBoxXYZ bounding_box; + /** \brief The 2D template region of the detection. */ + RegionXY region; + }; + + /** \brief Constructor */ + LineRGBD () + : intersection_volume_threshold_ (1.0f) + , color_gradient_mod_ () + , surface_normal_mod_ () + , cloud_xyz_ () + , cloud_rgb_ () + , detections_ () + { + } + + /** \brief Destructor */ + virtual ~LineRGBD () + { + } + + /** \brief Loads templates from a LMT (LineMod Template) file. Overrides old templates. + * + * LineMod Template files are TAR files that store pairs of PCD datasets + * together with their LINEMOD signatures in \ref + * SparseQuantizedMultiModTemplate format. + * + * \param[in] file_name The name of the file that stores the templates. + * \param object_id + * + * \return true, if the operation was successful, false otherwise. + */ + bool + loadTemplates (const std::string &file_name, std::size_t object_id = 0); + + bool + addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud & cloud, std::size_t object_id = 0); + + /** \brief Sets the threshold for the detection responses. Responses are between 0 and 1, where 1 is a best. + * \param[in] threshold The threshold used to decide where a template is detected. + */ + inline void + setDetectionThreshold (float threshold) + { + linemod_.setDetectionThreshold (threshold); + } + + /** \brief Sets the threshold on the magnitude of color gradients. Color gradients with a magnitude below + * this threshold are not considered in the detection process. + * \param[in] threshold The threshold on the magnitude of color gradients. + */ + inline void + setGradientMagnitudeThreshold (const float threshold) + { + color_gradient_mod_.setGradientMagnitudeThreshold (threshold); + } + + /** \brief Sets the threshold for the decision whether two detections of the same template are merged or not. + * If ratio between the intersection of the bounding boxes of two detections and the original bounding + * boxes is larger than the specified threshold then they are merged. If detection A overlaps with + * detection B and B with C than A, B, and C are merged. Threshold has to be between 0 and 1. + * \param[in] threshold The threshold on the ratio between the intersection bounding box and the original + * bounding box. + */ + inline void + setIntersectionVolumeThreshold (const float threshold = 1.0f) + { + intersection_volume_threshold_ = threshold; + } + + /** \brief Sets the input cloud with xyz point coordinates. The cloud has to be organized. + * \param[in] cloud The input cloud with xyz point coordinates. + */ + inline void + setInputCloud (const typename pcl::PointCloud::ConstPtr & cloud) + { + cloud_xyz_ = cloud; + + surface_normal_mod_.setInputCloud (cloud); + surface_normal_mod_.processInputData (); + } + + /** \brief Sets the input cloud with rgb values. The cloud has to be organized. + * \param[in] cloud The input cloud with rgb values. + */ + inline void + setInputColors (const typename pcl::PointCloud::ConstPtr & cloud) + { + cloud_rgb_ = cloud; + + color_gradient_mod_.setInputCloud (cloud); + color_gradient_mod_.processInputData (); + } + + /** \brief Creates a template from the specified data and adds it to the matching queue. + * \param cloud + * \param object_id + * \param[in] mask_xyz the mask that determine which parts of the xyz-modality are used for creating the template. + * \param[in] mask_rgb the mask that determine which parts of the rgb-modality are used for creating the template. + * \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps). + */ + int + createAndAddTemplate ( + pcl::PointCloud & cloud, + const std::size_t object_id, + const MaskMap & mask_xyz, + const MaskMap & mask_rgb, + const RegionXY & region); + + + /** \brief Applies the detection process and fills the supplied vector with the detection instances. + * \param[out] detections The storage for the detection instances. + */ + void + detect (std::vector::Detection> & detections); + + /** \brief Applies the detection process in a semi-scale-invariant manner. This is done by actually + * scaling the template to different sizes. + */ + void + detectSemiScaleInvariant (std::vector::Detection> & detections, + float min_scale = 0.6944444f, + float max_scale = 1.44f, + float scale_multiplier = 1.2f); + + /** \brief Computes and returns the point cloud of the specified detection. This is the template point + * cloud transformed to the detection coordinates. The detection ID refers to the last call of + * the method detect (...). + * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). + * \param[out] cloud The storage for the transformed points. + */ + void + computeTransformedTemplatePoints (const std::size_t detection_id, + pcl::PointCloud & cloud); + + /** \brief Finds the indices of the points in the input cloud which correspond to the specified detection. + * The detection ID refers to the last call of the method detect (...). + * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). + */ + inline std::vector + findObjectPointIndices (const std::size_t detection_id) + { + if (detection_id >= detections_.size ()) + PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); + + // TODO: compute transform from detection + // TODO: transform template points + std::vector vec; + return (vec); + } + + + protected: + + /** \brief Aligns the template points with the points found at the detection position of the specified detection. + * The detection ID refers to the last call of the method detect (...). + * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). + */ + inline std::vector + alignTemplatePoints (const std::size_t detection_id) + { + if (detection_id >= detections_.size ()) + PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); + + // TODO: compute transform from detection + // TODO: transform template points + std::vector vec; + return (vec); + } + + /** \brief Refines the found detections along the depth. */ + void + refineDetectionsAlongDepth (); + + /** \brief Applies projective ICP on detections to find their correct position in depth. */ + void + applyProjectiveDepthICPOnDetections (); + + /** \brief Checks for overlapping detections, removes them and keeps only the strongest one. */ + void + removeOverlappingDetections (); + + /** \brief Computes the volume of the intersection between two bounding boxes. + * \param[in] box1 First bounding box. + * \param[in] box2 Second bounding box. + */ + static float + computeBoundingBoxIntersectionVolume (const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2); + + private: + /** \brief Read another LTM header chunk. */ + bool + readLTMHeader (int fd, pcl::io::TARHeader &header); + + /** \brief Intersection volume threshold. */ + float intersection_volume_threshold_; + + /** \brief LINEMOD instance. */ + public: pcl::LINEMOD linemod_; + /** \brief Color gradient modality. */ + pcl::ColorGradientModality color_gradient_mod_; + /** \brief Surface normal modality. */ + pcl::SurfaceNormalModality surface_normal_mod_; + + /** \brief XYZ point cloud. */ + typename pcl::PointCloud::ConstPtr cloud_xyz_; + /** \brief RGB point cloud. */ + typename pcl::PointCloud::ConstPtr cloud_rgb_; + + /** \brief Point clouds corresponding to the templates. */ + pcl::PointCloud::CloudVectorType template_point_clouds_; + /** \brief Bounding boxes corresponding to the templates. */ + std::vector bounding_boxes_; + /** \brief Object IDs corresponding to the templates. */ + std::vector object_ids_; + + /** \brief Detections from last call of method detect (...). */ + std::vector::Detection> detections_; + }; + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/recognition/linemod.h b/deps_install/include/pcl-1.10/pcl/recognition/linemod.h new file mode 100755 index 0000000..0617614 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/linemod.h @@ -0,0 +1,476 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + + /** \brief Stores a set of energy maps. + * \author Stefan Holzer + */ + class PCL_EXPORTS EnergyMaps + { + public: + /** \brief Constructor. */ + EnergyMaps () : width_ (0), height_ (0), nr_bins_ (0) + { + } + + /** \brief Destructor. */ + virtual ~EnergyMaps () + { + } + + /** \brief Returns the width of the energy maps. */ + inline std::size_t + getWidth () const + { + return (width_); + } + + /** \brief Returns the height of the energy maps. */ + inline std::size_t + getHeight () const + { + return (height_); + } + + /** \brief Returns the number of bins used for quantization (which is equal to the number of energy maps). */ + inline std::size_t + getNumOfBins () const + { + return (nr_bins_); + } + + /** \brief Initializes the set of energy maps. + * \param[in] width the width of the energy maps. + * \param[in] height the height of the energy maps. + * \param[in] nr_bins the number of bins used for quantization. + */ + void + initialize (const std::size_t width, const std::size_t height, const std::size_t nr_bins) + { + maps_.resize(nr_bins, nullptr); + width_ = width; + height_ = height; + nr_bins_ = nr_bins; + + const std::size_t mapsSize = width*height; + + for (auto &map : maps_) + { + //maps_[map_index] = new unsigned char[mapsSize]; + map = reinterpret_cast (aligned_malloc (mapsSize)); + memset (map, 0, mapsSize); + } + } + + /** \brief Releases the internal data. */ + void + releaseAll () + { + for (auto &map : maps_) + //if (maps_[map_index] != NULL) delete[] maps_[map_index]; + if (map != nullptr) aligned_free (map); + + maps_.clear (); + width_ = 0; + height_ = 0; + nr_bins_ = 0; + } + + /** \brief Operator for accessing a specific element in the set of energy maps. + * \param[in] bin_index the quantization bin (states which of the energy maps to access). + * \param[in] col_index the column index within the specified energy map. + * \param[in] row_index the row index within the specified energy map. + */ + inline unsigned char & + operator() (const std::size_t bin_index, const std::size_t col_index, const std::size_t row_index) + { + return (maps_[bin_index][row_index*width_ + col_index]); + } + + /** \brief Operator for accessing a specific element in the set of energy maps. + * \param[in] bin_index the quantization bin (states which of the energy maps to access). + * \param[in] index the element index within the specified energy map. + */ + inline unsigned char & + operator() (const std::size_t bin_index, const std::size_t index) + { + return (maps_[bin_index][index]); + } + + /** \brief Returns a pointer to the data of the specified energy map. + * \param[in] bin_index the index of the energy map to return (== the quantization bin). + */ + inline unsigned char * + operator() (const std::size_t bin_index) + { + return (maps_[bin_index]); + } + + /** \brief Operator for accessing a specific element in the set of energy maps. + * \param[in] bin_index the quantization bin (states which of the energy maps to access). + * \param[in] col_index the column index within the specified energy map. + * \param[in] row_index the row index within the specified energy map. + */ + inline const unsigned char & + operator() (const std::size_t bin_index, const std::size_t col_index, const std::size_t row_index) const + { + return (maps_[bin_index][row_index*width_ + col_index]); + } + + /** \brief Operator for accessing a specific element in the set of energy maps. + * \param[in] bin_index the quantization bin (states which of the energy maps to access). + * \param[in] index the element index within the specified energy map. + */ + inline const unsigned char & + operator() (const std::size_t bin_index, const std::size_t index) const + { + return (maps_[bin_index][index]); + } + + /** \brief Returns a pointer to the data of the specified energy map. + * \param[in] bin_index the index of the energy map to return (== the quantization bin). + */ + inline const unsigned char * + operator() (const std::size_t bin_index) const + { + return (maps_[bin_index]); + } + + private: + /** \brief The width of the energy maps. */ + std::size_t width_; + /** \brief The height of the energy maps. */ + std::size_t height_; + /** \brief The number of quantization bins (== the number of internally stored energy maps). */ + std::size_t nr_bins_; + /** \brief Storage for the energy maps. */ + std::vector maps_; + }; + + /** \brief Stores a set of linearized maps. + * \author Stefan Holzer + */ + class PCL_EXPORTS LinearizedMaps + { + public: + /** \brief Constructor. */ + LinearizedMaps () : width_ (0), height_ (0), mem_width_ (0), mem_height_ (0), step_size_ (0) + { + } + + /** \brief Destructor. */ + virtual ~LinearizedMaps () + { + } + + /** \brief Returns the width of the linearized map. */ + inline std::size_t + getWidth () const { return (width_); } + + /** \brief Returns the height of the linearized map. */ + inline std::size_t + getHeight () const { return (height_); } + + /** \brief Returns the step-size used to construct the linearized map. */ + inline std::size_t + getStepSize () const { return (step_size_); } + + /** \brief Returns the size of the memory map. */ + inline std::size_t + getMapMemorySize () const { return (mem_width_ * mem_height_); } + + /** \brief Initializes the linearized map. + * \param[in] width the width of the source map. + * \param[in] height the height of the source map. + * \param[in] step_size the step-size used to sample the source map. + */ + void + initialize (const std::size_t width, const std::size_t height, const std::size_t step_size) + { + maps_.resize(step_size*step_size, nullptr); + width_ = width; + height_ = height; + mem_width_ = width / step_size; + mem_height_ = height / step_size; + step_size_ = step_size; + + const std::size_t mapsSize = mem_width_ * mem_height_; + + for (auto &map : maps_) + { + //maps_[map_index] = new unsigned char[2*mapsSize]; + map = reinterpret_cast (aligned_malloc (2*mapsSize)); + memset (map, 0, 2*mapsSize); + } + } + + /** \brief Releases the internal memory. */ + void + releaseAll () + { + for (auto &map : maps_) + //if (maps_[map_index] != NULL) delete[] maps_[map_index]; + if (map != nullptr) aligned_free (map); + + maps_.clear (); + width_ = 0; + height_ = 0; + mem_width_ = 0; + mem_height_ = 0; + step_size_ = 0; + } + + /** \brief Operator to access elements of the linearized map by column and row index. + * \param[in] col_index the column index. + * \param[in] row_index the row index. + */ + inline unsigned char * + operator() (const std::size_t col_index, const std::size_t row_index) + { + return (maps_[row_index*step_size_ + col_index]); + } + + /** \brief Returns a linearized map starting at the specified position. + * \param[in] col_index the column index at which the returned map starts. + * \param[in] row_index the row index at which the returned map starts. + */ + inline unsigned char * + getOffsetMap (const std::size_t col_index, const std::size_t row_index) + { + const std::size_t map_col = col_index % step_size_; + const std::size_t map_row = row_index % step_size_; + + const std::size_t map_mem_col_index = col_index / step_size_; + const std::size_t map_mem_row_index = row_index / step_size_; + + return (maps_[map_row*step_size_ + map_col] + map_mem_row_index*mem_width_ + map_mem_col_index); + } + + private: + /** \brief the original width of the data represented by the map. */ + std::size_t width_; + /** \brief the original height of the data represented by the map. */ + std::size_t height_; + /** \brief the actual width of the linearized map. */ + std::size_t mem_width_; + /** \brief the actual height of the linearized map. */ + std::size_t mem_height_; + /** \brief the step-size used for sampling the original data. */ + std::size_t step_size_; + /** \brief a vector containing all the linearized maps. */ + std::vector maps_; + }; + + /** \brief Represents a detection of a template using the LINEMOD approach. + * \author Stefan Holzer + */ + struct PCL_EXPORTS LINEMODDetection + { + /** \brief Constructor. */ + LINEMODDetection () : x (0), y (0), template_id (0), score (0.0f), scale (1.0f) {} + + /** \brief x-position of the detection. */ + int x; + /** \brief y-position of the detection. */ + int y; + /** \brief ID of the detected template. */ + int template_id; + /** \brief score of the detection. */ + float score; + /** \brief scale at which the template was detected. */ + float scale; + }; + + /** + * \brief Template matching using the LINEMOD approach. + * \author Stefan Holzer, Stefan Hinterstoisser + */ + class PCL_EXPORTS LINEMOD + { + public: + /** \brief Constructor */ + LINEMOD (); + + /** \brief Destructor */ + virtual ~LINEMOD (); + + /** \brief Creates a template from the specified data and adds it to the matching queue. + * \param[in] modalities the modalities used to create the template. + * \param[in] masks the masks that determine which parts of the modalities are used for creating the template. + * \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps). + */ + int + createAndAddTemplate (const std::vector & modalities, + const std::vector & masks, + const RegionXY & region); + + /** \brief Adds the specified template to the matching queue. + * \param[in] linemod_template the template to add. + */ + int + addTemplate (const SparseQuantizedMultiModTemplate & linemod_template); + + /** \brief Detects the stored templates in the supplied modality data. + * \param[in] modalities the modalities that will be used for detection. + * \param[out] detections the destination for the detections. + */ + void + detectTemplates (const std::vector & modalities, + std::vector & detections) const; + + /** \brief Detects the stored templates in a semi scale invariant manner + * by applying the detection to multiple scaled versions of the input data. + * \param[in] modalities the modalities that will be used for detection. + * \param[out] detections the destination for the detections. + * \param[in] min_scale the minimum scale. + * \param[in] max_scale the maximum scale. + * \param[in] scale_multiplier the multiplier for getting from one scale to the next. + */ + void + detectTemplatesSemiScaleInvariant (const std::vector & modalities, + std::vector & detections, + float min_scale = 0.6944444f, + float max_scale = 1.44f, + float scale_multiplier = 1.2f) const; + + /** \brief Matches the stored templates to the supplied modality data. + * \param[in] modalities the modalities that will be used for matching. + * \param[out] matches the found matches. + */ + void + matchTemplates (const std::vector & modalities, + std::vector & matches) const; + + /** \brief Sets the detection threshold. + * \param[in] threshold the detection threshold. + */ + inline void + setDetectionThreshold (float threshold) + { + template_threshold_ = threshold; + } + + /** \brief Enables/disables non-maximum suppression. + * \param[in] use_non_max_suppression determines whether to use non-maximum suppression or not. + */ + inline void + setNonMaxSuppression (bool use_non_max_suppression) + { + use_non_max_suppression_ = use_non_max_suppression; + } + + /** \brief Enables/disables averaging of close detections. + * \param[in] average_detections determines whether to average close detections or not. + */ + inline void + setDetectionAveraging (bool average_detections) + { + average_detections_ = average_detections; + } + + /** \brief Returns the template with the specified ID. + * \param[in] template_id the ID of the template to return. + */ + inline const SparseQuantizedMultiModTemplate & + getTemplate (int template_id) const + { + return (templates_[template_id]); + } + + /** \brief Returns the number of stored/trained templates. */ + inline std::size_t + getNumOfTemplates () const + { + return (templates_.size ()); + } + + /** \brief Saves the stored templates to the specified file. + * \param[in] file_name the name of the file to save the templates to. + */ + void + saveTemplates (const char * file_name) const; + + /** \brief Loads templates from the specified file. + * \param[in] file_name the name of the file to load the template from. + */ + void + loadTemplates (const char * file_name); + + /** \brief Loads templates from the specified files. + * \param[in] file_names vector of files to load the templates from. + */ + + void + loadTemplates (std::vector & file_names); + + /** \brief Serializes the stored templates to the specified stream. + * \param[in] stream the stream the templates will be written to. + */ + void + serialize (std::ostream & stream) const; + + /** \brief Deserializes templates from the specified stream. + * \param[in] stream the stream the templates will be read from. + */ + void + deserialize (std::istream & stream); + + + private: + /** template response threshold */ + float template_threshold_; + /** states whether non-max-suppression on detections is enabled or not */ + bool use_non_max_suppression_; + /** states whether to return an averaged detection */ + bool average_detections_; + /** template storage */ + std::vector templates_; + }; + +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/linemod/line_rgbd.h b/deps_install/include/pcl-1.10/pcl/recognition/linemod/line_rgbd.h new file mode 100755 index 0000000..340b8ce --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/linemod/line_rgbd.h @@ -0,0 +1,313 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + + struct BoundingBoxXYZ + { + /** \brief Constructor. */ + BoundingBoxXYZ () : x (0.0f), y (0.0f), z (0.0f), width (0.0f), height (0.0f), depth (0.0f) {} + + /** \brief X-coordinate of the upper left front point */ + float x; + /** \brief Y-coordinate of the upper left front point */ + float y; + /** \brief Z-coordinate of the upper left front point */ + float z; + + /** \brief Width of the bounding box */ + float width; + /** \brief Height of the bounding box */ + float height; + /** \brief Depth of the bounding box */ + float depth; + }; + + /** \brief High-level class for template matching using the LINEMOD approach based on RGB and Depth data. + * \author Stefan Holzer + */ + template + class PCL_EXPORTS LineRGBD + { + public: + + /** \brief A LineRGBD detection. */ + struct Detection + { + /** \brief Constructor. */ + Detection () : template_id (0), object_id (0), detection_id (0), response (0.0f) {} + + /** \brief The ID of the template. */ + std::size_t template_id; + /** \brief The ID of the object corresponding to the template. */ + std::size_t object_id; + /** \brief The ID of this detection. This is only valid for the last call of the method detect (...). */ + std::size_t detection_id; + /** \brief The response of this detection. Responses are between 0 and 1, where 1 is best. */ + float response; + /** \brief The 3D bounding box of the detection. */ + BoundingBoxXYZ bounding_box; + /** \brief The 2D template region of the detection. */ + RegionXY region; + }; + + /** \brief Constructor */ + LineRGBD () + : intersection_volume_threshold_ (1.0f) + , color_gradient_mod_ () + , surface_normal_mod_ () + , cloud_xyz_ () + , cloud_rgb_ () + , detections_ () + { + } + + /** \brief Destructor */ + virtual ~LineRGBD () + { + } + + /** \brief Loads templates from a LMT (LineMod Template) file. Overrides old templates. + * + * LineMod Template files are TAR files that store pairs of PCD datasets + * together with their LINEMOD signatures in \ref + * SparseQuantizedMultiModTemplate format. + * + * \param[in] file_name The name of the file that stores the templates. + * \param object_id + * + * \return true, if the operation was successful, false otherwise. + */ + bool + loadTemplates (const std::string &file_name, std::size_t object_id = 0); + + bool + addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud & cloud, std::size_t object_id = 0); + + /** \brief Sets the threshold for the detection responses. Responses are between 0 and 1, where 1 is a best. + * \param[in] threshold The threshold used to decide where a template is detected. + */ + inline void + setDetectionThreshold (float threshold) + { + linemod_.setDetectionThreshold (threshold); + } + + /** \brief Sets the threshold on the magnitude of color gradients. Color gradients with a magnitude below + * this threshold are not considered in the detection process. + * \param[in] threshold The threshold on the magnitude of color gradients. + */ + inline void + setGradientMagnitudeThreshold (const float threshold) + { + color_gradient_mod_.setGradientMagnitudeThreshold (threshold); + } + + /** \brief Sets the threshold for the decision whether two detections of the same template are merged or not. + * If ratio between the intersection of the bounding boxes of two detections and the original bounding + * boxes is larger than the specified threshold then they are merged. If detection A overlaps with + * detection B and B with C than A, B, and C are merged. Threshold has to be between 0 and 1. + * \param[in] threshold The threshold on the ratio between the intersection bounding box and the original + * bounding box. + */ + inline void + setIntersectionVolumeThreshold (const float threshold = 1.0f) + { + intersection_volume_threshold_ = threshold; + } + + /** \brief Sets the input cloud with xyz point coordinates. The cloud has to be organized. + * \param[in] cloud The input cloud with xyz point coordinates. + */ + inline void + setInputCloud (const typename pcl::PointCloud::ConstPtr & cloud) + { + cloud_xyz_ = cloud; + + surface_normal_mod_.setInputCloud (cloud); + surface_normal_mod_.processInputData (); + } + + /** \brief Sets the input cloud with rgb values. The cloud has to be organized. + * \param[in] cloud The input cloud with rgb values. + */ + inline void + setInputColors (const typename pcl::PointCloud::ConstPtr & cloud) + { + cloud_rgb_ = cloud; + + color_gradient_mod_.setInputCloud (cloud); + color_gradient_mod_.processInputData (); + } + + /** \brief Creates a template from the specified data and adds it to the matching queue. + * \param cloud + * \param object_id + * \param[in] mask_xyz the mask that determine which parts of the xyz-modality are used for creating the template. + * \param[in] mask_rgb the mask that determine which parts of the rgb-modality are used for creating the template. + * \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps). + */ + int + createAndAddTemplate ( + pcl::PointCloud & cloud, + const std::size_t object_id, + const MaskMap & mask_xyz, + const MaskMap & mask_rgb, + const RegionXY & region); + + + /** \brief Applies the detection process and fills the supplied vector with the detection instances. + * \param[out] detections The storage for the detection instances. + */ + void + detect (std::vector::Detection> & detections); + + /** \brief Applies the detection process in a semi-scale-invariant manner. This is done by actually + * scaling the template to different sizes. + */ + void + detectSemiScaleInvariant (std::vector::Detection> & detections, + float min_scale = 0.6944444f, + float max_scale = 1.44f, + float scale_multiplier = 1.2f); + + /** \brief Computes and returns the point cloud of the specified detection. This is the template point + * cloud transformed to the detection coordinates. The detection ID refers to the last call of + * the method detect (...). + * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). + * \param[out] cloud The storage for the transformed points. + */ + void + computeTransformedTemplatePoints (const std::size_t detection_id, + pcl::PointCloud & cloud); + + /** \brief Finds the indices of the points in the input cloud which correspond to the specified detection. + * The detection ID refers to the last call of the method detect (...). + * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). + */ + inline std::vector + findObjectPointIndices (const std::size_t detection_id) + { + if (detection_id >= detections_.size ()) + PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); + + // TODO: compute transform from detection + // TODO: transform template points + std::vector vec; + return (vec); + } + + + protected: + + /** \brief Aligns the template points with the points found at the detection position of the specified detection. + * The detection ID refers to the last call of the method detect (...). + * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). + */ + inline std::vector + alignTemplatePoints (const std::size_t detection_id) + { + if (detection_id >= detections_.size ()) + PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); + + // TODO: compute transform from detection + // TODO: transform template points + std::vector vec; + return (vec); + } + + /** \brief Refines the found detections along the depth. */ + void + refineDetectionsAlongDepth (); + + /** \brief Applies projective ICP on detections to find their correct position in depth. */ + void + applyProjectiveDepthICPOnDetections (); + + /** \brief Checks for overlapping detections, removes them and keeps only the strongest one. */ + void + removeOverlappingDetections (); + + /** \brief Computes the volume of the intersection between two bounding boxes. + * \param[in] box1 First bounding box. + * \param[in] box2 Second bounding box. + */ + static float + computeBoundingBoxIntersectionVolume (const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2); + + private: + /** \brief Read another LTM header chunk. */ + bool + readLTMHeader (int fd, pcl::io::TARHeader &header); + + /** \brief Intersection volume threshold. */ + float intersection_volume_threshold_; + + /** \brief LINEMOD instance. */ + public: pcl::LINEMOD linemod_; + /** \brief Color gradient modality. */ + pcl::ColorGradientModality color_gradient_mod_; + /** \brief Surface normal modality. */ + pcl::SurfaceNormalModality surface_normal_mod_; + + /** \brief XYZ point cloud. */ + typename pcl::PointCloud::ConstPtr cloud_xyz_; + /** \brief RGB point cloud. */ + typename pcl::PointCloud::ConstPtr cloud_rgb_; + + /** \brief Point clouds corresponding to the templates. */ + pcl::PointCloud::CloudVectorType template_point_clouds_; + /** \brief Bounding boxes corresponding to the templates. */ + std::vector bounding_boxes_; + /** \brief Object IDs corresponding to the templates. */ + std::vector object_ids_; + + /** \brief Detections from last call of method detect (...). */ + std::vector::Detection> detections_; + }; + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/recognition/mask_map.h b/deps_install/include/pcl-1.10/pcl/recognition/mask_map.h new file mode 100755 index 0000000..5407461 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/mask_map.h @@ -0,0 +1,134 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { +class PCL_EXPORTS MaskMap { +public: + MaskMap() = default; + + MaskMap(std::size_t width, std::size_t height); + + virtual ~MaskMap() = default; + + void + resize(std::size_t width, std::size_t height); + + inline std::size_t + getWidth() const + { + return (width_); + } + + inline std::size_t + getHeight() const + { + return (height_); + } + + inline unsigned char* + getData() + { + return (data_.data()); + } + + inline const unsigned char* + getData() const + { + return (data_.data()); + } + + PCL_DEPRECATED("Use new version diff getDifferenceMask(mask0, mask1)") + static void + getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1, MaskMap& diff_mask); + + PCL_NODISCARD + static MaskMap + getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1); + + inline void + set(const std::size_t x, const std::size_t y) + { + data_[y * width_ + x] = 255; + } + + inline void + unset(const std::size_t x, const std::size_t y) + { + data_[y * width_ + x] = 0; + } + + inline bool + isSet(const std::size_t x, const std::size_t y) const + { + return (data_[y * width_ + x] != 0); + } + + inline void + reset() + { + data_.assign(data_.size(), 0); + } + + inline unsigned char& + operator()(const std::size_t x, const std::size_t y) + { + return (data_[y * width_ + x]); + } + + inline const unsigned char& + operator()(const std::size_t x, const std::size_t y) const + { + return (data_[y * width_ + x]); + } + + void + erode(MaskMap& eroded_mask) const; + +private: + std::vector data_; + std::size_t width_ = 0; + std::size_t height_ = 0; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/model_library.h b/deps_install/include/pcl-1.10/pcl/recognition/model_library.h new file mode 100755 index 0000000..3cb7456 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/model_library.h @@ -0,0 +1,271 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include "auxiliary.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + class PCL_EXPORTS ModelLibrary + { + public: + using PointCloudIn = pcl::PointCloud; + using PointCloudN = pcl::PointCloud; + + /** \brief Stores some information about the model. */ + class Model + { + public: + Model (const PointCloudIn& points, const PointCloudN& normals, float voxel_size, const std::string& object_name, + float frac_of_points_for_registration, void* user_data = nullptr) + : obj_name_(object_name), + user_data_ (user_data) + { + octree_.build (points, voxel_size, &normals); + + const std::vector& full_leaves = octree_.getFullLeaves (); + if ( full_leaves.empty () ) + return; + + // Initialize + std::vector::const_iterator it = full_leaves.begin (); + const float *p = (*it)->getData ()->getPoint (); + aux::copy3 (p, octree_center_of_mass_); + bounds_of_octree_points_[0] = bounds_of_octree_points_[1] = p[0]; + bounds_of_octree_points_[2] = bounds_of_octree_points_[3] = p[1]; + bounds_of_octree_points_[4] = bounds_of_octree_points_[5] = p[2]; + + // Compute both the bounds and the center of mass of the octree points + for ( ++it ; it != full_leaves.end () ; ++it ) + { + aux::add3 (octree_center_of_mass_, (*it)->getData ()->getPoint ()); + aux::expandBoundingBoxToContainPoint (bounds_of_octree_points_, (*it)->getData ()->getPoint ()); + } + + int num_octree_points = static_cast (full_leaves.size ()); + // Finalize the center of mass computation + aux::mult3 (octree_center_of_mass_, 1.0f/static_cast (num_octree_points)); + + int num_points_for_registration = static_cast (static_cast (num_octree_points)*frac_of_points_for_registration); + points_for_registration_.resize (static_cast (num_points_for_registration)); + + // Prepare for random point sampling + std::vector ids (num_octree_points); + for ( int i = 0 ; i < num_octree_points ; ++i ) + ids[i] = i; + + // The random generator + pcl::common::UniformGenerator randgen (0, num_octree_points - 1, static_cast (time (nullptr))); + + // Randomly sample some points from the octree + for ( int i = 0 ; i < num_points_for_registration ; ++i ) + { + // Choose a random position within the array of ids + randgen.setParameters (0, static_cast (ids.size ()) - 1); + int rand_pos = randgen.run (); + + // Copy the randomly selected octree point + aux::copy3 (octree_.getFullLeaves ()[ids[rand_pos]]->getData ()->getPoint (), points_for_registration_[i]); + + // Delete the selected id + ids.erase (ids.begin() + rand_pos); + } + } + + virtual ~Model () + { + } + + inline const std::string& + getObjectName () const + { + return (obj_name_); + } + + inline const ORROctree& + getOctree () const + { + return (octree_); + } + + inline void* + getUserData () const + { + return (user_data_); + } + + inline const float* + getOctreeCenterOfMass () const + { + return (octree_center_of_mass_); + } + + inline const float* + getBoundsOfOctreePoints () const + { + return (bounds_of_octree_points_); + } + + inline const PointCloudIn& + getPointsForRegistration () const + { + return (points_for_registration_); + } + + protected: + const std::string obj_name_; + ORROctree octree_; + float octree_center_of_mass_[3]; + float bounds_of_octree_points_[6]; + PointCloudIn points_for_registration_; + void* user_data_; + }; + + using node_data_pair_list = std::list >; + using HashTableCell = std::map; + using HashTable = VoxelStructure; + + public: + /** \brief This class is used by 'ObjRecRANSAC' to maintain the object models to be recognized. Normally, you do not need to use + * this class directly. */ + ModelLibrary (float pair_width, float voxel_size, float max_coplanarity_angle = 3.0f*AUX_DEG_TO_RADIANS/*3 degrees*/); + virtual ~ModelLibrary () + { + this->clear(); + } + + /** \brief Removes all models from the library and clears the hash table. */ + void + removeAllModels (); + + /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will + * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if + * "ignore co-planar points" is on. Call this method before calling addModel. */ + inline void + setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees) + { + max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS; + } + + /** \brief Call this method in order NOT to add co-planar point pairs to the hash table. The default behavior + * is ignoring co-planar points on. */ + inline void + ignoreCoplanarPointPairsOn () + { + ignore_coplanar_opps_ = true; + } + + /** \brief Call this method in order to add all point pairs (co-planar as well) to the hash table. The default + * behavior is ignoring co-planar points on. */ + inline void + ignoreCoplanarPointPairsOff () + { + ignore_coplanar_opps_ = false; + } + + /** \brief Adds a model to the hash table. + * + * \param[in] points represents the model to be added. + * \param[in] normals are the normals at the model points. + * \param[in] object_name is the unique name of the object to be added. + * \param[in] frac_of_points_for_registration is the number of points used for fast ICP registration prior to hypothesis testing + * \param[in] user_data is a pointer to some data (can be NULL) + * + * Returns true if model successfully added and false otherwise (e.g., if object_name is not unique). */ + bool + addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, + float frac_of_points_for_registration, void* user_data = nullptr); + + /** \brief Returns the hash table built by this instance. */ + inline const HashTable& + getHashTable () const + { + return (hash_table_); + } + + inline const Model* + getModel (const std::string& name) const + { + std::map::const_iterator it = models_.find (name); + if ( it != models_.end () ) + return (it->second); + + return (nullptr); + } + + inline const std::map& + getModels () const + { + return (models_); + } + + protected: + /** \brief Removes all models from the library and destroys the hash table. This method should be called upon destroying this object. */ + void + clear (); + + /** \brief Returns true if the oriented point pair was added to the hash table and false otherwise. */ + bool + addToHashTable (Model* model, const ORROctree::Node::Data* data1, const ORROctree::Node::Data* data2); + + protected: + float pair_width_; + float voxel_size_; + float max_coplanarity_angle_; + bool ignore_coplanar_opps_; + + std::map models_; + HashTable hash_table_; + int num_of_cells_[3]; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/obj_rec_ransac.h b/deps_install/include/pcl-1.10/pcl/recognition/obj_rec_ransac.h new file mode 100755 index 0000000..2a28c8d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/obj_rec_ransac.h @@ -0,0 +1,481 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define OBJ_REC_RANSAC_VERBOSE + +namespace pcl +{ + namespace recognition + { + /** \brief This is a RANSAC-based 3D object recognition method. Do the following to use it: (i) call addModel() k times with k different models + * representing the objects to be recognized and (ii) call recognize() with the 3D scene in which the objects should be recognized. Recognition means both + * object identification and pose (position + orientation) estimation. Check the method descriptions for more details. + * + * \note If you use this code in any academic work, please cite: + * + * - Chavdar Papazov, Sami Haddadin, Sven Parusel, Kai Krieger and Darius Burschka. + * Rigid 3D geometry matching for grasping of known objects in cluttered scenes. + * The International Journal of Robotics Research 2012. DOI: 10.1177/0278364911436019 + * + * - Chavdar Papazov and Darius Burschka. + * An Efficient RANSAC for 3D Object Recognition in Noisy and Occluded Scenes. + * In Proceedings of the 10th Asian Conference on Computer Vision (ACCV'10), + * November 2010. + * + * + * \author Chavdar Papazov + * \ingroup recognition + */ + class PCL_EXPORTS ObjRecRANSAC + { + public: + using PointCloudIn = ModelLibrary::PointCloudIn; + using PointCloudN = ModelLibrary::PointCloudN; + + using BVHH = BVH; + + /** \brief This is an output item of the ObjRecRANSAC::recognize() method. It contains the recognized model, its name (the ones passed to + * ObjRecRANSAC::addModel()), the rigid transform which aligns the model with the input scene and the match confidence which is a number + * in the interval (0, 1] which gives the fraction of the model surface area matched to the scene. E.g., a match confidence of 0.3 means + * that 30% of the object surface area was matched to the scene points. If the scene is represented by a single range image, the match + * confidence can not be greater than 0.5 since the range scanner sees only one side of each object. + */ + class Output + { + public: + Output (const std::string& object_name, const float rigid_transform[12], float match_confidence, void* user_data) : + object_name_ (object_name), + match_confidence_ (match_confidence), + user_data_ (user_data) + { + memcpy(this->rigid_transform_, rigid_transform, 12*sizeof (float)); + } + virtual ~Output (){} + + public: + std::string object_name_; + float rigid_transform_[12]; + float match_confidence_; + void* user_data_; + }; + + class OrientedPointPair + { + public: + OrientedPointPair (const float *p1, const float *n1, const float *p2, const float *n2) + : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2) + { + } + + virtual ~OrientedPointPair (){} + + public: + const float *p1_, *n1_, *p2_, *n2_; + }; + + class HypothesisCreator + { + public: + HypothesisCreator (){} + virtual ~HypothesisCreator (){} + + Hypothesis* create (const SimpleOctree::Node* ) const { return new Hypothesis ();} + }; + + using HypothesisOctree = SimpleOctree; + + public: + /** \brief Constructor with some important parameters which can not be changed once an instance of that class is created. + * + * \param[in] pair_width should be roughly half the extent of the visible object part. This means, for each object point p there should be (at least) + * one point q (from the same object) such that ||p - q|| <= pair_width. Tradeoff: smaller values allow for detection in more occluded scenes but lead + * to more imprecise alignment. Bigger values lead to better alignment but require large visible object parts (i.e., less occlusion). + * + * \param[in] voxel_size is the size of the leafs of the octree, i.e., the "size" of the discretization. Tradeoff: High values lead to less + * computation time but ignore object details. Small values allow to better distinguish between objects, but will introduce more holes in the resulting + * "voxel-surface" (especially for a sparsely sampled scene). */ + ObjRecRANSAC (float pair_width, float voxel_size); + virtual ~ObjRecRANSAC () + { + this->clear (); + this->clearTestData (); + } + + /** \brief Removes all models from the model library and releases some memory dynamically allocated by this instance. */ + void + inline clear() + { + model_library_.removeAllModels (); + scene_octree_.clear (); + scene_octree_proj_.clear (); + sampled_oriented_point_pairs_.clear (); + transform_space_.clear (); + scene_octree_points_.reset (); + } + + /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will + * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if + * "ignore co-planar points" is on. Call this method before calling addModel. This method calls the corresponding + * method of the model library. */ + inline void + setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees) + { + max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS; + model_library_.setMaxCoplanarityAngleDegrees (max_coplanarity_angle_degrees); + } + + inline void + setSceneBoundsEnlargementFactor (float value) + { + scene_bounds_enlargement_factor_ = value; + } + + /** \brief Default is on. This method calls the corresponding method of the model library. */ + inline void + ignoreCoplanarPointPairsOn () + { + ignore_coplanar_opps_ = true; + model_library_.ignoreCoplanarPointPairsOn (); + } + + /** \brief Default is on. This method calls the corresponding method of the model library. */ + inline void + ignoreCoplanarPointPairsOff () + { + ignore_coplanar_opps_ = false; + model_library_.ignoreCoplanarPointPairsOff (); + } + + inline void + icpHypothesesRefinementOn () + { + do_icp_hypotheses_refinement_ = true; + } + + inline void + icpHypothesesRefinementOff () + { + do_icp_hypotheses_refinement_ = false; + } + + /** \brief Add an object model to be recognized. + * + * \param[in] points are the object points. + * \param[in] normals at each point. + * \param[in] object_name is an identifier for the object. If that object is detected in the scene 'object_name' + * is returned by the recognition method and you know which object has been detected. Note that 'object_name' has + * to be unique! + * \param[in] user_data is a pointer to some data (can be NULL) + * + * The method returns true if the model was successfully added to the model library and false otherwise (e.g., if 'object_name' is already in use). + */ + inline bool + addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, void* user_data = nullptr) + { + return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data)); + } + + /** \brief This method performs the recognition of the models loaded to the model library with the method addModel(). + * + * \param[in] scene is the 3d scene in which the object should be recognized. + * \param[in] normals are the scene normals. + * \param[out] recognized_objects is the list of output items each one containing the recognized model instance, its name, the aligning rigid transform + * and the match confidence (see ObjRecRANSAC::Output for further explanations). + * \param[in] success_probability is the user-defined probability of detecting all objects in the scene. + */ + void + recognize (const PointCloudIn& scene, const PointCloudN& normals, std::list& recognized_objects, double success_probability = 0.99); + + inline void + enterTestModeSampleOPP () + { + rec_mode_ = ObjRecRANSAC::SAMPLE_OPP; + } + + inline void + enterTestModeTestHypotheses () + { + rec_mode_ = ObjRecRANSAC::TEST_HYPOTHESES; + } + + inline void + leaveTestMode () + { + rec_mode_ = ObjRecRANSAC::FULL_RECOGNITION; + } + + /** \brief This function is useful for testing purposes. It returns the oriented point pairs which were sampled from the + * scene during the recognition process. Makes sense only if some of the testing modes are active. */ + inline const std::list& + getSampledOrientedPointPairs () const + { + return (sampled_oriented_point_pairs_); + } + + /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the + * recognition process. Makes sense only if some of the testing modes are active. */ + inline const std::vector& + getAcceptedHypotheses () const + { + return (accepted_hypotheses_); + } + + /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the + * recognition process. Makes sense only if some of the testing modes are active. */ + inline void + getAcceptedHypotheses (std::vector& out) const + { + out = accepted_hypotheses_; + } + + /** \brief Returns the hash table in the model library. */ + inline const pcl::recognition::ModelLibrary::HashTable& + getHashTable () const + { + return (model_library_.getHashTable ()); + } + + inline const ModelLibrary& + getModelLibrary () const + { + return (model_library_); + } + + inline const ModelLibrary::Model* + getModel (const std::string& name) const + { + return (model_library_.getModel (name)); + } + + inline const ORROctree& + getSceneOctree () const + { + return (scene_octree_); + } + + inline RigidTransformSpace& + getRigidTransformSpace () + { + return (transform_space_); + } + + inline float + getPairWidth () const + { + return pair_width_; + } + + protected: + enum Recognition_Mode {SAMPLE_OPP, TEST_HYPOTHESES, /*BUILD_CONFLICT_GRAPH,*/ FULL_RECOGNITION}; + + friend class ModelLibrary; + + inline int + computeNumberOfIterations (double success_probability) const + { + // 'p_obj' is the probability that given that the first sample point belongs to an object, + // the second sample point will belong to the same object + const double p_obj = 0.25f; + // old version: p = p_obj*relative_obj_size_*fraction_of_pairs_in_hash_table_; + const double p = p_obj*relative_obj_size_; + + if ( 1.0 - p <= 0.0 ) + return 1; + + return static_cast (std::log (1.0-success_probability)/std::log (1.0-p) + 1.0); + } + + inline void + clearTestData () + { + sampled_oriented_point_pairs_.clear (); + accepted_hypotheses_.clear (); + transform_space_.clear (); + } + + void + sampleOrientedPointPairs (int num_iterations, const std::vector& full_scene_leaves, std::list& output) const; + + int + generateHypotheses (const std::list& pairs, std::list& out) const; + + /** \brief Groups close hypotheses in 'hypotheses'. Saves a representative for each group in 'out'. Returns the + * number of hypotheses after grouping. */ + int + groupHypotheses(std::list& hypotheses, int num_hypotheses, RigidTransformSpace& transform_space, + HypothesisOctree& grouped_hypotheses) const; + + inline void + testHypothesis (Hypothesis* hypothesis, int& match, int& penalty) const; + + inline void + testHypothesisNormalBased (Hypothesis* hypothesis, float& match) const; + + void + buildGraphOfCloseHypotheses (HypothesisOctree& hypotheses, ORRGraph& graph) const; + + void + filterGraphOfCloseHypotheses (ORRGraph& graph, std::vector& out) const; + + void + buildGraphOfConflictingHypotheses (const BVHH& bvh, ORRGraph& graph) const; + + void + filterGraphOfConflictingHypotheses (ORRGraph& graph, std::list& recognized_objects) const; + + /** \brief Computes the rigid transform that maps the line (a1, b1) to (a2, b2). + * The computation is based on the corresponding points 'a1' <-> 'a2' and 'b1' <-> 'b2' + * and the normals 'a1_n', 'b1_n', 'a2_n', and 'b2_n'. The result is saved in + * 'rigid_transform' which is an array of length 12. The first 9 elements are the + * rotational part (row major order) and the last 3 are the translation. */ + inline void + computeRigidTransform( + const float *a1, const float *a1_n, const float *b1, const float* b1_n, + const float *a2, const float *a2_n, const float *b2, const float* b2_n, + float* rigid_transform) const + { + // Some local variables + float o1[3], o2[3], x1[3], x2[3], y1[3], y2[3], z1[3], z2[3], tmp1[3], tmp2[3], Ro1[3], invFrame1[3][3]; + + // Compute the origins + o1[0] = 0.5f*(a1[0] + b1[0]); + o1[1] = 0.5f*(a1[1] + b1[1]); + o1[2] = 0.5f*(a1[2] + b1[2]); + + o2[0] = 0.5f*(a2[0] + b2[0]); + o2[1] = 0.5f*(a2[1] + b2[1]); + o2[2] = 0.5f*(a2[2] + b2[2]); + + // Compute the x-axes + aux::diff3 (b1, a1, x1); aux::normalize3 (x1); + aux::diff3 (b2, a2, x2); aux::normalize3 (x2); + // Compute the y-axes. First y-axis + aux::projectOnPlane3 (a1_n, x1, tmp1); aux::normalize3 (tmp1); + aux::projectOnPlane3 (b1_n, x1, tmp2); aux::normalize3 (tmp2); + aux::sum3 (tmp1, tmp2, y1); aux::normalize3 (y1); + // Second y-axis + aux::projectOnPlane3 (a2_n, x2, tmp1); aux::normalize3 (tmp1); + aux::projectOnPlane3 (b2_n, x2, tmp2); aux::normalize3 (tmp2); + aux::sum3 (tmp1, tmp2, y2); aux::normalize3 (y2); + // Compute the z-axes + aux::cross3 (x1, y1, z1); + aux::cross3 (x2, y2, z2); + + // 1. Invert the matrix [x1|y1|z1] (note that x1, y1, and z1 are treated as columns!) + invFrame1[0][0] = x1[0]; invFrame1[0][1] = x1[1]; invFrame1[0][2] = x1[2]; + invFrame1[1][0] = y1[0]; invFrame1[1][1] = y1[1]; invFrame1[1][2] = y1[2]; + invFrame1[2][0] = z1[0]; invFrame1[2][1] = z1[1]; invFrame1[2][2] = z1[2]; + // 2. Compute the desired rotation as rigid_transform = [x2|y2|z2]*invFrame1 + aux::mult3x3 (x2, y2, z2, invFrame1, rigid_transform); + + // Construct the translation which is the difference between the rotated o1 and o2 + aux::mult3x3 (rigid_transform, o1, Ro1); + rigid_transform[9] = o2[0] - Ro1[0]; + rigid_transform[10] = o2[1] - Ro1[1]; + rigid_transform[11] = o2[2] - Ro1[2]; + } + + /** \brief Computes the signature of the oriented point pair ((p1, n1), (p2, n2)) consisting of the angles between + * \param p1 + * \param n1 + * \param p2 + * \param n2 + * \param[out] signature is an array of three doubles saving the three angles in the order shown above. */ + static inline void + compute_oriented_point_pair_signature (const float *p1, const float *n1, const float *p2, const float *n2, float signature[3]) + { + // Get the line from p1 to p2 + float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]}; + aux::normalize3 (cl); + + signature[0] = std::acos (aux::clamp (aux::dot3 (n1,cl), -1.0f, 1.0f)); cl[0] = -cl[0]; cl[1] = -cl[1]; cl[2] = -cl[2]; + signature[1] = std::acos (aux::clamp (aux::dot3 (n2,cl), -1.0f, 1.0f)); + signature[2] = std::acos (aux::clamp (aux::dot3 (n1,n2), -1.0f, 1.0f)); + } + + protected: + // Parameters + float pair_width_; + float voxel_size_; + float position_discretization_; + float rotation_discretization_; + float abs_zdist_thresh_; + float relative_obj_size_; + float visibility_; + float relative_num_of_illegal_pts_; + float intersection_fraction_; + float max_coplanarity_angle_; + float scene_bounds_enlargement_factor_; + bool ignore_coplanar_opps_; + float frac_of_points_for_icp_refinement_; + bool do_icp_hypotheses_refinement_; + + ModelLibrary model_library_; + ORROctree scene_octree_; + ORROctreeZProjection scene_octree_proj_; + RigidTransformSpace transform_space_; + TrimmedICP trimmed_icp_; + PointCloudIn::Ptr scene_octree_points_; + + std::list sampled_oriented_point_pairs_; + std::vector accepted_hypotheses_; + Recognition_Mode rec_mode_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/orr_graph.h b/deps_install/include/pcl-1.10/pcl/recognition/orr_graph.h new file mode 100755 index 0000000..cf118ba --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/orr_graph.h @@ -0,0 +1,226 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * orr_graph.h + * + * Created on: Nov 23, 2012 + * Author: papazov + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class ORRGraph + { + public: + class Node + { + public: + enum State {ON, OFF, UNDEF}; + + Node (int id) + : id_ (id), + state_(UNDEF) + {} + + virtual ~Node (){} + + inline const std::set& + getNeighbors () const + { + return (neighbors_); + } + + inline const NodeData& + getData () const + { + return (data_); + } + + inline void + setData (const NodeData& data) + { + data_ = data; + } + + inline int + getId () const + { + return (id_); + } + + inline void + setId (int id) + { + id_ = id; + } + + inline void + setFitness (int fitness) + { + fitness_ = fitness; + } + + static inline bool + compare (const Node* a, const Node* b) + { + return a->fitness_ > b->fitness_; + } + + friend class ORRGraph; + + protected: + std::set neighbors_; + NodeData data_; + int id_; + int fitness_; + State state_; + }; + + public: + ORRGraph (){} + virtual ~ORRGraph (){ this->clear ();} + + inline void + clear () + { + for ( typename std::vector::iterator nit = nodes_.begin () ; nit != nodes_.end () ; ++nit ) + delete *nit; + + nodes_.clear (); + } + + /** \brief Drops all existing graph nodes and creates 'n' new ones. */ + inline void + resize (int n) + { + if ( !n ) + return; + + for ( typename std::vector::iterator nit = nodes_.begin () ; nit != nodes_.end () ; ++nit ) + delete *nit; + + nodes_.resize (static_cast (n)); + + for ( int i = 0 ; i < n ; ++i ) + nodes_[i] = new Node (i); + } + + inline void + computeMaximalOnOffPartition (std::list& on_nodes, std::list& off_nodes) + { + std::vector sorted_nodes (nodes_.size ()); + int i = 0; + + // Set all nodes to undefined + for ( typename std::vector::iterator it = nodes_.begin () ; it != nodes_.end () ; ++it ) + { + sorted_nodes[i++] = *it; + (*it)->state_ = Node::UNDEF; + } + + // Now sort the nodes according to the fitness + std::sort (sorted_nodes.begin (), sorted_nodes.end (), Node::compare); + + // Now run through the array and start switching nodes on and off + for ( typename std::vector::iterator it = sorted_nodes.begin () ; it != sorted_nodes.end () ; ++it ) + { + // Ignore graph nodes which are already OFF + if ( (*it)->state_ == Node::OFF ) + continue; + + // Set the node to ON + (*it)->state_ = Node::ON; + + // Set all its neighbors to OFF + for ( typename std::set::iterator neigh = (*it)->neighbors_.begin () ; neigh != (*it)->neighbors_.end () ; ++neigh ) + { + (*neigh)->state_ = Node::OFF; + off_nodes.push_back (*neigh); + } + + // Output the node + on_nodes.push_back (*it); + } + } + + inline void + insertUndirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.insert (nodes_[id2]); + nodes_[id2]->neighbors_.insert (nodes_[id1]); + } + + inline void + insertDirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.insert (nodes_[id2]); + } + + inline void + deleteUndirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.erase (nodes_[id2]); + nodes_[id2]->neighbors_.erase (nodes_[id1]); + } + + inline void + deleteDirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.erase (nodes_[id2]); + } + + inline typename std::vector& + getNodes (){ return nodes_;} + + public: + typename std::vector nodes_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/orr_octree.h b/deps_install/include/pcl-1.10/pcl/recognition/orr_octree.h new file mode 100755 index 0000000..0655064 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/orr_octree.h @@ -0,0 +1,489 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * orr_octree.h + * + * Created on: Oct 23, 2012 + * Author: papazov + */ + +#pragma once + +#include "auxiliary.h" +#include +#include +#include +#include +#include +#include +#include +#include + +//#define PCL_REC_ORR_OCTREE_VERBOSE + +namespace pcl +{ + namespace recognition + { + /** \brief That's a very specialized and simple octree class. That's the way it is intended to + * be, that's why no templates and stuff like this. + * + * \author Chavdar Papazov + * \ingroup recognition + */ + class PCL_EXPORTS ORROctree + { + public: + using PointCloudIn = pcl::PointCloud; + using PointCloudOut = pcl::PointCloud; + using PointCloudN = pcl::PointCloud; + + class Node + { + public: + class Data + { + public: + Data (int id_x, int id_y, int id_z, int lin_id, void* user_data = nullptr) + : id_x_ (id_x), + id_y_ (id_y), + id_z_ (id_z), + lin_id_ (lin_id), + num_points_ (0), + user_data_ (user_data) + { + n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f; + } + + virtual~ Data (){} + + inline void + addToPoint (float x, float y, float z) + { + p_[0] += x; p_[1] += y; p_[2] += z; + ++num_points_; + } + + inline void + computeAveragePoint () + { + if ( num_points_ < 2 ) + return; + + aux::mult3 (p_, 1.0f/static_cast (num_points_)); + num_points_ = 1; + } + + inline void + addToNormal (float x, float y, float z) { n_[0] += x; n_[1] += y; n_[2] += z;} + + inline const float* + getPoint () const { return p_;} + + inline float* + getPoint (){ return p_;} + + inline const float* + getNormal () const { return n_;} + + inline float* + getNormal (){ return n_;} + + inline void + get3dId (int id[3]) const + { + id[0] = id_x_; + id[1] = id_y_; + id[2] = id_z_; + } + + inline int + get3dIdX () const {return id_x_;} + + inline int + get3dIdY () const {return id_y_;} + + inline int + get3dIdZ () const {return id_z_;} + + inline int + getLinearId () const { return lin_id_;} + + inline void + setUserData (void* user_data){ user_data_ = user_data;} + + inline void* + getUserData () const { return user_data_;} + + inline void + insertNeighbor (Node* node){ neighbors_.insert (node);} + + inline const std::set& + getNeighbors () const { return (neighbors_);} + + protected: + float n_[3], p_[3]; + int id_x_, id_y_, id_z_, lin_id_, num_points_; + std::set neighbors_; + void *user_data_; + }; + + Node () + : data_ (nullptr), + parent_ (nullptr), + children_(nullptr) + {} + + virtual~ Node () + { + this->deleteChildren (); + this->deleteData (); + } + + inline void + setCenter(const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];} + + inline void + setBounds(const float *b) { bounds_[0] = b[0]; bounds_[1] = b[1]; bounds_[2] = b[2]; bounds_[3] = b[3]; bounds_[4] = b[4]; bounds_[5] = b[5];} + + inline void + setParent(Node* parent) { parent_ = parent;} + + inline void + setData(Node::Data* data) { data_ = data;} + + /** \brief Computes the "radius" of the node which is half the diagonal length. */ + inline void + computeRadius() + { + float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])}; + radius_ = static_cast (aux::length3 (v)); + } + + inline const float* + getCenter() const { return center_;} + + inline const float* + getBounds() const { return bounds_;} + + inline void + getBounds(float b[6]) const + { + memcpy (b, bounds_, 6*sizeof (float)); + } + + inline Node* + getChild (int id) { return &children_[id];} + + inline Node* + getChildren () { return children_;} + + inline Node::Data* + getData (){ return data_;} + + inline const Node::Data* + getData () const { return data_;} + + inline void + setUserData (void* user_data){ data_->setUserData (user_data);} + + inline Node* + getParent (){ return parent_;} + + inline bool + hasData (){ return static_cast (data_);} + + inline bool + hasChildren (){ return static_cast (children_);} + + /** \brief Computes the "radius" of the node which is half the diagonal length. */ + inline float + getRadius (){ return radius_;} + + bool + createChildren (); + + inline void + deleteChildren () + { + if ( children_ ) + { + delete[] children_; + children_ = nullptr; + } + } + + inline void + deleteData () + { + if ( data_ ) + { + delete data_; + data_ = nullptr; + } + } + + /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens + * of either of the nodes has no data. */ + inline void + makeNeighbors (Node* node) + { + if ( !this->getData () || !node->getData () ) + return; + + this->getData ()->insertNeighbor (node); + node->getData ()->insertNeighbor (this); + } + + protected: + Node::Data *data_; + float center_[3], bounds_[6], radius_; + Node *parent_, *children_; + }; + + ORROctree (); + virtual ~ORROctree (){ this->clear ();} + + void + clear (); + + /** \brief Creates an octree which encloses 'points' and with leaf size equal to 'voxel_size'. + * 'enlarge_bounds' makes sure that no points from the input will lie on the octree boundary + * by enlarging the bounds by that factor. For example, enlarge_bounds = 1 means that the + * bounds will be enlarged by 100%. The default value is fine. */ + void + build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals = nullptr, float enlarge_bounds = 0.00001f); + + /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf + * size equal to 'voxel_size'. */ + void + build (const float* bounds, float voxel_size); + + /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within + * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method + * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and + * method just returns a pointer to the leaf. */ + inline ORROctree::Node* + createLeaf (float x, float y, float z) + { + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + ORROctree::Node* node = root_; + const float *c; + int id; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + node->createChildren (); + c = node->getCenter (); + id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->getData () ) + { + Node::Data* data = new Node::Data ( + static_cast ((node->getCenter ()[0] - bounds_[0])/voxel_size_), + static_cast ((node->getCenter ()[1] - bounds_[2])/voxel_size_), + static_cast ((node->getCenter ()[2] - bounds_[4])/voxel_size_), + static_cast (full_leaves_.size ())); + + node->setData (data); + this->insertNeighbors (node); + full_leaves_.push_back (node); + } + + return (node); + } + + /** \brief This method returns a super set of the full leavess which are intersected by the sphere + * with radius 'radius' and centered at 'p'. Pointers to the intersected full leaves are saved in + * 'out'. The method computes a super set in the sense that in general not all leaves saved in 'out' + * are really intersected by the sphere. The intersection test is based on the leaf radius (since + * its faster than checking all leaf corners and sides), so we report more leaves than we should, + * but still, this is a fair approximation. */ + void + getFullLeavesIntersectedBySphere (const float* p, float radius, std::list& out) const; + + /** \brief Randomly chooses and returns a full leaf that is intersected by the sphere with center 'p' + * and 'radius'. Returns NULL if no leaf is intersected by that sphere. */ + ORROctree::Node* + getRandomFullLeafOnSphere (const float* p, float radius) const; + + /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the leaf + * with id [i, j, k] or NULL is no such leaf exists. */ + ORROctree::Node* + getLeaf (int i, int j, int k) + { + float offset = 0.5f*voxel_size_; + float p[3] = {bounds_[0] + offset + static_cast (i)*voxel_size_, + bounds_[2] + offset + static_cast (j)*voxel_size_, + bounds_[4] + offset + static_cast (k)*voxel_size_}; + + return (this->getLeaf (p[0], p[1], p[2])); + } + + /** \brief Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists. */ + inline ORROctree::Node* + getLeaf (float x, float y, float z) + { + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + ORROctree::Node* node = root_; + const float *c; + int id; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + if ( !node->hasChildren () ) + return (nullptr); + + c = node->getCenter (); + id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + return (node); + } + + /** \brief Deletes the branch 'node' is part of. */ + void + deleteBranch (Node* node); + + /** \brief Returns a vector with all octree leaves which contain at least one point. */ + inline std::vector& + getFullLeaves () { return full_leaves_;} + + inline const std::vector& + getFullLeaves () const { return full_leaves_;} + + void + getFullLeavesPoints (PointCloudOut& out) const; + + void + getNormalsOfFullLeaves (PointCloudN& out) const; + + inline ORROctree::Node* + getRoot (){ return root_;} + + inline const float* + getBounds () const + { + return (bounds_); + } + + inline void + getBounds (float b[6]) const + { + memcpy (b, bounds_, 6*sizeof (float)); + } + + inline float + getVoxelSize () const { return voxel_size_;} + + inline void + insertNeighbors (Node* node) + { + const float* c = node->getCenter (); + float s = 0.5f*voxel_size_; + Node *neigh; + + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + //neigh = this->getLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + } + + protected: + float voxel_size_, bounds_[6]; + int tree_levels_; + Node* root_; + std::vector full_leaves_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/orr_octree_zprojection.h b/deps_install/include/pcl-1.10/pcl/recognition/orr_octree_zprojection.h new file mode 100755 index 0000000..ea36329 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/orr_octree_zprojection.h @@ -0,0 +1,211 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * orr_octree_zprojection.h + * + * Created on: Nov 17, 2012 + * Author: papazov + */ + +#pragma once + +#include "orr_octree.h" +#include +#include + +namespace pcl +{ + namespace recognition + { + class ORROctree; + + class PCL_EXPORTS ORROctreeZProjection + { + public: + class Pixel + { + public: + Pixel (int id): id_ (id) {} + + inline void + set_z1 (float z1) { z1_ = z1;} + + inline void + set_z2 (float z2) { z2_ = z2;} + + float + z1 () const { return z1_;} + + float + z2 () const { return z2_;} + + int + getId () const { return id_;} + + protected: + float z1_, z2_; + int id_; + }; + + public: + class Set + { + public: + Set (int x, int y) + : nodes_ (compare_nodes_z), x_ (x), y_ (y) + {} + + static inline bool + compare_nodes_z (ORROctree::Node* node1, ORROctree::Node* node2) + { + return node1->getData()->get3dIdZ() < node2->getData()->get3dIdZ(); + } + + inline void + insert (ORROctree::Node* leaf) { nodes_.insert(leaf);} + + inline std::set& + get_nodes (){ return nodes_;} + + inline int + get_x () const { return x_;} + + inline int + get_y () const { return y_;} + + protected: + std::set nodes_; + int x_, y_; + }; + + public: + ORROctreeZProjection () + : pixels_(nullptr), + sets_(nullptr) + {} + virtual ~ORROctreeZProjection (){ this->clear();} + + void + build (const ORROctree& input, float eps_front, float eps_back); + + void + clear (); + + inline void + getPixelCoordinates (const float* p, int& x, int& y) const + { + x = static_cast ((p[0] - bounds_[0])*inv_pixel_size_); + y = static_cast ((p[1] - bounds_[2])*inv_pixel_size_); + } + + inline const Pixel* + getPixel (const float* p) const + { + int x, y; this->getPixelCoordinates (p, x, y); + + if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); + if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); + + return (pixels_[x][y]); + } + + inline Pixel* + getPixel (const float* p) + { + int x, y; this->getPixelCoordinates (p, x, y); + + if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); + if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); + + return (pixels_[x][y]); + } + + inline const std::set* + getOctreeNodes (const float* p) const + { + int x, y; this->getPixelCoordinates (p, x, y); + + if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); + if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); + + if ( !sets_[x][y] ) + return nullptr; + + return (&sets_[x][y]->get_nodes ()); + } + + inline std::list& + getFullPixels (){ return full_pixels_;} + + inline const Pixel* + getPixel (int i, int j) const + { + return pixels_[i][j]; + } + + inline float + getPixelSize () const + { + return pixel_size_; + } + + inline const float* + getBounds () const + { + return bounds_; + } + + /** \brief Get the width ('num_x') and height ('num_y') of the image. */ + inline void + getNumberOfPixels (int& num_x, int& num_y) const + { + num_x = num_pixels_x_; + num_y = num_pixels_y_; + } + + protected: + float pixel_size_, inv_pixel_size_, bounds_[4], extent_x_, extent_y_; + int num_pixels_x_, num_pixels_y_, num_pixels_; + Pixel ***pixels_; + Set ***sets_; + std::list full_sets_; + std::list full_pixels_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/point_types.h b/deps_install/include/pcl-1.10/pcl/recognition/point_types.h new file mode 100755 index 0000000..7e37a17 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/point_types.h @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + + +namespace pcl +{ + /** \brief A point structure representing Euclidean xyz coordinates, and the intensity value. + * \ingroup common + */ + struct EIGEN_ALIGN16 GradientXY + { + union + { + struct + { + float x; + float y; + float angle; + float magnitude; + }; + float data[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + + inline bool operator< (const GradientXY & rhs) + { + return (magnitude > rhs.magnitude); + } + }; + inline std::ostream & operator << (std::ostream & os, const GradientXY & p) + { + os << "(" << p.x << "," << p.y << " - " << p.magnitude << ")"; + return (os); + } + +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/quantizable_modality.h b/deps_install/include/pcl-1.10/pcl/recognition/quantizable_modality.h new file mode 100755 index 0000000..6c1fc3f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/quantizable_modality.h @@ -0,0 +1,89 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Interface for a quantizable modality. + * \author Stefan Holzer + */ + class PCL_EXPORTS QuantizableModality + { + public: + /** \brief Constructor. */ + QuantizableModality (); + /** \brief Destructor. */ + virtual ~QuantizableModality (); + + /** \brief Returns a reference to the internally computed quantized map. */ + virtual QuantizedMap & + getQuantizedMap () = 0; + + /** \brief Returns a reference to the internally computed spread quantized map. */ + virtual QuantizedMap & + getSpreadedQuantizedMap () = 0; + + /** \brief Extracts features from this modality within the specified mask. + * \param[in] mask defines the areas where features are searched in. + * \param[in] nr_features defines the number of features to be extracted + * (might be less if not sufficient information is present in the modality). + * \param[in] modality_index the index which is stored in the extracted features. + * \param[out] features the destination for the extracted features. + */ + virtual void + extractFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modality_index, + std::vector & features) const = 0; + + /** \brief Extracts all possible features from the modality within the specified mask. + * \param[in] mask defines the areas where features are searched in. + * \param[in] nr_features IGNORED (TODO: remove this parameter). + * \param[in] modality_index the index which is stored in the extracted features. + * \param[out] features the destination for the extracted features. + */ + virtual void + extractAllFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modality_index, + std::vector & features) const = 0; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/quantized_map.h b/deps_install/include/pcl-1.10/pcl/recognition/quantized_map.h new file mode 100755 index 0000000..756c4ba --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/quantized_map.h @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + class PCL_EXPORTS QuantizedMap + { + public: + + QuantizedMap (); + QuantizedMap (std::size_t width, std::size_t height); + QuantizedMap (const QuantizedMap & copy_me); + + virtual ~QuantizedMap (); + + inline std::size_t + getWidth () const { return (width_); } + + inline std::size_t + getHeight () const { return (height_); } + + inline unsigned char* + getData () { return (&data_[0]); } + + inline const unsigned char* + getData () const { return (&data_[0]); } + + inline QuantizedMap + getSubMap (std::size_t x, + std::size_t y, + std::size_t width, + std::size_t height) + { + QuantizedMap subMap(width, height); + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + //const std::size_t index = (row_index+y)*width_ + (col_index+x); + //const unsigned char value = data_[index]; + //subMap.data_[row_index*width + col_index] = value;//data_[(row_index+y)*width_ + (col_index+x)]; + subMap (col_index, row_index) = (*this) (col_index + x, row_index + y); + } + } + + return subMap; + } + + void + resize (std::size_t width, std::size_t height); + + inline unsigned char & + operator() (const std::size_t x, const std::size_t y) + { + return (data_[y*width_+x]); + } + + inline const unsigned char & + operator() (const std::size_t x, const std::size_t y) const + { + return (data_[y*width_+x]); + } + + static void + spreadQuantizedMap (const QuantizedMap & input_map, QuantizedMap & output_map, std::size_t spreading_size); + + void + serialize (std::ostream & stream) const + { + const int width = static_cast (width_); + const int height = static_cast (height_); + + stream.write (reinterpret_cast (&width), sizeof (width)); + stream.write (reinterpret_cast (&height), sizeof (height)); + + const int num_of_elements = static_cast (data_.size ()); + stream.write (reinterpret_cast (&num_of_elements), sizeof (num_of_elements)); + for (int element_index = 0; element_index < num_of_elements; ++element_index) + { + stream.write (reinterpret_cast (&(data_[element_index])), sizeof (data_[element_index])); + } + } + + void + deserialize (std::istream & stream) + { + int width; + int height; + + stream.read (reinterpret_cast (&width), sizeof (width)); + stream.read (reinterpret_cast (&height), sizeof (height)); + + width_ = static_cast (width); + height_ = static_cast (height); + + int num_of_elements; + stream.read (reinterpret_cast (&num_of_elements), sizeof (num_of_elements)); + data_.resize (num_of_elements); + for (int element_index = 0; element_index < num_of_elements; ++element_index) + { + stream.read (reinterpret_cast (&(data_[element_index])), sizeof (data_[element_index])); + } + } + + + //private: + std::vector data_; + std::size_t width_; + std::size_t height_; + + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/auxiliary.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/auxiliary.h new file mode 100755 index 0000000..0a553ed --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/auxiliary.h @@ -0,0 +1,464 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +#define AUX_PI_FLOAT 3.14159265358979323846f +#define AUX_HALF_PI 1.57079632679489661923f +#define AUX_DEG_TO_RADIANS (3.14159265358979323846f/180.0f) + +namespace pcl +{ + namespace recognition + { + namespace aux + { + template bool + compareOrderedPairs (const std::pair& a, const std::pair& b) + { + if ( a.first == b.first ) + return a.second < b.second; + + return a.first < b.first; + } + + template T + sqr (T a) + { + return (a*a); + } + + template T + clamp (T value, T min, T max) + { + if ( value < min ) + return min; + if ( value > max ) + return max; + + return value; + } + + /** \brief Expands the destination bounding box 'dst' such that it contains 'src'. */ + template void + expandBoundingBox (T dst[6], const T src[6]) + { + if ( src[0] < dst[0] ) dst[0] = src[0]; + if ( src[2] < dst[2] ) dst[2] = src[2]; + if ( src[4] < dst[4] ) dst[4] = src[4]; + + if ( src[1] > dst[1] ) dst[1] = src[1]; + if ( src[3] > dst[3] ) dst[3] = src[3]; + if ( src[5] > dst[5] ) dst[5] = src[5]; + } + + /** \brief Expands the bounding box 'bbox' such that it contains the point 'p'. */ + template void + expandBoundingBoxToContainPoint (T bbox[6], const T p[3]) + { + if ( p[0] < bbox[0] ) bbox[0] = p[0]; + else if ( p[0] > bbox[1] ) bbox[1] = p[0]; + + if ( p[1] < bbox[2] ) bbox[2] = p[1]; + else if ( p[1] > bbox[3] ) bbox[3] = p[1]; + + if ( p[2] < bbox[4] ) bbox[4] = p[2]; + else if ( p[2] > bbox[5] ) bbox[5] = p[2]; + } + + /** \brief v[0] = v[1] = v[2] = value */ + template void + set3 (T v[3], T value) + { + v[0] = v[1] = v[2] = value; + } + + /** \brief dst = src */ + template void + copy3 (const T src[3], T dst[3]) + { + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + } + + /** \brief dst = src */ + template void + copy3 (const T src[3], pcl::PointXYZ& dst) + { + dst.x = src[0]; + dst.y = src[1]; + dst.z = src[2]; + } + + /** \brief a = -a */ + template void + flip3 (T a[3]) + { + a[0] = -a[0]; + a[1] = -a[1]; + a[2] = -a[2]; + } + + /** \brief a = b */ + template bool + equal3 (const T a[3], const T b[3]) + { + return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]); + } + + /** \brief a += b */ + template void + add3 (T a[3], const T b[3]) + { + a[0] += b[0]; + a[1] += b[1]; + a[2] += b[2]; + } + + /** \brief c = a + b */ + template void + sum3 (const T a[3], const T b[3], T c[3]) + { + c[0] = a[0] + b[0]; + c[1] = a[1] + b[1]; + c[2] = a[2] + b[2]; + } + + /** \brief c = a - b */ + template void + diff3 (const T a[3], const T b[3], T c[3]) + { + c[0] = a[0] - b[0]; + c[1] = a[1] - b[1]; + c[2] = a[2] - b[2]; + } + + template void + cross3 (const T v1[3], const T v2[3], T out[3]) + { + out[0] = v1[1]*v2[2] - v1[2]*v2[1]; + out[1] = v1[2]*v2[0] - v1[0]*v2[2]; + out[2] = v1[0]*v2[1] - v1[1]*v2[0]; + } + + /** \brief Returns the length of v. */ + template T + length3 (const T v[3]) + { + return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); + } + + /** \brief Returns the Euclidean distance between a and b. */ + template T + distance3 (const T a[3], const T b[3]) + { + T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]}; + return (length3 (l)); + } + + /** \brief Returns the squared Euclidean distance between a and b. */ + template T + sqrDistance3 (const T a[3], const T b[3]) + { + return (aux::sqr (a[0]-b[0]) + aux::sqr (a[1]-b[1]) + aux::sqr (a[2]-b[2])); + } + + /** \brief Returns the dot product a*b */ + template T + dot3 (const T a[3], const T b[3]) + { + return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]); + } + + /** \brief Returns the dot product (x, y, z)*(u, v, w) = x*u + y*v + z*w */ + template T + dot3 (T x, T y, T z, T u, T v, T w) + { + return (x*u + y*v + z*w); + } + + /** \brief v = scalar*v. */ + template void + mult3 (T* v, T scalar) + { + v[0] *= scalar; + v[1] *= scalar; + v[2] *= scalar; + } + + /** \brief out = scalar*v. */ + template void + mult3 (const T* v, T scalar, T* out) + { + out[0] = v[0]*scalar; + out[1] = v[1]*scalar; + out[2] = v[2]*scalar; + } + + /** \brief Normalize v */ + template void + normalize3 (T v[3]) + { + T inv_len = (static_cast (1.0))/aux::length3 (v); + v[0] *= inv_len; + v[1] *= inv_len; + v[2] *= inv_len; + } + + /** \brief Returns the square length of v. */ + template T + sqrLength3 (const T v[3]) + { + return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); + } + + /** Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'. */ + template void + projectOnPlane3 (const T x[3], const T planeNormal[3], T out[3]) + { + T dot = aux::dot3 (planeNormal, x); + // Project 'x' on the plane normal + T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]}; + aux::sum3 (x, nproj, out); + } + + /** \brief Sets 'm' to the 3x3 identity matrix. */ + template void + identity3x3 (T m[9]) + { + m[0] = m[4] = m[8] = 1.0; + m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0; + } + + /** \brief out = mat*v. 'm' is an 1D array of 9 elements treated as a 3x3 matrix (row major order). */ + template void + mult3x3(const T m[9], const T v[3], T out[3]) + { + out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2]; + out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5]; + out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8]; + } + + /** Let x, y, z be the columns of the matrix a = [x|y|z]. The method computes out = a*m. + * Note that 'out' is a 1D array of 9 elements and the resulting matrix is stored in row + * major order, i.e., the first matrix row is (out[0] out[1] out[2]), the second + * (out[3] out[4] out[5]) and the third (out[6] out[7] out[8]). */ + template void + mult3x3 (const T x[3], const T y[3], const T z[3], const T m[3][3], T out[9]) + { + out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0]; + out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1]; + out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2]; + + out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0]; + out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1]; + out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2]; + + out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0]; + out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1]; + out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2]; + } + + /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. + * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */ + template void + transform(const T t[12], const T p[3], T out[3]) + { + out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9]; + out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10]; + out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11]; + } + + /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. + * First, (x, y, z) is multiplied by that matrix and then translated. The result is saved in 'out'. */ + template void + transform(const T t[12], T x, T y, T z, T out[3]) + { + out[0] = t[0]*x + t[1]*y + t[2]*z + t[9]; + out[1] = t[3]*x + t[4]*y + t[5]*z + t[10]; + out[2] = t[6]*x + t[7]*y + t[8]*z + t[11]; + } + + /** \brief Compute out = (upper left 3x3 of mat)*p + last column of mat. */ + template void + transform(const Eigen::Matrix& mat, const pcl::PointXYZ& p, pcl::PointXYZ& out) + { + out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3); + out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3); + out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3); + } + + /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. + * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */ + template void + transform(const T t[12], const pcl::PointXYZ& p, T out[3]) + { + out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9]; + out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10]; + out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11]; + } + + /** \brief Returns true if the points 'p1' and 'p2' are co-planar and false otherwise. The method assumes that 'n1' + * is a normal at 'p1' and 'n2' is a normal at 'p2'. 'max_angle' is the threshold used for the test. The bigger + * the value the larger the deviation between the normals can be which still leads to a positive test result. The + * angle has to be in radians. */ + template bool + pointsAreCoplanar (const T p1[3], const T n1[3], const T p2[3], const T n2[3], T max_angle) + { + // Compute the angle between 'n1' and 'n2' and compare it with 'max_angle' + if ( std::acos (aux::clamp (aux::dot3 (n1, n2), -1.0f, 1.0f)) > max_angle ) + return (false); + + T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]}; + aux::normalize3 (cl); + + // Compute the angle between 'cl' and 'n1' + T tmp_angle = std::acos (aux::clamp (aux::dot3 (n1, cl), -1.0f, 1.0f)); + + // 'tmp_angle' should not deviate too much from 90 degrees + if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle ) + return (false); + + // All tests passed => the points are coplanar + return (true); + } + + template void + array12ToMatrix4x4 (const Scalar src[12], Eigen::Matrix& dst) + { + dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9]; + dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10]; + dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11]; + dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0; + } + + template void + matrix4x4ToArray12 (const Eigen::Matrix& src, Scalar dst[12]) + { + dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3); + dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3); + dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3); + } + + /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order. + * dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); + * dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); + * dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); + * */ + template void + eigenMatrix3x3ToArray9RowMajor (const Eigen::Matrix& src, T dst[9]) + { + dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); + dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); + dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); + } + + /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order. + * dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; + * dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; + * dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; + * */ + template void + toEigenMatrix3x3RowMajor (const T src[9], Eigen::Matrix& dst) + { + dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; + dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; + dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; + } + + /** brief Computes a rotation matrix from the provided input vector 'axis_angle'. The direction of 'axis_angle' is the rotation axis + * and its magnitude is the angle of rotation about that axis. 'rotation_matrix' is the output rotation matrix saved in row major order. */ + template void + axisAngleToRotationMatrix (const T axis_angle[3], T rotation_matrix[9]) + { + // Get the angle of rotation + T angle = aux::length3 (axis_angle); + if ( angle == 0.0 ) + { + // Undefined rotation -> set to identity + aux::identity3x3 (rotation_matrix); + return; + } + + // Normalize the input + T normalized_axis_angle[3]; + aux::mult3 (axis_angle, static_cast (1.0)/angle, normalized_axis_angle); + + // The eigen objects + Eigen::Matrix mat_axis(normalized_axis_angle); + Eigen::AngleAxis eigen_angle_axis (angle, mat_axis); + + // Save the output + aux::eigenMatrix3x3ToArray9RowMajor (eigen_angle_axis.toRotationMatrix (), rotation_matrix); + } + + /** brief Extracts the angle-axis representation from 'rotation_matrix', i.e., computes a rotation 'axis' and an 'angle' + * of rotation about that axis from the provided input. The output 'angle' is in the range [0, pi] and 'axis' is normalized. */ + template void + rotationMatrixToAxisAngle (const T rotation_matrix[9], T axis[3], T& angle) + { + // The eigen objects + Eigen::AngleAxis angle_axis; + Eigen::Matrix rot_mat; + // Copy the input matrix to the eigen matrix in row major order + aux::toEigenMatrix3x3RowMajor (rotation_matrix, rot_mat); + + // Do the computation + angle_axis.fromRotationMatrix (rot_mat); + + // Save the result + axis[0] = angle_axis.axis () (0,0); + axis[1] = angle_axis.axis () (1,0); + axis[2] = angle_axis.axis () (2,0); + angle = angle_axis.angle (); + + // Make sure that 'angle' is in the range [0, pi] + if ( angle > AUX_PI_FLOAT ) + { + angle = 2.0f*AUX_PI_FLOAT - angle; + aux::flip3 (axis); + } + } + } // namespace aux + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/bvh.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/bvh.h new file mode 100755 index 0000000..fd9e054 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/bvh.h @@ -0,0 +1,310 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * bvh.h + * + * Created on: Mar 7, 2013 + * Author: papazov + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + /** \brief This class is an implementation of bounding volume hierarchies. Use the build method to construct + * the data structure. To use the class, construct an std::vector of pointers to BVH::BoundedObject objects + * and pass it to the build method. BVH::BoundedObject is a template class, so you can save user-defined data + * in it. + * + * The tree is built such that each leaf contains exactly one object. */ + template + class PCL_EXPORTS BVH + { + public: + class BoundedObject + { + public: + BoundedObject (const UserData& data) + : data_ (data) + { + } + + virtual ~BoundedObject () + { + } + + /** \brief This method is for std::sort. */ + inline static bool + compareCentroidsXCoordinates (const BoundedObject* a, const BoundedObject* b) + { + return a->getCentroid ()[0] < b->getCentroid ()[0]; + } + + float* + getBounds () + { + return (bounds_); + } + + float* + getCentroid () + { + return (centroid_); + } + + const float* + getCentroid () const + { + return (centroid_); + } + + UserData& + getData () + { + return (data_); + } + + protected: + /** These are the bounds of the object.*/ + float bounds_[6]; + /** This is the centroid. */ + float centroid_[3]; + /** This is the user-defined data object. */ + UserData data_; + }; + + protected: + class Node + { + public: + /** \brief 'sorted_objects' is a sorted vector of bounded objects. It has to be sorted in ascending order according + * to the objects' x-coordinates. The constructor recursively calls itself with the right 'first_id' and 'last_id' + * and with the same vector 'sorted_objects'. */ + Node (std::vector& sorted_objects, int first_id, int last_id) + { + // Initialize the bounds of the node + memcpy (bounds_, sorted_objects[first_id]->getBounds (), 6*sizeof (float)); + + // Expand the bounds of the node + for ( int i = first_id + 1 ; i <= last_id ; ++i ) + aux::expandBoundingBox(bounds_, sorted_objects[i]->getBounds()); + + // Shall we create children? + if ( first_id != last_id ) + { + // Division by 2 + int mid_id = (first_id + last_id) >> 1; + children_[0] = new Node(sorted_objects, first_id, mid_id); + children_[1] = new Node(sorted_objects, mid_id + 1, last_id); + } + else + { + // We reached a leaf + object_ = sorted_objects[first_id]; + children_[0] = children_[1] = nullptr; + } + } + + virtual ~Node () + { + if ( children_[0] ) + { + delete children_[0]; + delete children_[1]; + } + } + + bool + hasChildren () const + { + return static_cast(children_[0]); + } + + Node* + getLeftChild () + { + return children_[0]; + } + + Node* + getRightChild () + { + return children_[1]; + } + + BoundedObject* + getObject () + { + return object_; + } + + bool + isLeaf () const + { + return !static_cast(children_[0]); + } + + /** \brief Returns true if 'box' intersects or touches (with a side or a vertex) this node. */ + inline bool + intersect(const float box[6]) const + { + return !(box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] || + box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5]); + } + + /** \brief Computes and returns the volume of the bounding box of this node. */ + double + computeBoundingBoxVolume() const + { + return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]); + } + + friend class BVH; + + protected: + float bounds_[6]; + Node* children_[2]; + BoundedObject* object_; + }; + + public: + BVH() + : root_ (nullptr), + sorted_objects_ (nullptr) + { + } + + virtual ~BVH() + { + this->clear (); + } + + /** \brief Creates the tree. No need to call clear, it's called within the method. 'objects' is a vector of + * pointers to bounded objects which have to have valid bounds and centroids. Use the getData method of + * BoundedObject to retrieve the user-defined data saved in the object. Note that vector will be sorted within + * the method! + * + * The tree is built such that each leaf contains exactly one object. */ + void + build(std::vector& objects) + { + this->clear(); + + if ( objects.empty () ) + return; + + sorted_objects_ = &objects; + + // Now sort the objects according to the x-coordinates of their centroids + std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates); + + // Create the root -> it recursively creates the children nodes until each leaf contains exactly one object + root_ = new Node (objects, 0, static_cast (objects.size () - 1)); + } + + /** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */ + void + clear() + { + if ( root_ ) + { + delete root_; + root_ = nullptr; + } + } + + inline const std::vector* + getInputObjects () const + { + return (sorted_objects_); + } + + /** \brief Pushes back in 'intersected_objects' the bounded objects intersected by the input 'box' and returns true. + * Returns false if no objects are intersected. */ + inline bool + intersect(const float box[6], std::list& intersected_objects) const + { + if ( !root_ ) + return false; + + bool got_intersection = false; + + // Start the intersection process at the root + std::list working_list; + working_list.push_back (root_); + + while ( !working_list.empty () ) + { + Node* node = working_list.front (); + working_list.pop_front (); + + // Is 'node' intersected by the box? + if ( node->intersect (box) ) + { + // We have to check the children of the intersected 'node' + if ( node->hasChildren () ) + { + working_list.push_back (node->getLeftChild ()); + working_list.push_back (node->getRightChild ()); + } + else // 'node' is a leaf -> save it's object in the output list + { + intersected_objects.push_back (node->getObject ()); + got_intersection = true; + } + } + } + + return (got_intersection); + } + + protected: + Node* root_; + std::vector* sorted_objects_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/hypothesis.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/hypothesis.h new file mode 100755 index 0000000..e4dc596 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/hypothesis.h @@ -0,0 +1,157 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * hypothesis.h + * + * Created on: Mar 12, 2013 + * Author: papazov + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace recognition + { + class HypothesisBase + { + public: + HypothesisBase (const ModelLibrary::Model* obj_model) + : obj_model_ (obj_model) + {} + + HypothesisBase (const ModelLibrary::Model* obj_model, const float* rigid_transform) + : obj_model_ (obj_model) + { + memcpy (rigid_transform_, rigid_transform, 12*sizeof (float)); + } + + virtual ~HypothesisBase (){} + + void + setModel (const ModelLibrary::Model* model) + { + obj_model_ = model; + } + + public: + float rigid_transform_[12]; + const ModelLibrary::Model* obj_model_; + }; + + class Hypothesis: public HypothesisBase + { + public: + Hypothesis (const ModelLibrary::Model* obj_model = nullptr) + : HypothesisBase (obj_model), + match_confidence_ (-1.0f), + linear_id_ (-1) + { + } + + Hypothesis (const Hypothesis& src) + : HypothesisBase (src.obj_model_, src.rigid_transform_), + match_confidence_ (src.match_confidence_), + explained_pixels_ (src.explained_pixels_) + { + } + + ~Hypothesis (){} + + const Hypothesis& + operator =(const Hypothesis& src) + { + memcpy (this->rigid_transform_, src.rigid_transform_, 12*sizeof (float)); + this->obj_model_ = src.obj_model_; + this->match_confidence_ = src.match_confidence_; + this->explained_pixels_ = src.explained_pixels_; + + return *this; + } + + void + setLinearId (int id) + { + linear_id_ = id; + } + + int + getLinearId () const + { + return (linear_id_); + } + + void + computeBounds (float bounds[6]) const + { + const float *b = obj_model_->getBoundsOfOctreePoints (); + float p[3]; + + // Initialize 'bounds' + aux::transform (rigid_transform_, b[0], b[2], b[4], p); + bounds[0] = bounds[1] = p[0]; + bounds[2] = bounds[3] = p[1]; + bounds[4] = bounds[5] = p[2]; + + // Expand 'bounds' to contain the other 7 points of the octree bounding box + aux::transform (rigid_transform_, b[0], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[0], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[0], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[2], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[2], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[3], b[4], p); aux::expandBoundingBoxToContainPoint (bounds, p); + aux::transform (rigid_transform_, b[1], b[3], b[5], p); aux::expandBoundingBoxToContainPoint (bounds, p); + } + + void + computeCenterOfMass (float center_of_mass[3]) const + { + aux::transform (rigid_transform_, obj_model_->getOctreeCenterOfMass (), center_of_mass); + } + + public: + float match_confidence_; + std::set explained_pixels_; + int linear_id_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/model_library.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/model_library.h new file mode 100755 index 0000000..3cb7456 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/model_library.h @@ -0,0 +1,271 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include "auxiliary.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + class PCL_EXPORTS ModelLibrary + { + public: + using PointCloudIn = pcl::PointCloud; + using PointCloudN = pcl::PointCloud; + + /** \brief Stores some information about the model. */ + class Model + { + public: + Model (const PointCloudIn& points, const PointCloudN& normals, float voxel_size, const std::string& object_name, + float frac_of_points_for_registration, void* user_data = nullptr) + : obj_name_(object_name), + user_data_ (user_data) + { + octree_.build (points, voxel_size, &normals); + + const std::vector& full_leaves = octree_.getFullLeaves (); + if ( full_leaves.empty () ) + return; + + // Initialize + std::vector::const_iterator it = full_leaves.begin (); + const float *p = (*it)->getData ()->getPoint (); + aux::copy3 (p, octree_center_of_mass_); + bounds_of_octree_points_[0] = bounds_of_octree_points_[1] = p[0]; + bounds_of_octree_points_[2] = bounds_of_octree_points_[3] = p[1]; + bounds_of_octree_points_[4] = bounds_of_octree_points_[5] = p[2]; + + // Compute both the bounds and the center of mass of the octree points + for ( ++it ; it != full_leaves.end () ; ++it ) + { + aux::add3 (octree_center_of_mass_, (*it)->getData ()->getPoint ()); + aux::expandBoundingBoxToContainPoint (bounds_of_octree_points_, (*it)->getData ()->getPoint ()); + } + + int num_octree_points = static_cast (full_leaves.size ()); + // Finalize the center of mass computation + aux::mult3 (octree_center_of_mass_, 1.0f/static_cast (num_octree_points)); + + int num_points_for_registration = static_cast (static_cast (num_octree_points)*frac_of_points_for_registration); + points_for_registration_.resize (static_cast (num_points_for_registration)); + + // Prepare for random point sampling + std::vector ids (num_octree_points); + for ( int i = 0 ; i < num_octree_points ; ++i ) + ids[i] = i; + + // The random generator + pcl::common::UniformGenerator randgen (0, num_octree_points - 1, static_cast (time (nullptr))); + + // Randomly sample some points from the octree + for ( int i = 0 ; i < num_points_for_registration ; ++i ) + { + // Choose a random position within the array of ids + randgen.setParameters (0, static_cast (ids.size ()) - 1); + int rand_pos = randgen.run (); + + // Copy the randomly selected octree point + aux::copy3 (octree_.getFullLeaves ()[ids[rand_pos]]->getData ()->getPoint (), points_for_registration_[i]); + + // Delete the selected id + ids.erase (ids.begin() + rand_pos); + } + } + + virtual ~Model () + { + } + + inline const std::string& + getObjectName () const + { + return (obj_name_); + } + + inline const ORROctree& + getOctree () const + { + return (octree_); + } + + inline void* + getUserData () const + { + return (user_data_); + } + + inline const float* + getOctreeCenterOfMass () const + { + return (octree_center_of_mass_); + } + + inline const float* + getBoundsOfOctreePoints () const + { + return (bounds_of_octree_points_); + } + + inline const PointCloudIn& + getPointsForRegistration () const + { + return (points_for_registration_); + } + + protected: + const std::string obj_name_; + ORROctree octree_; + float octree_center_of_mass_[3]; + float bounds_of_octree_points_[6]; + PointCloudIn points_for_registration_; + void* user_data_; + }; + + using node_data_pair_list = std::list >; + using HashTableCell = std::map; + using HashTable = VoxelStructure; + + public: + /** \brief This class is used by 'ObjRecRANSAC' to maintain the object models to be recognized. Normally, you do not need to use + * this class directly. */ + ModelLibrary (float pair_width, float voxel_size, float max_coplanarity_angle = 3.0f*AUX_DEG_TO_RADIANS/*3 degrees*/); + virtual ~ModelLibrary () + { + this->clear(); + } + + /** \brief Removes all models from the library and clears the hash table. */ + void + removeAllModels (); + + /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will + * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if + * "ignore co-planar points" is on. Call this method before calling addModel. */ + inline void + setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees) + { + max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS; + } + + /** \brief Call this method in order NOT to add co-planar point pairs to the hash table. The default behavior + * is ignoring co-planar points on. */ + inline void + ignoreCoplanarPointPairsOn () + { + ignore_coplanar_opps_ = true; + } + + /** \brief Call this method in order to add all point pairs (co-planar as well) to the hash table. The default + * behavior is ignoring co-planar points on. */ + inline void + ignoreCoplanarPointPairsOff () + { + ignore_coplanar_opps_ = false; + } + + /** \brief Adds a model to the hash table. + * + * \param[in] points represents the model to be added. + * \param[in] normals are the normals at the model points. + * \param[in] object_name is the unique name of the object to be added. + * \param[in] frac_of_points_for_registration is the number of points used for fast ICP registration prior to hypothesis testing + * \param[in] user_data is a pointer to some data (can be NULL) + * + * Returns true if model successfully added and false otherwise (e.g., if object_name is not unique). */ + bool + addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, + float frac_of_points_for_registration, void* user_data = nullptr); + + /** \brief Returns the hash table built by this instance. */ + inline const HashTable& + getHashTable () const + { + return (hash_table_); + } + + inline const Model* + getModel (const std::string& name) const + { + std::map::const_iterator it = models_.find (name); + if ( it != models_.end () ) + return (it->second); + + return (nullptr); + } + + inline const std::map& + getModels () const + { + return (models_); + } + + protected: + /** \brief Removes all models from the library and destroys the hash table. This method should be called upon destroying this object. */ + void + clear (); + + /** \brief Returns true if the oriented point pair was added to the hash table and false otherwise. */ + bool + addToHashTable (Model* model, const ORROctree::Node::Data* data1, const ORROctree::Node::Data* data2); + + protected: + float pair_width_; + float voxel_size_; + float max_coplanarity_angle_; + bool ignore_coplanar_opps_; + + std::map models_; + HashTable hash_table_; + int num_of_cells_[3]; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/obj_rec_ransac.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/obj_rec_ransac.h new file mode 100755 index 0000000..2a28c8d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/obj_rec_ransac.h @@ -0,0 +1,481 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define OBJ_REC_RANSAC_VERBOSE + +namespace pcl +{ + namespace recognition + { + /** \brief This is a RANSAC-based 3D object recognition method. Do the following to use it: (i) call addModel() k times with k different models + * representing the objects to be recognized and (ii) call recognize() with the 3D scene in which the objects should be recognized. Recognition means both + * object identification and pose (position + orientation) estimation. Check the method descriptions for more details. + * + * \note If you use this code in any academic work, please cite: + * + * - Chavdar Papazov, Sami Haddadin, Sven Parusel, Kai Krieger and Darius Burschka. + * Rigid 3D geometry matching for grasping of known objects in cluttered scenes. + * The International Journal of Robotics Research 2012. DOI: 10.1177/0278364911436019 + * + * - Chavdar Papazov and Darius Burschka. + * An Efficient RANSAC for 3D Object Recognition in Noisy and Occluded Scenes. + * In Proceedings of the 10th Asian Conference on Computer Vision (ACCV'10), + * November 2010. + * + * + * \author Chavdar Papazov + * \ingroup recognition + */ + class PCL_EXPORTS ObjRecRANSAC + { + public: + using PointCloudIn = ModelLibrary::PointCloudIn; + using PointCloudN = ModelLibrary::PointCloudN; + + using BVHH = BVH; + + /** \brief This is an output item of the ObjRecRANSAC::recognize() method. It contains the recognized model, its name (the ones passed to + * ObjRecRANSAC::addModel()), the rigid transform which aligns the model with the input scene and the match confidence which is a number + * in the interval (0, 1] which gives the fraction of the model surface area matched to the scene. E.g., a match confidence of 0.3 means + * that 30% of the object surface area was matched to the scene points. If the scene is represented by a single range image, the match + * confidence can not be greater than 0.5 since the range scanner sees only one side of each object. + */ + class Output + { + public: + Output (const std::string& object_name, const float rigid_transform[12], float match_confidence, void* user_data) : + object_name_ (object_name), + match_confidence_ (match_confidence), + user_data_ (user_data) + { + memcpy(this->rigid_transform_, rigid_transform, 12*sizeof (float)); + } + virtual ~Output (){} + + public: + std::string object_name_; + float rigid_transform_[12]; + float match_confidence_; + void* user_data_; + }; + + class OrientedPointPair + { + public: + OrientedPointPair (const float *p1, const float *n1, const float *p2, const float *n2) + : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2) + { + } + + virtual ~OrientedPointPair (){} + + public: + const float *p1_, *n1_, *p2_, *n2_; + }; + + class HypothesisCreator + { + public: + HypothesisCreator (){} + virtual ~HypothesisCreator (){} + + Hypothesis* create (const SimpleOctree::Node* ) const { return new Hypothesis ();} + }; + + using HypothesisOctree = SimpleOctree; + + public: + /** \brief Constructor with some important parameters which can not be changed once an instance of that class is created. + * + * \param[in] pair_width should be roughly half the extent of the visible object part. This means, for each object point p there should be (at least) + * one point q (from the same object) such that ||p - q|| <= pair_width. Tradeoff: smaller values allow for detection in more occluded scenes but lead + * to more imprecise alignment. Bigger values lead to better alignment but require large visible object parts (i.e., less occlusion). + * + * \param[in] voxel_size is the size of the leafs of the octree, i.e., the "size" of the discretization. Tradeoff: High values lead to less + * computation time but ignore object details. Small values allow to better distinguish between objects, but will introduce more holes in the resulting + * "voxel-surface" (especially for a sparsely sampled scene). */ + ObjRecRANSAC (float pair_width, float voxel_size); + virtual ~ObjRecRANSAC () + { + this->clear (); + this->clearTestData (); + } + + /** \brief Removes all models from the model library and releases some memory dynamically allocated by this instance. */ + void + inline clear() + { + model_library_.removeAllModels (); + scene_octree_.clear (); + scene_octree_proj_.clear (); + sampled_oriented_point_pairs_.clear (); + transform_space_.clear (); + scene_octree_points_.reset (); + } + + /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will + * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if + * "ignore co-planar points" is on. Call this method before calling addModel. This method calls the corresponding + * method of the model library. */ + inline void + setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees) + { + max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS; + model_library_.setMaxCoplanarityAngleDegrees (max_coplanarity_angle_degrees); + } + + inline void + setSceneBoundsEnlargementFactor (float value) + { + scene_bounds_enlargement_factor_ = value; + } + + /** \brief Default is on. This method calls the corresponding method of the model library. */ + inline void + ignoreCoplanarPointPairsOn () + { + ignore_coplanar_opps_ = true; + model_library_.ignoreCoplanarPointPairsOn (); + } + + /** \brief Default is on. This method calls the corresponding method of the model library. */ + inline void + ignoreCoplanarPointPairsOff () + { + ignore_coplanar_opps_ = false; + model_library_.ignoreCoplanarPointPairsOff (); + } + + inline void + icpHypothesesRefinementOn () + { + do_icp_hypotheses_refinement_ = true; + } + + inline void + icpHypothesesRefinementOff () + { + do_icp_hypotheses_refinement_ = false; + } + + /** \brief Add an object model to be recognized. + * + * \param[in] points are the object points. + * \param[in] normals at each point. + * \param[in] object_name is an identifier for the object. If that object is detected in the scene 'object_name' + * is returned by the recognition method and you know which object has been detected. Note that 'object_name' has + * to be unique! + * \param[in] user_data is a pointer to some data (can be NULL) + * + * The method returns true if the model was successfully added to the model library and false otherwise (e.g., if 'object_name' is already in use). + */ + inline bool + addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, void* user_data = nullptr) + { + return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data)); + } + + /** \brief This method performs the recognition of the models loaded to the model library with the method addModel(). + * + * \param[in] scene is the 3d scene in which the object should be recognized. + * \param[in] normals are the scene normals. + * \param[out] recognized_objects is the list of output items each one containing the recognized model instance, its name, the aligning rigid transform + * and the match confidence (see ObjRecRANSAC::Output for further explanations). + * \param[in] success_probability is the user-defined probability of detecting all objects in the scene. + */ + void + recognize (const PointCloudIn& scene, const PointCloudN& normals, std::list& recognized_objects, double success_probability = 0.99); + + inline void + enterTestModeSampleOPP () + { + rec_mode_ = ObjRecRANSAC::SAMPLE_OPP; + } + + inline void + enterTestModeTestHypotheses () + { + rec_mode_ = ObjRecRANSAC::TEST_HYPOTHESES; + } + + inline void + leaveTestMode () + { + rec_mode_ = ObjRecRANSAC::FULL_RECOGNITION; + } + + /** \brief This function is useful for testing purposes. It returns the oriented point pairs which were sampled from the + * scene during the recognition process. Makes sense only if some of the testing modes are active. */ + inline const std::list& + getSampledOrientedPointPairs () const + { + return (sampled_oriented_point_pairs_); + } + + /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the + * recognition process. Makes sense only if some of the testing modes are active. */ + inline const std::vector& + getAcceptedHypotheses () const + { + return (accepted_hypotheses_); + } + + /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the + * recognition process. Makes sense only if some of the testing modes are active. */ + inline void + getAcceptedHypotheses (std::vector& out) const + { + out = accepted_hypotheses_; + } + + /** \brief Returns the hash table in the model library. */ + inline const pcl::recognition::ModelLibrary::HashTable& + getHashTable () const + { + return (model_library_.getHashTable ()); + } + + inline const ModelLibrary& + getModelLibrary () const + { + return (model_library_); + } + + inline const ModelLibrary::Model* + getModel (const std::string& name) const + { + return (model_library_.getModel (name)); + } + + inline const ORROctree& + getSceneOctree () const + { + return (scene_octree_); + } + + inline RigidTransformSpace& + getRigidTransformSpace () + { + return (transform_space_); + } + + inline float + getPairWidth () const + { + return pair_width_; + } + + protected: + enum Recognition_Mode {SAMPLE_OPP, TEST_HYPOTHESES, /*BUILD_CONFLICT_GRAPH,*/ FULL_RECOGNITION}; + + friend class ModelLibrary; + + inline int + computeNumberOfIterations (double success_probability) const + { + // 'p_obj' is the probability that given that the first sample point belongs to an object, + // the second sample point will belong to the same object + const double p_obj = 0.25f; + // old version: p = p_obj*relative_obj_size_*fraction_of_pairs_in_hash_table_; + const double p = p_obj*relative_obj_size_; + + if ( 1.0 - p <= 0.0 ) + return 1; + + return static_cast (std::log (1.0-success_probability)/std::log (1.0-p) + 1.0); + } + + inline void + clearTestData () + { + sampled_oriented_point_pairs_.clear (); + accepted_hypotheses_.clear (); + transform_space_.clear (); + } + + void + sampleOrientedPointPairs (int num_iterations, const std::vector& full_scene_leaves, std::list& output) const; + + int + generateHypotheses (const std::list& pairs, std::list& out) const; + + /** \brief Groups close hypotheses in 'hypotheses'. Saves a representative for each group in 'out'. Returns the + * number of hypotheses after grouping. */ + int + groupHypotheses(std::list& hypotheses, int num_hypotheses, RigidTransformSpace& transform_space, + HypothesisOctree& grouped_hypotheses) const; + + inline void + testHypothesis (Hypothesis* hypothesis, int& match, int& penalty) const; + + inline void + testHypothesisNormalBased (Hypothesis* hypothesis, float& match) const; + + void + buildGraphOfCloseHypotheses (HypothesisOctree& hypotheses, ORRGraph& graph) const; + + void + filterGraphOfCloseHypotheses (ORRGraph& graph, std::vector& out) const; + + void + buildGraphOfConflictingHypotheses (const BVHH& bvh, ORRGraph& graph) const; + + void + filterGraphOfConflictingHypotheses (ORRGraph& graph, std::list& recognized_objects) const; + + /** \brief Computes the rigid transform that maps the line (a1, b1) to (a2, b2). + * The computation is based on the corresponding points 'a1' <-> 'a2' and 'b1' <-> 'b2' + * and the normals 'a1_n', 'b1_n', 'a2_n', and 'b2_n'. The result is saved in + * 'rigid_transform' which is an array of length 12. The first 9 elements are the + * rotational part (row major order) and the last 3 are the translation. */ + inline void + computeRigidTransform( + const float *a1, const float *a1_n, const float *b1, const float* b1_n, + const float *a2, const float *a2_n, const float *b2, const float* b2_n, + float* rigid_transform) const + { + // Some local variables + float o1[3], o2[3], x1[3], x2[3], y1[3], y2[3], z1[3], z2[3], tmp1[3], tmp2[3], Ro1[3], invFrame1[3][3]; + + // Compute the origins + o1[0] = 0.5f*(a1[0] + b1[0]); + o1[1] = 0.5f*(a1[1] + b1[1]); + o1[2] = 0.5f*(a1[2] + b1[2]); + + o2[0] = 0.5f*(a2[0] + b2[0]); + o2[1] = 0.5f*(a2[1] + b2[1]); + o2[2] = 0.5f*(a2[2] + b2[2]); + + // Compute the x-axes + aux::diff3 (b1, a1, x1); aux::normalize3 (x1); + aux::diff3 (b2, a2, x2); aux::normalize3 (x2); + // Compute the y-axes. First y-axis + aux::projectOnPlane3 (a1_n, x1, tmp1); aux::normalize3 (tmp1); + aux::projectOnPlane3 (b1_n, x1, tmp2); aux::normalize3 (tmp2); + aux::sum3 (tmp1, tmp2, y1); aux::normalize3 (y1); + // Second y-axis + aux::projectOnPlane3 (a2_n, x2, tmp1); aux::normalize3 (tmp1); + aux::projectOnPlane3 (b2_n, x2, tmp2); aux::normalize3 (tmp2); + aux::sum3 (tmp1, tmp2, y2); aux::normalize3 (y2); + // Compute the z-axes + aux::cross3 (x1, y1, z1); + aux::cross3 (x2, y2, z2); + + // 1. Invert the matrix [x1|y1|z1] (note that x1, y1, and z1 are treated as columns!) + invFrame1[0][0] = x1[0]; invFrame1[0][1] = x1[1]; invFrame1[0][2] = x1[2]; + invFrame1[1][0] = y1[0]; invFrame1[1][1] = y1[1]; invFrame1[1][2] = y1[2]; + invFrame1[2][0] = z1[0]; invFrame1[2][1] = z1[1]; invFrame1[2][2] = z1[2]; + // 2. Compute the desired rotation as rigid_transform = [x2|y2|z2]*invFrame1 + aux::mult3x3 (x2, y2, z2, invFrame1, rigid_transform); + + // Construct the translation which is the difference between the rotated o1 and o2 + aux::mult3x3 (rigid_transform, o1, Ro1); + rigid_transform[9] = o2[0] - Ro1[0]; + rigid_transform[10] = o2[1] - Ro1[1]; + rigid_transform[11] = o2[2] - Ro1[2]; + } + + /** \brief Computes the signature of the oriented point pair ((p1, n1), (p2, n2)) consisting of the angles between + * \param p1 + * \param n1 + * \param p2 + * \param n2 + * \param[out] signature is an array of three doubles saving the three angles in the order shown above. */ + static inline void + compute_oriented_point_pair_signature (const float *p1, const float *n1, const float *p2, const float *n2, float signature[3]) + { + // Get the line from p1 to p2 + float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]}; + aux::normalize3 (cl); + + signature[0] = std::acos (aux::clamp (aux::dot3 (n1,cl), -1.0f, 1.0f)); cl[0] = -cl[0]; cl[1] = -cl[1]; cl[2] = -cl[2]; + signature[1] = std::acos (aux::clamp (aux::dot3 (n2,cl), -1.0f, 1.0f)); + signature[2] = std::acos (aux::clamp (aux::dot3 (n1,n2), -1.0f, 1.0f)); + } + + protected: + // Parameters + float pair_width_; + float voxel_size_; + float position_discretization_; + float rotation_discretization_; + float abs_zdist_thresh_; + float relative_obj_size_; + float visibility_; + float relative_num_of_illegal_pts_; + float intersection_fraction_; + float max_coplanarity_angle_; + float scene_bounds_enlargement_factor_; + bool ignore_coplanar_opps_; + float frac_of_points_for_icp_refinement_; + bool do_icp_hypotheses_refinement_; + + ModelLibrary model_library_; + ORROctree scene_octree_; + ORROctreeZProjection scene_octree_proj_; + RigidTransformSpace transform_space_; + TrimmedICP trimmed_icp_; + PointCloudIn::Ptr scene_octree_points_; + + std::list sampled_oriented_point_pairs_; + std::vector accepted_hypotheses_; + Recognition_Mode rec_mode_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_graph.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_graph.h new file mode 100755 index 0000000..cf118ba --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_graph.h @@ -0,0 +1,226 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * orr_graph.h + * + * Created on: Nov 23, 2012 + * Author: papazov + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class ORRGraph + { + public: + class Node + { + public: + enum State {ON, OFF, UNDEF}; + + Node (int id) + : id_ (id), + state_(UNDEF) + {} + + virtual ~Node (){} + + inline const std::set& + getNeighbors () const + { + return (neighbors_); + } + + inline const NodeData& + getData () const + { + return (data_); + } + + inline void + setData (const NodeData& data) + { + data_ = data; + } + + inline int + getId () const + { + return (id_); + } + + inline void + setId (int id) + { + id_ = id; + } + + inline void + setFitness (int fitness) + { + fitness_ = fitness; + } + + static inline bool + compare (const Node* a, const Node* b) + { + return a->fitness_ > b->fitness_; + } + + friend class ORRGraph; + + protected: + std::set neighbors_; + NodeData data_; + int id_; + int fitness_; + State state_; + }; + + public: + ORRGraph (){} + virtual ~ORRGraph (){ this->clear ();} + + inline void + clear () + { + for ( typename std::vector::iterator nit = nodes_.begin () ; nit != nodes_.end () ; ++nit ) + delete *nit; + + nodes_.clear (); + } + + /** \brief Drops all existing graph nodes and creates 'n' new ones. */ + inline void + resize (int n) + { + if ( !n ) + return; + + for ( typename std::vector::iterator nit = nodes_.begin () ; nit != nodes_.end () ; ++nit ) + delete *nit; + + nodes_.resize (static_cast (n)); + + for ( int i = 0 ; i < n ; ++i ) + nodes_[i] = new Node (i); + } + + inline void + computeMaximalOnOffPartition (std::list& on_nodes, std::list& off_nodes) + { + std::vector sorted_nodes (nodes_.size ()); + int i = 0; + + // Set all nodes to undefined + for ( typename std::vector::iterator it = nodes_.begin () ; it != nodes_.end () ; ++it ) + { + sorted_nodes[i++] = *it; + (*it)->state_ = Node::UNDEF; + } + + // Now sort the nodes according to the fitness + std::sort (sorted_nodes.begin (), sorted_nodes.end (), Node::compare); + + // Now run through the array and start switching nodes on and off + for ( typename std::vector::iterator it = sorted_nodes.begin () ; it != sorted_nodes.end () ; ++it ) + { + // Ignore graph nodes which are already OFF + if ( (*it)->state_ == Node::OFF ) + continue; + + // Set the node to ON + (*it)->state_ = Node::ON; + + // Set all its neighbors to OFF + for ( typename std::set::iterator neigh = (*it)->neighbors_.begin () ; neigh != (*it)->neighbors_.end () ; ++neigh ) + { + (*neigh)->state_ = Node::OFF; + off_nodes.push_back (*neigh); + } + + // Output the node + on_nodes.push_back (*it); + } + } + + inline void + insertUndirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.insert (nodes_[id2]); + nodes_[id2]->neighbors_.insert (nodes_[id1]); + } + + inline void + insertDirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.insert (nodes_[id2]); + } + + inline void + deleteUndirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.erase (nodes_[id2]); + nodes_[id2]->neighbors_.erase (nodes_[id1]); + } + + inline void + deleteDirectedEdge (int id1, int id2) + { + nodes_[id1]->neighbors_.erase (nodes_[id2]); + } + + inline typename std::vector& + getNodes (){ return nodes_;} + + public: + typename std::vector nodes_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_octree.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_octree.h new file mode 100755 index 0000000..0655064 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_octree.h @@ -0,0 +1,489 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * orr_octree.h + * + * Created on: Oct 23, 2012 + * Author: papazov + */ + +#pragma once + +#include "auxiliary.h" +#include +#include +#include +#include +#include +#include +#include +#include + +//#define PCL_REC_ORR_OCTREE_VERBOSE + +namespace pcl +{ + namespace recognition + { + /** \brief That's a very specialized and simple octree class. That's the way it is intended to + * be, that's why no templates and stuff like this. + * + * \author Chavdar Papazov + * \ingroup recognition + */ + class PCL_EXPORTS ORROctree + { + public: + using PointCloudIn = pcl::PointCloud; + using PointCloudOut = pcl::PointCloud; + using PointCloudN = pcl::PointCloud; + + class Node + { + public: + class Data + { + public: + Data (int id_x, int id_y, int id_z, int lin_id, void* user_data = nullptr) + : id_x_ (id_x), + id_y_ (id_y), + id_z_ (id_z), + lin_id_ (lin_id), + num_points_ (0), + user_data_ (user_data) + { + n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f; + } + + virtual~ Data (){} + + inline void + addToPoint (float x, float y, float z) + { + p_[0] += x; p_[1] += y; p_[2] += z; + ++num_points_; + } + + inline void + computeAveragePoint () + { + if ( num_points_ < 2 ) + return; + + aux::mult3 (p_, 1.0f/static_cast (num_points_)); + num_points_ = 1; + } + + inline void + addToNormal (float x, float y, float z) { n_[0] += x; n_[1] += y; n_[2] += z;} + + inline const float* + getPoint () const { return p_;} + + inline float* + getPoint (){ return p_;} + + inline const float* + getNormal () const { return n_;} + + inline float* + getNormal (){ return n_;} + + inline void + get3dId (int id[3]) const + { + id[0] = id_x_; + id[1] = id_y_; + id[2] = id_z_; + } + + inline int + get3dIdX () const {return id_x_;} + + inline int + get3dIdY () const {return id_y_;} + + inline int + get3dIdZ () const {return id_z_;} + + inline int + getLinearId () const { return lin_id_;} + + inline void + setUserData (void* user_data){ user_data_ = user_data;} + + inline void* + getUserData () const { return user_data_;} + + inline void + insertNeighbor (Node* node){ neighbors_.insert (node);} + + inline const std::set& + getNeighbors () const { return (neighbors_);} + + protected: + float n_[3], p_[3]; + int id_x_, id_y_, id_z_, lin_id_, num_points_; + std::set neighbors_; + void *user_data_; + }; + + Node () + : data_ (nullptr), + parent_ (nullptr), + children_(nullptr) + {} + + virtual~ Node () + { + this->deleteChildren (); + this->deleteData (); + } + + inline void + setCenter(const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];} + + inline void + setBounds(const float *b) { bounds_[0] = b[0]; bounds_[1] = b[1]; bounds_[2] = b[2]; bounds_[3] = b[3]; bounds_[4] = b[4]; bounds_[5] = b[5];} + + inline void + setParent(Node* parent) { parent_ = parent;} + + inline void + setData(Node::Data* data) { data_ = data;} + + /** \brief Computes the "radius" of the node which is half the diagonal length. */ + inline void + computeRadius() + { + float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])}; + radius_ = static_cast (aux::length3 (v)); + } + + inline const float* + getCenter() const { return center_;} + + inline const float* + getBounds() const { return bounds_;} + + inline void + getBounds(float b[6]) const + { + memcpy (b, bounds_, 6*sizeof (float)); + } + + inline Node* + getChild (int id) { return &children_[id];} + + inline Node* + getChildren () { return children_;} + + inline Node::Data* + getData (){ return data_;} + + inline const Node::Data* + getData () const { return data_;} + + inline void + setUserData (void* user_data){ data_->setUserData (user_data);} + + inline Node* + getParent (){ return parent_;} + + inline bool + hasData (){ return static_cast (data_);} + + inline bool + hasChildren (){ return static_cast (children_);} + + /** \brief Computes the "radius" of the node which is half the diagonal length. */ + inline float + getRadius (){ return radius_;} + + bool + createChildren (); + + inline void + deleteChildren () + { + if ( children_ ) + { + delete[] children_; + children_ = nullptr; + } + } + + inline void + deleteData () + { + if ( data_ ) + { + delete data_; + data_ = nullptr; + } + } + + /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens + * of either of the nodes has no data. */ + inline void + makeNeighbors (Node* node) + { + if ( !this->getData () || !node->getData () ) + return; + + this->getData ()->insertNeighbor (node); + node->getData ()->insertNeighbor (this); + } + + protected: + Node::Data *data_; + float center_[3], bounds_[6], radius_; + Node *parent_, *children_; + }; + + ORROctree (); + virtual ~ORROctree (){ this->clear ();} + + void + clear (); + + /** \brief Creates an octree which encloses 'points' and with leaf size equal to 'voxel_size'. + * 'enlarge_bounds' makes sure that no points from the input will lie on the octree boundary + * by enlarging the bounds by that factor. For example, enlarge_bounds = 1 means that the + * bounds will be enlarged by 100%. The default value is fine. */ + void + build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals = nullptr, float enlarge_bounds = 0.00001f); + + /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf + * size equal to 'voxel_size'. */ + void + build (const float* bounds, float voxel_size); + + /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within + * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method + * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and + * method just returns a pointer to the leaf. */ + inline ORROctree::Node* + createLeaf (float x, float y, float z) + { + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + ORROctree::Node* node = root_; + const float *c; + int id; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + node->createChildren (); + c = node->getCenter (); + id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + if ( !node->getData () ) + { + Node::Data* data = new Node::Data ( + static_cast ((node->getCenter ()[0] - bounds_[0])/voxel_size_), + static_cast ((node->getCenter ()[1] - bounds_[2])/voxel_size_), + static_cast ((node->getCenter ()[2] - bounds_[4])/voxel_size_), + static_cast (full_leaves_.size ())); + + node->setData (data); + this->insertNeighbors (node); + full_leaves_.push_back (node); + } + + return (node); + } + + /** \brief This method returns a super set of the full leavess which are intersected by the sphere + * with radius 'radius' and centered at 'p'. Pointers to the intersected full leaves are saved in + * 'out'. The method computes a super set in the sense that in general not all leaves saved in 'out' + * are really intersected by the sphere. The intersection test is based on the leaf radius (since + * its faster than checking all leaf corners and sides), so we report more leaves than we should, + * but still, this is a fair approximation. */ + void + getFullLeavesIntersectedBySphere (const float* p, float radius, std::list& out) const; + + /** \brief Randomly chooses and returns a full leaf that is intersected by the sphere with center 'p' + * and 'radius'. Returns NULL if no leaf is intersected by that sphere. */ + ORROctree::Node* + getRandomFullLeafOnSphere (const float* p, float radius) const; + + /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the leaf + * with id [i, j, k] or NULL is no such leaf exists. */ + ORROctree::Node* + getLeaf (int i, int j, int k) + { + float offset = 0.5f*voxel_size_; + float p[3] = {bounds_[0] + offset + static_cast (i)*voxel_size_, + bounds_[2] + offset + static_cast (j)*voxel_size_, + bounds_[4] + offset + static_cast (k)*voxel_size_}; + + return (this->getLeaf (p[0], p[1], p[2])); + } + + /** \brief Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists. */ + inline ORROctree::Node* + getLeaf (float x, float y, float z) + { + // Make sure that the input point is within the octree bounds + if ( x < bounds_[0] || x > bounds_[1] || + y < bounds_[2] || y > bounds_[3] || + z < bounds_[4] || z > bounds_[5] ) + { + return (nullptr); + } + + ORROctree::Node* node = root_; + const float *c; + int id; + + // Go down to the right leaf + for ( int l = 0 ; l < tree_levels_ ; ++l ) + { + if ( !node->hasChildren () ) + return (nullptr); + + c = node->getCenter (); + id = 0; + + if ( x >= c[0] ) id |= 4; + if ( y >= c[1] ) id |= 2; + if ( z >= c[2] ) id |= 1; + + node = node->getChild (id); + } + + return (node); + } + + /** \brief Deletes the branch 'node' is part of. */ + void + deleteBranch (Node* node); + + /** \brief Returns a vector with all octree leaves which contain at least one point. */ + inline std::vector& + getFullLeaves () { return full_leaves_;} + + inline const std::vector& + getFullLeaves () const { return full_leaves_;} + + void + getFullLeavesPoints (PointCloudOut& out) const; + + void + getNormalsOfFullLeaves (PointCloudN& out) const; + + inline ORROctree::Node* + getRoot (){ return root_;} + + inline const float* + getBounds () const + { + return (bounds_); + } + + inline void + getBounds (float b[6]) const + { + memcpy (b, bounds_, 6*sizeof (float)); + } + + inline float + getVoxelSize () const { return voxel_size_;} + + inline void + insertNeighbors (Node* node) + { + const float* c = node->getCenter (); + float s = 0.5f*voxel_size_; + Node *neigh; + + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + //neigh = this->getLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); + neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); + } + + protected: + float voxel_size_, bounds_[6]; + int tree_levels_; + Node* root_; + std::vector full_leaves_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_octree_zprojection.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_octree_zprojection.h new file mode 100755 index 0000000..ea36329 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/orr_octree_zprojection.h @@ -0,0 +1,211 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * orr_octree_zprojection.h + * + * Created on: Nov 17, 2012 + * Author: papazov + */ + +#pragma once + +#include "orr_octree.h" +#include +#include + +namespace pcl +{ + namespace recognition + { + class ORROctree; + + class PCL_EXPORTS ORROctreeZProjection + { + public: + class Pixel + { + public: + Pixel (int id): id_ (id) {} + + inline void + set_z1 (float z1) { z1_ = z1;} + + inline void + set_z2 (float z2) { z2_ = z2;} + + float + z1 () const { return z1_;} + + float + z2 () const { return z2_;} + + int + getId () const { return id_;} + + protected: + float z1_, z2_; + int id_; + }; + + public: + class Set + { + public: + Set (int x, int y) + : nodes_ (compare_nodes_z), x_ (x), y_ (y) + {} + + static inline bool + compare_nodes_z (ORROctree::Node* node1, ORROctree::Node* node2) + { + return node1->getData()->get3dIdZ() < node2->getData()->get3dIdZ(); + } + + inline void + insert (ORROctree::Node* leaf) { nodes_.insert(leaf);} + + inline std::set& + get_nodes (){ return nodes_;} + + inline int + get_x () const { return x_;} + + inline int + get_y () const { return y_;} + + protected: + std::set nodes_; + int x_, y_; + }; + + public: + ORROctreeZProjection () + : pixels_(nullptr), + sets_(nullptr) + {} + virtual ~ORROctreeZProjection (){ this->clear();} + + void + build (const ORROctree& input, float eps_front, float eps_back); + + void + clear (); + + inline void + getPixelCoordinates (const float* p, int& x, int& y) const + { + x = static_cast ((p[0] - bounds_[0])*inv_pixel_size_); + y = static_cast ((p[1] - bounds_[2])*inv_pixel_size_); + } + + inline const Pixel* + getPixel (const float* p) const + { + int x, y; this->getPixelCoordinates (p, x, y); + + if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); + if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); + + return (pixels_[x][y]); + } + + inline Pixel* + getPixel (const float* p) + { + int x, y; this->getPixelCoordinates (p, x, y); + + if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); + if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); + + return (pixels_[x][y]); + } + + inline const std::set* + getOctreeNodes (const float* p) const + { + int x, y; this->getPixelCoordinates (p, x, y); + + if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); + if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); + + if ( !sets_[x][y] ) + return nullptr; + + return (&sets_[x][y]->get_nodes ()); + } + + inline std::list& + getFullPixels (){ return full_pixels_;} + + inline const Pixel* + getPixel (int i, int j) const + { + return pixels_[i][j]; + } + + inline float + getPixelSize () const + { + return pixel_size_; + } + + inline const float* + getBounds () const + { + return bounds_; + } + + /** \brief Get the width ('num_x') and height ('num_y') of the image. */ + inline void + getNumberOfPixels (int& num_x, int& num_y) const + { + num_x = num_pixels_x_; + num_y = num_pixels_y_; + } + + protected: + float pixel_size_, inv_pixel_size_, bounds_[4], extent_x_, extent_y_; + int num_pixels_x_, num_pixels_y_, num_pixels_; + Pixel ***pixels_; + Set ***sets_; + std::list full_sets_; + std::list full_pixels_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/rigid_transform_space.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/rigid_transform_space.h new file mode 100755 index 0000000..7d65d3f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/rigid_transform_space.h @@ -0,0 +1,411 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * rigid_transform_space.h + * + * Created on: Feb 15, 2013 + * Author: papazov + */ + +#pragma once + +#include "simple_octree.h" +#include "model_library.h" +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + class RotationSpaceCell + { + public: + class Entry + { + public: + Entry () + : num_transforms_ (0) + { + aux::set3 (axis_angle_, 0.0f); + aux::set3 (translation_, 0.0f); + } + + Entry (const Entry& src) + : num_transforms_ (src.num_transforms_) + { + aux::copy3 (src.axis_angle_, this->axis_angle_); + aux::copy3 (src.translation_, this->translation_); + } + + const Entry& operator = (const Entry& src) + { + num_transforms_ = src.num_transforms_; + aux::copy3 (src.axis_angle_, this->axis_angle_); + aux::copy3 (src.translation_, this->translation_); + + return *this; + } + + inline const Entry& + addRigidTransform (const float axis_angle[3], const float translation[3]) + { + aux::add3 (this->axis_angle_, axis_angle); + aux::add3 (this->translation_, translation); + ++num_transforms_; + + return *this; + } + + inline void + computeAverageRigidTransform (float *rigid_transform = nullptr) + { + if ( num_transforms_ >= 2 ) + { + float factor = 1.0f/static_cast (num_transforms_); + aux::mult3 (axis_angle_, factor); + aux::mult3 (translation_, factor); + num_transforms_ = 1; + } + + if ( rigid_transform ) + { + // Save the rotation (in matrix form) + aux::axisAngleToRotationMatrix (axis_angle_, rigid_transform); + // Save the translation + aux::copy3 (translation_, rigid_transform + 9); + } + } + + inline const float* + getAxisAngle () const + { + return (axis_angle_); + } + + inline const float* + getTranslation () const + { + return (translation_); + } + + inline int + getNumberOfTransforms () const + { + return (num_transforms_); + } + + protected: + float axis_angle_[3], translation_[3]; + int num_transforms_; + };// class Entry + + public: + RotationSpaceCell (){} + virtual ~RotationSpaceCell () + { + model_to_entry_.clear (); + } + + inline std::map& + getEntries () + { + return (model_to_entry_); + } + + inline const RotationSpaceCell::Entry* + getEntry (const ModelLibrary::Model* model) const + { + std::map::const_iterator res = model_to_entry_.find (model); + + if ( res != model_to_entry_.end () ) + return (&res->second); + + return (nullptr); + } + + inline const RotationSpaceCell::Entry& + addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3]) + { + return model_to_entry_[model].addRigidTransform (axis_angle, translation); + } + + protected: + std::map model_to_entry_; + }; // class RotationSpaceCell + + class RotationSpaceCellCreator + { + public: + RotationSpaceCellCreator (){} + virtual ~RotationSpaceCellCreator (){} + + RotationSpaceCell* create (const SimpleOctree::Node* ) + { + return (new RotationSpaceCell ()); + } + }; + + using CellOctree = SimpleOctree; + + /** \brief This is a class for a discrete representation of the rotation space based on the axis-angle representation. + * This class is not supposed to be very general. That's why it is dependent on the class ModelLibrary. + * + * \author Chavdar Papazov + * \ingroup recognition + */ + class PCL_EXPORTS RotationSpace + { + public: + /** \brief We use the axis-angle representation for rotations. The axis is encoded in the vector + * and the angle is its magnitude. This is represented in an octree with bounds [-pi, pi]^3. */ + RotationSpace (float discretization) + { + float min = -(AUX_PI_FLOAT + 0.000000001f), max = AUX_PI_FLOAT + 0.000000001f; + float bounds[6] = {min, max, min, max, min, max}; + + // Build the voxel structure + octree_.build (bounds, discretization, &cell_creator_); + } + + virtual ~RotationSpace () + { + octree_.clear (); + } + + inline void + setCenter (const float* c) + { + center_[0] = c[0]; + center_[1] = c[1]; + center_[2] = c[2]; + } + + inline const float* + getCenter () const { return center_;} + + inline bool + getTransformWithMostVotes (const ModelLibrary::Model* model, float rigid_transform[12]) const + { + RotationSpaceCell::Entry with_most_votes; + const std::vector& full_leaves = octree_.getFullLeaves (); + int max_num_transforms = 0; + + // For each full leaf + for (const auto &full_leaf : full_leaves) + { + // Is there an entry for 'model' in the current cell + const RotationSpaceCell::Entry *entry = full_leaf->getData ().getEntry (model); + if ( !entry ) + continue; + + int num_transforms = entry->getNumberOfTransforms (); + const std::set& neighs = full_leaf->getNeighbors (); + + // For each neighbor + for (const auto &neigh : neighs) + { + const RotationSpaceCell::Entry *neigh_entry = neigh->getData ().getEntry (model); + if ( !neigh_entry ) + continue; + + num_transforms += neigh_entry->getNumberOfTransforms (); + } + + if ( num_transforms > max_num_transforms ) + { + with_most_votes = *entry; + max_num_transforms = num_transforms; + } + } + + if ( !max_num_transforms ) + return false; + + with_most_votes.computeAverageRigidTransform (rigid_transform); + + return true; + } + + inline bool + addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3]) + { + CellOctree::Node* cell = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]); + + if ( !cell ) + { + const float *b = octree_.getBounds (); + printf ("WARNING in 'RotationSpace::%s()': the provided axis-angle input (%f, %f, %f) is " + "out of the rotation space bounds ([%f, %f], [%f, %f], [%f, %f]).\n", + __func__, axis_angle[0], axis_angle[1], axis_angle[2], b[0], b[1], b[2], b[3], b[4], b[5]); + return (false); + } + + // Add the rigid transform to the cell + cell->getData ().addRigidTransform (model, axis_angle, translation); + + return (true); + } + + protected: + CellOctree octree_; + RotationSpaceCellCreator cell_creator_; + float center_[3]; + };// class RotationSpace + + class RotationSpaceCreator + { + public: + RotationSpaceCreator() + : counter_ (0) + {} + + virtual ~RotationSpaceCreator(){} + + RotationSpace* create(const SimpleOctree::Node* leaf) + { + RotationSpace *rot_space = new RotationSpace (discretization_); + rot_space->setCenter (leaf->getCenter ()); + rotation_spaces_.push_back (rot_space); + + ++counter_; + + return (rot_space); + } + + void + setDiscretization (float value){ discretization_ = value;} + + int + getNumberOfRotationSpaces () const { return (counter_);} + + const std::list& + getRotationSpaces () const { return (rotation_spaces_);} + + std::list& + getRotationSpaces (){ return (rotation_spaces_);} + + void + reset () + { + counter_ = 0; + rotation_spaces_.clear (); + } + + protected: + float discretization_; + int counter_; + std::list rotation_spaces_; + }; + + using RotationSpaceOctree = SimpleOctree; + + class PCL_EXPORTS RigidTransformSpace + { + public: + RigidTransformSpace (){} + virtual ~RigidTransformSpace (){ this->clear ();} + + inline void + build (const float* pos_bounds, float translation_cell_size, float rotation_cell_size) + { + this->clear (); + + rotation_space_creator_.setDiscretization (rotation_cell_size); + + pos_octree_.build (pos_bounds, translation_cell_size, &rotation_space_creator_); + } + + inline void + clear () + { + pos_octree_.clear (); + rotation_space_creator_.reset (); + } + + inline std::list& + getRotationSpaces () + { + return (rotation_space_creator_.getRotationSpaces ()); + } + + inline const std::list& + getRotationSpaces () const + { + return (rotation_space_creator_.getRotationSpaces ()); + } + + inline int + getNumberOfOccupiedRotationSpaces () + { + return (rotation_space_creator_.getNumberOfRotationSpaces ()); + } + + inline bool + addRigidTransform (const ModelLibrary::Model* model, const float position[3], const float rigid_transform[12]) + { + // Get the leaf 'position' ends up in + RotationSpaceOctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]); + + if ( !leaf ) + { + printf ("WARNING in 'RigidTransformSpace::%s()': the input position (%f, %f, %f) is out of bounds.\n", + __func__, position[0], position[1], position[2]); + return (false); + } + + float rot_angle, axis_angle[3]; + // Extract the axis-angle representation from the rotation matrix + aux::rotationMatrixToAxisAngle (rigid_transform, axis_angle, rot_angle); + // Multiply the axis by the angle to get the final representation + aux::mult3 (axis_angle, rot_angle); + + // Now, add the rigid transform to the rotation space + leaf->getData ().addRigidTransform (model, axis_angle, rigid_transform + 9); + + return (true); + } + + protected: + RotationSpaceOctree pos_octree_; + RotationSpaceCreator rotation_space_creator_; + }; // class RigidTransformSpace + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/simple_octree.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/simple_octree.h new file mode 100755 index 0000000..38d8e26 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/simple_octree.h @@ -0,0 +1,210 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * simple_octree.h + * + * Created on: Mar 11, 2013 + * Author: papazov + */ + +#pragma once + +#include + +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class PCL_EXPORTS SimpleOctree + { + public: + class Node + { + public: + Node (); + + virtual~ Node (); + + inline void + setCenter (const Scalar *c); + + inline void + setBounds (const Scalar *b); + + inline const Scalar* + getCenter () const { return center_;} + + inline const Scalar* + getBounds () const { return bounds_;} + + inline void + getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} + + inline Node* + getChild (int id) { return &children_[id];} + + inline Node* + getChildren () { return children_;} + + inline void + setData (const NodeData& src){ *data_ = src;} + + inline NodeData& + getData (){ return *data_;} + + inline const NodeData& + getData () const { return *data_;} + + inline Node* + getParent (){ return parent_;} + + inline float + getRadius () const { return radius_;} + + inline bool + hasData (){ return static_cast (data_);} + + inline bool + hasChildren (){ return static_cast (children_);} + + inline const std::set& + getNeighbors () const { return (full_leaf_neighbors_);} + + inline void + deleteChildren (); + + inline void + deleteData (); + + friend class SimpleOctree; + + protected: + void + setData (NodeData* data){ delete data_; data_ = data;} + + inline bool + createChildren (); + + /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens + * of either of the nodes has no data. */ + inline void + makeNeighbors (Node* node); + + inline void + setParent (Node* parent){ parent_ = parent;} + + /** \brief Computes the "radius" of the node which is half the diagonal length. */ + inline void + computeRadius (); + + protected: + NodeData *data_; + Scalar center_[3], bounds_[6]; + Node *parent_, *children_; + Scalar radius_; + std::set full_leaf_neighbors_; + }; + + public: + SimpleOctree (); + + virtual ~SimpleOctree (); + + void + clear (); + + /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf + * size equal to 'voxel_size'. */ + void + build (const Scalar* bounds, Scalar voxel_size, NodeDataCreator* node_data_creator); + + /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within + * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method + * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and + * method just returns a pointer to the leaf. Note that for a new created leaf, the method also creates its data + * object. */ + inline Node* + createLeaf (Scalar x, Scalar y, Scalar z); + + /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the full + * leaf, i.e., the one having a data object, with id [i, j, k] or NULL is no such leaf exists. */ + inline Node* + getFullLeaf (int i, int j, int k); + + /** \brief Returns a pointer to the full leaf, i.e., one having a data pbject, containing p = (x, y, z) or NULL if no such leaf exists. */ + inline Node* + getFullLeaf (Scalar x, Scalar y, Scalar z); + + inline std::vector& + getFullLeaves () { return full_leaves_;} + + inline const std::vector& + getFullLeaves () const { return full_leaves_;} + + inline Node* + getRoot (){ return root_;} + + inline const Scalar* + getBounds () const { return (bounds_);} + + inline void + getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} + + inline Scalar + getVoxelSize () const { return voxel_size_;} + + protected: + inline void + insertNeighbors (Node* node); + + protected: + Scalar voxel_size_, bounds_[6]; + int tree_levels_; + Node* root_; + std::vector full_leaves_; + NodeDataCreator* node_data_creator_; + }; + } // namespace recognition +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/trimmed_icp.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/trimmed_icp.h new file mode 100755 index 0000000..de41b11 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/trimmed_icp.h @@ -0,0 +1,184 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +/* + * trimmed_icp.h + * + * Created on: Mar 10, 2013 + * Author: papazov + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class PCL_EXPORTS TrimmedICP: public pcl::registration::TransformationEstimationSVD + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using Matrix4 = typename Eigen::Matrix; + + public: + TrimmedICP () + : new_to_old_energy_ratio_ (0.99f) + {} + + ~TrimmedICP () + {} + + /** \brief Call this method before calling align(). + * + * \param[in] target is target point cloud. The method builds a kd-tree based on 'target' for performing fast closest point search. + * The source point cloud will be registered to 'target' (see align() method). + * */ + inline void + init (const PointCloudConstPtr& target) + { + target_points_ = target; + kdtree_.setInputCloud (target); + } + + /** \brief The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the init() method). + * + * \param[in] source_points is the point cloud to be registered to the target. + * \param[in] num_source_points_to_use gives the number of closest source points taken into account for registration. By closest + * source points we mean the source points closest to the target. These points are computed anew at each iteration. + * \param[in,out] guess_and_result is the estimated rigid transform. IMPORTANT: this matrix is also taken as the initial guess + * for the alignment. If there is no guess, set the matrix to identity! + * */ + inline void + align (const PointCloud& source_points, int num_source_points_to_use, Matrix4& guess_and_result) const + { + int num_trimmed_source_points = num_source_points_to_use, num_source_points = static_cast (source_points.size ()); + + if ( num_trimmed_source_points >= num_source_points ) + { + printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to " + "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set " + "the number of source points to use to a lower value or use standard ICP.\n", __func__); + num_trimmed_source_points = num_source_points; + } + + // These are vectors containing source to target correspondences + pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points); + + // Some variables for the closest point search + pcl::PointXYZ transformed_source_point; + std::vector target_index (1); + std::vector sqr_dist_to_target (1); + float old_energy, energy = std::numeric_limits::max (); + +// printf ("\nalign\n"); + + do + { + // Update the correspondences + for ( int i = 0 ; i < num_source_points ; ++i ) + { + // Transform the i-th source point based on the current transform matrix + aux::transform (guess_and_result, source_points.points[i], transformed_source_point); + + // Perform the closest point search + kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target); + + // Update the i-th correspondence + full_src_to_tgt[i].index_query = i; + full_src_to_tgt[i].index_match = target_index[0]; + full_src_to_tgt[i].distance = sqr_dist_to_target[0]; + } + + // Sort in ascending order according to the squared distance + std::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences); + + old_energy = energy; + energy = 0.0f; + + // Now, setup the trimmed correspondences used for the transform estimation + for ( int i = 0 ; i < num_trimmed_source_points ; ++i ) + { + trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query; + trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match; + energy += full_src_to_tgt[i].distance; + } + + this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result); + +// printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy); + } + while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress + +// printf ("\n"); + } + + inline void + setNewToOldEnergyRatio (float ratio) + { + if ( ratio >= 1 ) + new_to_old_energy_ratio_ = 0.99f; + else + new_to_old_energy_ratio_ = ratio; + } + + protected: + static inline bool + compareCorrespondences (const pcl::Correspondence& a, const pcl::Correspondence& b) + { + return a.distance < b.distance; + } + + protected: + PointCloudConstPtr target_points_; + pcl::KdTreeFLANN kdtree_; + float new_to_old_energy_ratio_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/voxel_structure.h b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/voxel_structure.h new file mode 100755 index 0000000..026a59b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/ransac_based/voxel_structure.h @@ -0,0 +1,168 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace recognition + { + /** \brief This class is a box in R3 built of voxels ordered in a regular rectangular grid. Each voxel is of type T. */ + template + class VoxelStructure + { + public: + inline VoxelStructure (): voxels_(nullptr){} + inline virtual ~VoxelStructure (){ this->clear();} + + /** \brief Call this method before using an instance of this class. Parameter meaning is obvious. */ + inline void + build (const REAL bounds[6], int num_of_voxels[3]); + + /** \brief Release the memory allocated for the voxels. */ + inline void + clear (){ if ( voxels_ ){ delete[] voxels_; voxels_ = nullptr;}} + + /** \brief Returns a pointer to the voxel which contains p or NULL if p is not inside the structure. */ + inline T* + getVoxel (const REAL p[3]); + + /** \brief Returns a pointer to the voxel with integer id (x,y,z) or NULL if (x,y,z) is out of bounds. */ + inline T* + getVoxel (int x, int y, int z) const; + + /** \brief Returns the linear voxel array. */ + const inline T* + getVoxels () const + { + return voxels_; + } + + /** \brief Returns the linear voxel array. */ + inline T* + getVoxels () + { + return voxels_; + } + + /** \brief Converts a linear id to a 3D id, i.e., computes the integer 3D coordinates of a voxel from its position in the voxel array. + * + * \param[in] linear_id the position of the voxel in the internal voxel array. + * \param[out] id3d an array of 3 integers for the integer coordinates of the voxel. */ + inline void + compute3dId (int linear_id, int id3d[3]) const + { + // Compute the z axis id + id3d[2] = linear_id / num_of_voxels_xy_plane_; + int proj_xy = linear_id % num_of_voxels_xy_plane_; + // Compute the y axis id + id3d[1] = proj_xy / num_of_voxels_[0]; + // Compute the x axis id + id3d[0] = proj_xy % num_of_voxels_[0]; + } + + /** \brief Returns the number of voxels in x, y and z direction. */ + inline const int* + getNumberOfVoxelsXYZ() const + { + return (num_of_voxels_); + } + + /** \brief Computes the center of the voxel with given integer coordinates. + * + * \param[in] id3 the integer coordinates along the x, y and z axis. + * \param[out] center */ + inline void + computeVoxelCenter (const int id3[3], REAL center[3]) const + { + center[0] = min_center_[0] + static_cast (id3[0])*spacing_[0]; + center[1] = min_center_[1] + static_cast (id3[1])*spacing_[1]; + center[2] = min_center_[2] + static_cast (id3[2])*spacing_[2]; + } + + /** \brief Returns the total number of voxels. */ + inline int + getNumberOfVoxels() const + { + return (total_num_of_voxels_); + } + + /** \brief Returns the bounds of the voxel structure, which is pointer to the internal array of 6 doubles: (min_x, max_x, min_y, max_y, min_z, max_z). */ + inline const float* + getBounds() const + { + return (bounds_); + } + + /** \brief Copies the bounds of the voxel structure to 'b'. */ + inline void + getBounds(REAL b[6]) const + { + b[0] = bounds_[0]; + b[1] = bounds_[1]; + b[2] = bounds_[2]; + b[3] = bounds_[3]; + b[4] = bounds_[4]; + b[5] = bounds_[5]; + } + + /** \brief Returns the voxel spacing in x, y and z direction. That's the same as the voxel size along each axis. */ + const REAL* + getVoxelSpacing() const + { + return (spacing_); + } + + /** \brief Saves pointers to the voxels which are neighbors of the voxels which contains 'p'. The containing voxel is returned too. + * 'neighs' has to be an array of pointers with space for at least 27 pointers (27 = 3^3 which is the max number of neighbors). The */ + inline int + getNeighbors (const REAL* p, T **neighs) const; + + protected: + T *voxels_; + int num_of_voxels_[3], num_of_voxels_xy_plane_, total_num_of_voxels_; + REAL bounds_[6]; + REAL spacing_[3]; // spacing between the voxel in x, y and z direction = voxel size in x, y and z direction + REAL min_center_[3]; // the center of the voxel with integer coordinates (0, 0, 0) + }; + } // namespace recognition +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/recognition/region_xy.h b/deps_install/include/pcl-1.10/pcl/recognition/region_xy.h new file mode 100755 index 0000000..78d413c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/region_xy.h @@ -0,0 +1,118 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief Function for reading data from a stream. */ + template + void read (std::istream & stream, Type & value) + { + stream.read (reinterpret_cast (&value), sizeof(value)); + } + + /** \brief Function for reading data arrays from a stream. */ + template + void read (std::istream & stream, Type * value, int nr_values) + { + for (int value_index = 0; value_index < nr_values; ++value_index) + { + read (stream, value[value_index]); + } + } + + /** \brief Function for writing data to a stream. */ + template + void write (std::ostream & stream, Type value) + { + stream.write (reinterpret_cast (&value), sizeof (value)); + } + + /** \brief Function for writing data arrays to a stream. */ + template + void write (std::ostream & stream, Type * value, int nr_values) + { + for (int value_index = 0; value_index < nr_values; ++value_index) + { + write (stream, value[value_index]); + } + } + + /** \brief Defines a region in XY-space. + * \author Stefan Holzer + */ + struct PCL_EXPORTS RegionXY + { + /** \brief Constructor. */ + RegionXY () : x (0), y (0), width (0), height (0) {} + + /** \brief x-position of the region. */ + int x; + /** \brief y-position of the region. */ + int y; + /** \brief width of the region. */ + int width; + /** \brief height of the region. */ + int height; + + /** \brief Serializes the object to the specified stream. + * \param[out] stream the stream the object will be serialized to. */ + void + serialize (std::ostream & stream) const + { + write (stream, x); + write (stream, y); + write (stream, width); + write (stream, height); + } + + /** \brief Deserializes the object from the specified stream. + * \param[in] stream the stream the object will be deserialized from. */ + void + deserialize (::std::istream & stream) + { + read (stream, x); + read (stream, y); + read (stream, width); + read (stream, height); + } + + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/rigid_transform_space.h b/deps_install/include/pcl-1.10/pcl/recognition/rigid_transform_space.h new file mode 100755 index 0000000..7d65d3f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/rigid_transform_space.h @@ -0,0 +1,411 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * rigid_transform_space.h + * + * Created on: Feb 15, 2013 + * Author: papazov + */ + +#pragma once + +#include "simple_octree.h" +#include "model_library.h" +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + class RotationSpaceCell + { + public: + class Entry + { + public: + Entry () + : num_transforms_ (0) + { + aux::set3 (axis_angle_, 0.0f); + aux::set3 (translation_, 0.0f); + } + + Entry (const Entry& src) + : num_transforms_ (src.num_transforms_) + { + aux::copy3 (src.axis_angle_, this->axis_angle_); + aux::copy3 (src.translation_, this->translation_); + } + + const Entry& operator = (const Entry& src) + { + num_transforms_ = src.num_transforms_; + aux::copy3 (src.axis_angle_, this->axis_angle_); + aux::copy3 (src.translation_, this->translation_); + + return *this; + } + + inline const Entry& + addRigidTransform (const float axis_angle[3], const float translation[3]) + { + aux::add3 (this->axis_angle_, axis_angle); + aux::add3 (this->translation_, translation); + ++num_transforms_; + + return *this; + } + + inline void + computeAverageRigidTransform (float *rigid_transform = nullptr) + { + if ( num_transforms_ >= 2 ) + { + float factor = 1.0f/static_cast (num_transforms_); + aux::mult3 (axis_angle_, factor); + aux::mult3 (translation_, factor); + num_transforms_ = 1; + } + + if ( rigid_transform ) + { + // Save the rotation (in matrix form) + aux::axisAngleToRotationMatrix (axis_angle_, rigid_transform); + // Save the translation + aux::copy3 (translation_, rigid_transform + 9); + } + } + + inline const float* + getAxisAngle () const + { + return (axis_angle_); + } + + inline const float* + getTranslation () const + { + return (translation_); + } + + inline int + getNumberOfTransforms () const + { + return (num_transforms_); + } + + protected: + float axis_angle_[3], translation_[3]; + int num_transforms_; + };// class Entry + + public: + RotationSpaceCell (){} + virtual ~RotationSpaceCell () + { + model_to_entry_.clear (); + } + + inline std::map& + getEntries () + { + return (model_to_entry_); + } + + inline const RotationSpaceCell::Entry* + getEntry (const ModelLibrary::Model* model) const + { + std::map::const_iterator res = model_to_entry_.find (model); + + if ( res != model_to_entry_.end () ) + return (&res->second); + + return (nullptr); + } + + inline const RotationSpaceCell::Entry& + addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3]) + { + return model_to_entry_[model].addRigidTransform (axis_angle, translation); + } + + protected: + std::map model_to_entry_; + }; // class RotationSpaceCell + + class RotationSpaceCellCreator + { + public: + RotationSpaceCellCreator (){} + virtual ~RotationSpaceCellCreator (){} + + RotationSpaceCell* create (const SimpleOctree::Node* ) + { + return (new RotationSpaceCell ()); + } + }; + + using CellOctree = SimpleOctree; + + /** \brief This is a class for a discrete representation of the rotation space based on the axis-angle representation. + * This class is not supposed to be very general. That's why it is dependent on the class ModelLibrary. + * + * \author Chavdar Papazov + * \ingroup recognition + */ + class PCL_EXPORTS RotationSpace + { + public: + /** \brief We use the axis-angle representation for rotations. The axis is encoded in the vector + * and the angle is its magnitude. This is represented in an octree with bounds [-pi, pi]^3. */ + RotationSpace (float discretization) + { + float min = -(AUX_PI_FLOAT + 0.000000001f), max = AUX_PI_FLOAT + 0.000000001f; + float bounds[6] = {min, max, min, max, min, max}; + + // Build the voxel structure + octree_.build (bounds, discretization, &cell_creator_); + } + + virtual ~RotationSpace () + { + octree_.clear (); + } + + inline void + setCenter (const float* c) + { + center_[0] = c[0]; + center_[1] = c[1]; + center_[2] = c[2]; + } + + inline const float* + getCenter () const { return center_;} + + inline bool + getTransformWithMostVotes (const ModelLibrary::Model* model, float rigid_transform[12]) const + { + RotationSpaceCell::Entry with_most_votes; + const std::vector& full_leaves = octree_.getFullLeaves (); + int max_num_transforms = 0; + + // For each full leaf + for (const auto &full_leaf : full_leaves) + { + // Is there an entry for 'model' in the current cell + const RotationSpaceCell::Entry *entry = full_leaf->getData ().getEntry (model); + if ( !entry ) + continue; + + int num_transforms = entry->getNumberOfTransforms (); + const std::set& neighs = full_leaf->getNeighbors (); + + // For each neighbor + for (const auto &neigh : neighs) + { + const RotationSpaceCell::Entry *neigh_entry = neigh->getData ().getEntry (model); + if ( !neigh_entry ) + continue; + + num_transforms += neigh_entry->getNumberOfTransforms (); + } + + if ( num_transforms > max_num_transforms ) + { + with_most_votes = *entry; + max_num_transforms = num_transforms; + } + } + + if ( !max_num_transforms ) + return false; + + with_most_votes.computeAverageRigidTransform (rigid_transform); + + return true; + } + + inline bool + addRigidTransform (const ModelLibrary::Model* model, const float axis_angle[3], const float translation[3]) + { + CellOctree::Node* cell = octree_.createLeaf (axis_angle[0], axis_angle[1], axis_angle[2]); + + if ( !cell ) + { + const float *b = octree_.getBounds (); + printf ("WARNING in 'RotationSpace::%s()': the provided axis-angle input (%f, %f, %f) is " + "out of the rotation space bounds ([%f, %f], [%f, %f], [%f, %f]).\n", + __func__, axis_angle[0], axis_angle[1], axis_angle[2], b[0], b[1], b[2], b[3], b[4], b[5]); + return (false); + } + + // Add the rigid transform to the cell + cell->getData ().addRigidTransform (model, axis_angle, translation); + + return (true); + } + + protected: + CellOctree octree_; + RotationSpaceCellCreator cell_creator_; + float center_[3]; + };// class RotationSpace + + class RotationSpaceCreator + { + public: + RotationSpaceCreator() + : counter_ (0) + {} + + virtual ~RotationSpaceCreator(){} + + RotationSpace* create(const SimpleOctree::Node* leaf) + { + RotationSpace *rot_space = new RotationSpace (discretization_); + rot_space->setCenter (leaf->getCenter ()); + rotation_spaces_.push_back (rot_space); + + ++counter_; + + return (rot_space); + } + + void + setDiscretization (float value){ discretization_ = value;} + + int + getNumberOfRotationSpaces () const { return (counter_);} + + const std::list& + getRotationSpaces () const { return (rotation_spaces_);} + + std::list& + getRotationSpaces (){ return (rotation_spaces_);} + + void + reset () + { + counter_ = 0; + rotation_spaces_.clear (); + } + + protected: + float discretization_; + int counter_; + std::list rotation_spaces_; + }; + + using RotationSpaceOctree = SimpleOctree; + + class PCL_EXPORTS RigidTransformSpace + { + public: + RigidTransformSpace (){} + virtual ~RigidTransformSpace (){ this->clear ();} + + inline void + build (const float* pos_bounds, float translation_cell_size, float rotation_cell_size) + { + this->clear (); + + rotation_space_creator_.setDiscretization (rotation_cell_size); + + pos_octree_.build (pos_bounds, translation_cell_size, &rotation_space_creator_); + } + + inline void + clear () + { + pos_octree_.clear (); + rotation_space_creator_.reset (); + } + + inline std::list& + getRotationSpaces () + { + return (rotation_space_creator_.getRotationSpaces ()); + } + + inline const std::list& + getRotationSpaces () const + { + return (rotation_space_creator_.getRotationSpaces ()); + } + + inline int + getNumberOfOccupiedRotationSpaces () + { + return (rotation_space_creator_.getNumberOfRotationSpaces ()); + } + + inline bool + addRigidTransform (const ModelLibrary::Model* model, const float position[3], const float rigid_transform[12]) + { + // Get the leaf 'position' ends up in + RotationSpaceOctree::Node* leaf = pos_octree_.createLeaf (position[0], position[1], position[2]); + + if ( !leaf ) + { + printf ("WARNING in 'RigidTransformSpace::%s()': the input position (%f, %f, %f) is out of bounds.\n", + __func__, position[0], position[1], position[2]); + return (false); + } + + float rot_angle, axis_angle[3]; + // Extract the axis-angle representation from the rotation matrix + aux::rotationMatrixToAxisAngle (rigid_transform, axis_angle, rot_angle); + // Multiply the axis by the angle to get the final representation + aux::mult3 (axis_angle, rot_angle); + + // Now, add the rigid transform to the rotation space + leaf->getData ().addRigidTransform (model, axis_angle, rigid_transform + 9); + + return (true); + } + + protected: + RotationSpaceOctree pos_octree_; + RotationSpaceCreator rotation_space_creator_; + }; // class RigidTransformSpace + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/simple_octree.h b/deps_install/include/pcl-1.10/pcl/recognition/simple_octree.h new file mode 100755 index 0000000..38d8e26 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/simple_octree.h @@ -0,0 +1,210 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +/* + * simple_octree.h + * + * Created on: Mar 11, 2013 + * Author: papazov + */ + +#pragma once + +#include + +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class PCL_EXPORTS SimpleOctree + { + public: + class Node + { + public: + Node (); + + virtual~ Node (); + + inline void + setCenter (const Scalar *c); + + inline void + setBounds (const Scalar *b); + + inline const Scalar* + getCenter () const { return center_;} + + inline const Scalar* + getBounds () const { return bounds_;} + + inline void + getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} + + inline Node* + getChild (int id) { return &children_[id];} + + inline Node* + getChildren () { return children_;} + + inline void + setData (const NodeData& src){ *data_ = src;} + + inline NodeData& + getData (){ return *data_;} + + inline const NodeData& + getData () const { return *data_;} + + inline Node* + getParent (){ return parent_;} + + inline float + getRadius () const { return radius_;} + + inline bool + hasData (){ return static_cast (data_);} + + inline bool + hasChildren (){ return static_cast (children_);} + + inline const std::set& + getNeighbors () const { return (full_leaf_neighbors_);} + + inline void + deleteChildren (); + + inline void + deleteData (); + + friend class SimpleOctree; + + protected: + void + setData (NodeData* data){ delete data_; data_ = data;} + + inline bool + createChildren (); + + /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens + * of either of the nodes has no data. */ + inline void + makeNeighbors (Node* node); + + inline void + setParent (Node* parent){ parent_ = parent;} + + /** \brief Computes the "radius" of the node which is half the diagonal length. */ + inline void + computeRadius (); + + protected: + NodeData *data_; + Scalar center_[3], bounds_[6]; + Node *parent_, *children_; + Scalar radius_; + std::set full_leaf_neighbors_; + }; + + public: + SimpleOctree (); + + virtual ~SimpleOctree (); + + void + clear (); + + /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf + * size equal to 'voxel_size'. */ + void + build (const Scalar* bounds, Scalar voxel_size, NodeDataCreator* node_data_creator); + + /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within + * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method + * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and + * method just returns a pointer to the leaf. Note that for a new created leaf, the method also creates its data + * object. */ + inline Node* + createLeaf (Scalar x, Scalar y, Scalar z); + + /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the full + * leaf, i.e., the one having a data object, with id [i, j, k] or NULL is no such leaf exists. */ + inline Node* + getFullLeaf (int i, int j, int k); + + /** \brief Returns a pointer to the full leaf, i.e., one having a data pbject, containing p = (x, y, z) or NULL if no such leaf exists. */ + inline Node* + getFullLeaf (Scalar x, Scalar y, Scalar z); + + inline std::vector& + getFullLeaves () { return full_leaves_;} + + inline const std::vector& + getFullLeaves () const { return full_leaves_;} + + inline Node* + getRoot (){ return root_;} + + inline const Scalar* + getBounds () const { return (bounds_);} + + inline void + getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));} + + inline Scalar + getVoxelSize () const { return voxel_size_;} + + protected: + inline void + insertNeighbors (Node* node); + + protected: + Scalar voxel_size_, bounds_[6]; + int tree_levels_; + Node* root_; + std::vector full_leaves_; + NodeDataCreator* node_data_creator_; + }; + } // namespace recognition +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/recognition/sparse_quantized_multi_mod_template.h b/deps_install/include/pcl-1.10/pcl/recognition/sparse_quantized_multi_mod_template.h new file mode 100755 index 0000000..d8b298e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/sparse_quantized_multi_mod_template.h @@ -0,0 +1,153 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + + /** \brief Feature that defines a position and quantized value in a specific modality. + * \author Stefan Holzer + */ + struct QuantizedMultiModFeature + { + /** \brief Constructor. */ + QuantizedMultiModFeature () : x (0), y (0), modality_index (0), quantized_value (0) {} + + /** \brief x-position. */ + int x; + /** \brief y-position. */ + int y; + /** \brief the index of the corresponding modality. */ + std::size_t modality_index; + /** \brief the quantized value attached to the feature. */ + unsigned char quantized_value; + + /** \brief Compares whether two features are the same. + * \param[in] base the feature to compare to. + */ + bool + compareForEquality (const QuantizedMultiModFeature & base) + { + if (base.x != x) + return false; + if (base.y != y) + return false; + if (base.modality_index != modality_index) + return false; + if (base.quantized_value != quantized_value) + return false; + + return true; + } + + /** \brief Serializes the object to the specified stream. + * \param[out] stream the stream the object will be serialized to. */ + void + serialize (std::ostream & stream) const + { + write (stream, x); + write (stream, y); + write (stream, modality_index); + write (stream, quantized_value); + } + + /** \brief Deserializes the object from the specified stream. + * \param[in] stream the stream the object will be deserialized from. */ + void + deserialize (std::istream & stream) + { + read (stream, x); + read (stream, y); + read (stream, modality_index); + read (stream, quantized_value); + } + }; + + /** \brief A multi-modality template constructed from a set of quantized multi-modality features. + * \author Stefan Holzer + */ + struct SparseQuantizedMultiModTemplate + { + /** \brief Constructor. */ + SparseQuantizedMultiModTemplate () {} + + /** \brief The storage for the multi-modality features. */ + std::vector features; + + /** \brief The region assigned to the template. */ + RegionXY region; + + /** \brief Serializes the object to the specified stream. + * \param[out] stream the stream the object will be serialized to. */ + void + serialize (std::ostream & stream) const + { + const int num_of_features = static_cast (features.size ()); + write (stream, num_of_features); + for (int feature_index = 0; feature_index < num_of_features; ++feature_index) + { + features[feature_index].serialize (stream); + } + + region.serialize (stream); + } + + /** \brief Deserializes the object from the specified stream. + * \param[in] stream the stream the object will be deserialized from. */ + void + deserialize (std::istream & stream) + { + features.clear (); + + int num_of_features; + read (stream, num_of_features); + features.resize (num_of_features); + for (int feature_index = 0; feature_index < num_of_features; ++feature_index) + { + features[feature_index].deserialize (stream); + } + + region.deserialize (stream); + } + }; + +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/surface_normal_modality.h b/deps_install/include/pcl-1.10/pcl/recognition/surface_normal_modality.h new file mode 100755 index 0000000..fd2e130 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/surface_normal_modality.h @@ -0,0 +1,1640 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +// #include +#include +#include +#include + +namespace pcl +{ + + /** \brief Map that stores orientations. + * \author Stefan Holzer + */ + struct PCL_EXPORTS LINEMOD_OrientationMap + { + public: + /** \brief Constructor. */ + inline LINEMOD_OrientationMap () : width_ (0), height_ (0) {} + /** \brief Destructor. */ + inline ~LINEMOD_OrientationMap () {} + + /** \brief Returns the width of the modality data map. */ + inline std::size_t + getWidth () const + { + return width_; + } + + /** \brief Returns the height of the modality data map. */ + inline std::size_t + getHeight () const + { + return height_; + } + + /** \brief Resizes the map to the specific width and height and initializes + * all new elements with the specified value. + * \param[in] width the width of the resized map. + * \param[in] height the height of the resized map. + * \param[in] value the value all new elements will be initialized with. + */ + inline void + resize (const std::size_t width, const std::size_t height, const float value) + { + width_ = width; + height_ = height; + map_.clear (); + map_.resize (width*height, value); + } + + /** \brief Operator to access elements of the map. + * \param[in] col_index the column index of the element to access. + * \param[in] row_index the row index of the element to access. + */ + inline float & + operator() (const std::size_t col_index, const std::size_t row_index) + { + return map_[row_index * width_ + col_index]; + } + + /** \brief Operator to access elements of the map. + * \param[in] col_index the column index of the element to access. + * \param[in] row_index the row index of the element to access. + */ + inline const float & + operator() (const std::size_t col_index, const std::size_t row_index) const + { + return map_[row_index * width_ + col_index]; + } + + private: + /** \brief The width of the map. */ + std::size_t width_; + /** \brief The height of the map. */ + std::size_t height_; + /** \brief Storage for the data of the map. */ + std::vector map_; + + }; + + /** \brief Look-up-table for fast surface normal quantization. + * \author Stefan Holzer + */ + struct QuantizedNormalLookUpTable + { + /** \brief The range of the LUT in x-direction. */ + int range_x; + /** \brief The range of the LUT in y-direction. */ + int range_y; + /** \brief The range of the LUT in z-direction. */ + int range_z; + + /** \brief The offset in x-direction. */ + int offset_x; + /** \brief The offset in y-direction. */ + int offset_y; + /** \brief The offset in z-direction. */ + int offset_z; + + /** \brief The size of the LUT in x-direction. */ + int size_x; + /** \brief The size of the LUT in y-direction. */ + int size_y; + /** \brief The size of the LUT in z-direction. */ + int size_z; + + /** \brief The LUT data. */ + unsigned char * lut; + + /** \brief Constructor. */ + QuantizedNormalLookUpTable () : + range_x (-1), range_y (-1), range_z (-1), + offset_x (-1), offset_y (-1), offset_z (-1), + size_x (-1), size_y (-1), size_z (-1), lut (nullptr) + {} + + /** \brief Destructor. */ + ~QuantizedNormalLookUpTable () + { + delete[] lut; + } + + /** \brief Initializes the LUT. + * \param[in] range_x_arg the range of the LUT in x-direction. + * \param[in] range_y_arg the range of the LUT in y-direction. + * \param[in] range_z_arg the range of the LUT in z-direction. + */ + void + initializeLUT (const int range_x_arg, const int range_y_arg, const int range_z_arg) + { + range_x = range_x_arg; + range_y = range_y_arg; + range_z = range_z_arg; + size_x = range_x_arg+1; + size_y = range_y_arg+1; + size_z = range_z_arg+1; + offset_x = range_x_arg/2; + offset_y = range_y_arg/2; + offset_z = range_z_arg; + + //if (lut != NULL) free16(lut); + //lut = malloc16(size_x*size_y*size_z); + + delete[] lut; + lut = new unsigned char[size_x*size_y*size_z]; + + const int nr_normals = 8; + pcl::PointCloud::VectorType ref_normals (nr_normals); + + const float normal0_angle = 40.0f * 3.14f / 180.0f; + ref_normals[0].x = std::cos (normal0_angle); + ref_normals[0].y = 0.0f; + ref_normals[0].z = -sinf (normal0_angle); + + const float inv_nr_normals = 1.0f / static_cast (nr_normals); + for (int normal_index = 1; normal_index < nr_normals; ++normal_index) + { + const float angle = 2.0f * static_cast (M_PI * normal_index * inv_nr_normals); + + ref_normals[normal_index].x = std::cos (angle) * ref_normals[0].x - sinf (angle) * ref_normals[0].y; + ref_normals[normal_index].y = sinf (angle) * ref_normals[0].x + std::cos (angle) * ref_normals[0].y; + ref_normals[normal_index].z = ref_normals[0].z; + } + + // normalize normals + for (int normal_index = 0; normal_index < nr_normals; ++normal_index) + { + const float length = std::sqrt (ref_normals[normal_index].x * ref_normals[normal_index].x + + ref_normals[normal_index].y * ref_normals[normal_index].y + + ref_normals[normal_index].z * ref_normals[normal_index].z); + + const float inv_length = 1.0f / length; + + ref_normals[normal_index].x *= inv_length; + ref_normals[normal_index].y *= inv_length; + ref_normals[normal_index].z *= inv_length; + } + + // set LUT + for (int z_index = 0; z_index < size_z; ++z_index) + { + for (int y_index = 0; y_index < size_y; ++y_index) + { + for (int x_index = 0; x_index < size_x; ++x_index) + { + PointXYZ normal (static_cast (x_index - range_x/2), + static_cast (y_index - range_y/2), + static_cast (z_index - range_z)); + const float length = std::sqrt (normal.x*normal.x + normal.y*normal.y + normal.z*normal.z); + const float inv_length = 1.0f / (length + 0.00001f); + + normal.x *= inv_length; + normal.y *= inv_length; + normal.z *= inv_length; + + float max_response = -1.0f; + int max_index = -1; + + for (int normal_index = 0; normal_index < nr_normals; ++normal_index) + { + const float response = normal.x * ref_normals[normal_index].x + + normal.y * ref_normals[normal_index].y + + normal.z * ref_normals[normal_index].z; + + const float abs_response = std::abs (response); + if (max_response < abs_response) + { + max_response = abs_response; + max_index = normal_index; + } + + lut[z_index*size_y*size_x + y_index*size_x + x_index] = static_cast (0x1 << max_index); + } + } + } + } + } + + /** \brief Operator to access an element in the LUT. + * \param[in] x the x-component of the normal. + * \param[in] y the y-component of the normal. + * \param[in] z the z-component of the normal. + */ + inline unsigned char + operator() (const float x, const float y, const float z) const + { + const std::size_t x_index = static_cast (x * static_cast (offset_x) + static_cast (offset_x)); + const std::size_t y_index = static_cast (y * static_cast (offset_y) + static_cast (offset_y)); + const std::size_t z_index = static_cast (z * static_cast (range_z) + static_cast (range_z)); + + const std::size_t index = z_index*size_y*size_x + y_index*size_x + x_index; + + return (lut[index]); + } + + /** \brief Operator to access an element in the LUT. + * \param[in] index the index of the element. + */ + inline unsigned char + operator() (const int index) const + { + return (lut[index]); + } + }; + + + /** \brief Modality based on surface normals. + * \author Stefan Holzer + */ + template + class SurfaceNormalModality : public QuantizableModality, public PCLBase + { + protected: + using PCLBase::input_; + + /** \brief Candidate for a feature (used in feature extraction methods). */ + struct Candidate + { + /** \brief Constructor. */ + Candidate () : distance (0.0f), bin_index (0), x (0), y (0) {} + + /** \brief Normal. */ + Normal normal; + /** \brief Distance to the next different quantized value. */ + float distance; + + /** \brief Quantized value. */ + unsigned char bin_index; + + /** \brief x-position of the feature. */ + std::size_t x; + /** \brief y-position of the feature. */ + std::size_t y; + + /** \brief Compares two candidates based on their distance to the next different quantized value. + * \param[in] rhs the candidate to compare with. + */ + bool + operator< (const Candidate & rhs) const + { + return (distance > rhs.distance); + } + }; + + public: + using PointCloudIn = pcl::PointCloud; + + /** \brief Constructor. */ + SurfaceNormalModality (); + /** \brief Destructor. */ + ~SurfaceNormalModality (); + + /** \brief Sets the spreading size. + * \param[in] spreading_size the spreading size. + */ + inline void + setSpreadingSize (const std::size_t spreading_size) + { + spreading_size_ = spreading_size; + } + + /** \brief Enables/disables the use of extracting a variable number of features. + * \param[in] enabled specifies whether extraction of a variable number of features will be enabled/disabled. + */ + inline void + setVariableFeatureNr (const bool enabled) + { + variable_feature_nr_ = enabled; + } + + /** \brief Returns the surface normals. */ + inline pcl::PointCloud & + getSurfaceNormals () + { + return surface_normals_; + } + + /** \brief Returns the surface normals. */ + inline const pcl::PointCloud & + getSurfaceNormals () const + { + return surface_normals_; + } + + /** \brief Returns a reference to the internal quantized map. */ + inline QuantizedMap & + getQuantizedMap () override + { + return (filtered_quantized_surface_normals_); + } + + /** \brief Returns a reference to the internal spread quantized map. */ + inline QuantizedMap & + getSpreadedQuantizedMap () override + { + return (spreaded_quantized_surface_normals_); + } + + /** \brief Returns a reference to the orientation map. */ + inline LINEMOD_OrientationMap & + getOrientationMap () + { + return (surface_normal_orientations_); + } + + /** \brief Extracts features from this modality within the specified mask. + * \param[in] mask defines the areas where features are searched in. + * \param[in] nr_features defines the number of features to be extracted + * (might be less if not sufficient information is present in the modality). + * \param[in] modality_index the index which is stored in the extracted features. + * \param[out] features the destination for the extracted features. + */ + void + extractFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modality_index, + std::vector & features) const override; + + /** \brief Extracts all possible features from the modality within the specified mask. + * \param[in] mask defines the areas where features are searched in. + * \param[in] nr_features IGNORED (TODO: remove this parameter). + * \param[in] modality_index the index which is stored in the extracted features. + * \param[out] features the destination for the extracted features. + */ + void + extractAllFeatures (const MaskMap & mask, std::size_t nr_features, std::size_t modality_index, + std::vector & features) const override; + + /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + void + setInputCloud (const typename PointCloudIn::ConstPtr & cloud) override + { + input_ = cloud; + } + + /** \brief Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading). */ + virtual void + processInputData (); + + /** \brief Processes the input data assuming that everything up to filtering is already done/available + * (so only spreading is performed). */ + virtual void + processInputDataFromFiltered (); + + protected: + + /** \brief Computes the surface normals from the input cloud. */ + void + computeSurfaceNormals (); + + /** \brief Computes and quantizes the surface normals. */ + void + computeAndQuantizeSurfaceNormals (); + + /** \brief Computes and quantizes the surface normals. */ + void + computeAndQuantizeSurfaceNormals2 (); + + /** \brief Quantizes the surface normals. */ + void + quantizeSurfaceNormals (); + + /** \brief Filters the quantized surface normals. */ + void + filterQuantizedSurfaceNormals (); + + /** \brief Computes a distance map from the supplied input mask. + * \param[in] input the mask for which a distance map will be computed. + * \param[out] output the destination for the distance map. + */ + void + computeDistanceMap (const MaskMap & input, DistanceMap & output) const; + + private: + + /** \brief Determines whether variable numbers of features are extracted or not. */ + bool variable_feature_nr_; + + /** \brief The feature distance threshold. */ + float feature_distance_threshold_; + /** \brief Minimum distance of a feature to a border. */ + float min_distance_to_border_; + + /** \brief Look-up-table for quantizing surface normals. */ + QuantizedNormalLookUpTable normal_lookup_; + + /** \brief The spreading size. */ + std::size_t spreading_size_; + + /** \brief Point cloud holding the computed surface normals. */ + pcl::PointCloud surface_normals_; + /** \brief Quantized surface normals. */ + pcl::QuantizedMap quantized_surface_normals_; + /** \brief Filtered quantized surface normals. */ + pcl::QuantizedMap filtered_quantized_surface_normals_; + /** \brief Spread quantized surface normals. */ + pcl::QuantizedMap spreaded_quantized_surface_normals_; + + /** \brief Map containing surface normal orientations. */ + pcl::LINEMOD_OrientationMap surface_normal_orientations_; + + }; + +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::SurfaceNormalModality:: +SurfaceNormalModality () + : variable_feature_nr_ (false) + , feature_distance_threshold_ (2.0f) + , min_distance_to_border_ (2.0f) + , spreading_size_ (8) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::SurfaceNormalModality::~SurfaceNormalModality () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::processInputData () +{ + // compute surface normals + //computeSurfaceNormals (); + + // quantize surface normals + //quantizeSurfaceNormals (); + + computeAndQuantizeSurfaceNormals2 (); + + // filter quantized surface normals + filterQuantizedSurfaceNormals (); + + // spread quantized surface normals + pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_, + spreaded_quantized_surface_normals_, + spreading_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::processInputDataFromFiltered () +{ + // spread quantized surface normals + pcl::QuantizedMap::spreadQuantizedMap (filtered_quantized_surface_normals_, + spreaded_quantized_surface_normals_, + spreading_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::computeSurfaceNormals () +{ + // compute surface normals + pcl::LinearLeastSquaresNormalEstimation ne; + ne.setMaxDepthChangeFactor(0.05f); + ne.setNormalSmoothingSize(5.0f); + ne.setDepthDependentSmoothing(false); + ne.setInputCloud (input_); + ne.compute (surface_normals_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::computeAndQuantizeSurfaceNormals () +{ + // compute surface normals + //pcl::LinearLeastSquaresNormalEstimation ne; + //ne.setMaxDepthChangeFactor(0.05f); + //ne.setNormalSmoothingSize(5.0f); + //ne.setDepthDependentSmoothing(false); + //ne.setInputCloud (input_); + //ne.compute (surface_normals_); + + + const float bad_point = std::numeric_limits::quiet_NaN (); + + const int width = input_->width; + const int height = input_->height; + + surface_normals_.resize (width*height); + surface_normals_.width = width; + surface_normals_.height = height; + surface_normals_.is_dense = false; + + quantized_surface_normals_.resize (width, height); + + // we compute the normals as follows: + // ---------------------------------- + // + // for the depth-gradient you can make the following first-order Taylor approximation: + // D(x + dx) - D(x) = dx^T \Delta D + h.o.t. + // + // build linear system by stacking up equation for 8 neighbor points: + // Y = X \Delta D + // + // => \Delta D = (X^T X)^{-1} X^T Y + // => \Delta D = (A)^{-1} b + + for (int y = 0; y < height; ++y) + { + for (int x = 0; x < width; ++x) + { + const int index = y * width + x; + + const float px = input_->points[index].x; + const float py = input_->points[index].y; + const float pz = input_->points[index].z; + + if (std::isnan(px) || pz > 2.0f) + { + surface_normals_.points[index].normal_x = bad_point; + surface_normals_.points[index].normal_y = bad_point; + surface_normals_.points[index].normal_z = bad_point; + surface_normals_.points[index].curvature = bad_point; + + quantized_surface_normals_ (x, y) = 0; + + continue; + } + + const int smoothingSizeInt = 5; + + float matA0 = 0.0f; + float matA1 = 0.0f; + float matA3 = 0.0f; + + float vecb0 = 0.0f; + float vecb1 = 0.0f; + + for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt) + { + for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt) + { + if (u < 0 || u >= width || v < 0 || v >= height) continue; + + const std::size_t index2 = v * width + u; + + const float qx = input_->points[index2].x; + const float qy = input_->points[index2].y; + const float qz = input_->points[index2].z; + + if (std::isnan(qx)) continue; + + const float delta = qz - pz; + const float i = qx - px; + const float j = qy - py; + + const float f = std::abs(delta) < 0.05f ? 1.0f : 0.0f; + + matA0 += f * i * i; + matA1 += f * i * j; + matA3 += f * j * j; + vecb0 += f * i * delta; + vecb1 += f * j * delta; + } + } + + const float det = matA0 * matA3 - matA1 * matA1; + const float ddx = matA3 * vecb0 - matA1 * vecb1; + const float ddy = -matA1 * vecb0 + matA0 * vecb1; + + const float nx = ddx; + const float ny = ddy; + const float nz = -det * pz; + + const float length = nx * nx + ny * ny + nz * nz; + + if (length <= 0.0f) + { + surface_normals_.points[index].normal_x = bad_point; + surface_normals_.points[index].normal_y = bad_point; + surface_normals_.points[index].normal_z = bad_point; + surface_normals_.points[index].curvature = bad_point; + + quantized_surface_normals_ (x, y) = 0; + } + else + { + const float normInv = 1.0f / std::sqrt (length); + + const float normal_x = nx * normInv; + const float normal_y = ny * normInv; + const float normal_z = nz * normInv; + + surface_normals_.points[index].normal_x = normal_x; + surface_normals_.points[index].normal_y = normal_y; + surface_normals_.points[index].normal_z = normal_z; + surface_normals_.points[index].curvature = bad_point; + + float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f; + + if (angle < 0.0f) angle += 360.0f; + if (angle >= 360.0f) angle -= 360.0f; + + int bin_index = static_cast (angle*8.0f/360.0f) & 7; + + quantized_surface_normals_ (x, y) = static_cast (bin_index); + } + } + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +// Contains GRANULARITY and NORMAL_LUT +//#include "normal_lut.i" + +static void accumBilateral(long delta, long i, long j, long * A, long * b, int threshold) +{ + long f = std::abs(delta) < threshold ? 1 : 0; + + const long fi = f * i; + const long fj = f * j; + + A[0] += fi * i; + A[1] += fi * j; + A[3] += fj * j; + b[0] += fi * delta; + b[1] += fj * delta; +} + +/** + * \brief Compute quantized normal image from depth image. + * + * Implements section 2.6 "Extension to Dense Depth Sensors." + * + * \todo Should also need camera model, or at least focal lengths? Replace distance_threshold with mask? + */ +template void +pcl::SurfaceNormalModality::computeAndQuantizeSurfaceNormals2 () +{ + const int width = input_->width; + const int height = input_->height; + + unsigned short * lp_depth = new unsigned short[width*height]; + unsigned char * lp_normals = new unsigned char[width*height]; + memset (lp_normals, 0, width*height); + + surface_normal_orientations_.resize (width, height, 0.0f); + + for (int row_index = 0; row_index < height; ++row_index) + { + for (int col_index = 0; col_index < width; ++col_index) + { + const float value = input_->points[row_index*width + col_index].z; + if (std::isfinite (value)) + { + lp_depth[row_index*width + col_index] = static_cast (value * 1000.0f); + } + else + { + lp_depth[row_index*width + col_index] = 0; + } + } + } + + const int l_W = width; + const int l_H = height; + + const int l_r = 5; // used to be 7 + //const int l_offset0 = -l_r - l_r * l_W; + //const int l_offset1 = 0 - l_r * l_W; + //const int l_offset2 = +l_r - l_r * l_W; + //const int l_offset3 = -l_r; + //const int l_offset4 = +l_r; + //const int l_offset5 = -l_r + l_r * l_W; + //const int l_offset6 = 0 + l_r * l_W; + //const int l_offset7 = +l_r + l_r * l_W; + + const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r}; + const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r}; + const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W + , offsets_i[1] + offsets_j[1] * l_W + , offsets_i[2] + offsets_j[2] * l_W + , offsets_i[3] + offsets_j[3] * l_W + , offsets_i[4] + offsets_j[4] * l_W + , offsets_i[5] + offsets_j[5] * l_W + , offsets_i[6] + offsets_j[6] * l_W + , offsets_i[7] + offsets_j[7] * l_W }; + + + //const int l_offsetx = GRANULARITY / 2; + //const int l_offsety = GRANULARITY / 2; + + const int difference_threshold = 50; + const int distance_threshold = 2000; + + //const double scale = 1000.0; + //const double difference_threshold = 0.05 * scale; + //const double distance_threshold = 2.0 * scale; + + for (int l_y = l_r; l_y < l_H - l_r - 1; ++l_y) + { + unsigned short * lp_line = lp_depth + (l_y * l_W + l_r); + unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r); + + for (int l_x = l_r; l_x < l_W - l_r - 1; ++l_x) + { + long l_d = lp_line[0]; + //float l_d = input_->points[(l_y * l_W + l_r) + l_x].z; + //float px = input_->points[(l_y * l_W + l_r) + l_x].x; + //float py = input_->points[(l_y * l_W + l_r) + l_x].y; + + if (l_d < distance_threshold) + { + // accum + long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0; + long l_b[2]; l_b[0] = l_b[1] = 0; + //double l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0; + //double l_b[2]; l_b[0] = l_b[1] = 0; + + accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold); + accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold); + accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold); + accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold); + accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold); + accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold); + accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold); + accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold); + + //for (std::size_t index = 0; index < 8; ++index) + //{ + // //accumBilateral(lp_line[offsets[index]] - l_d, offsets_i[index], offsets_j[index], l_A, l_b, difference_threshold); + + // //const long delta = lp_line[offsets[index]] - l_d; + // //const long i = offsets_i[index]; + // //const long j = offsets_j[index]; + // //long * A = l_A; + // //long * b = l_b; + // //const int threshold = difference_threshold; + + // //const long f = std::abs(delta) < threshold ? 1 : 0; + + // //const long fi = f * i; + // //const long fj = f * j; + + // //A[0] += fi * i; + // //A[1] += fi * j; + // //A[3] += fj * j; + // //b[0] += fi * delta; + // //b[1] += fj * delta; + + + // const double delta = 1000.0f * (input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].z - l_d); + // const double i = offsets_i[index]; + // const double j = offsets_j[index]; + // //const float i = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].x - px;//offsets_i[index]; + // //const float j = input_->points[(l_y * l_W + l_r) + l_x + offsets[index]].y - py;//offsets_j[index]; + // double * A = l_A; + // double * b = l_b; + // const double threshold = difference_threshold; + + // const double f = std::fabs(delta) < threshold ? 1.0f : 0.0f; + + // const double fi = f * i; + // const double fj = f * j; + + // A[0] += fi * i; + // A[1] += fi * j; + // A[3] += fj * j; + // b[0] += fi * delta; + // b[1] += fj * delta; + //} + + //long f = std::abs(delta) < threshold ? 1 : 0; + + //const long fi = f * i; + //const long fj = f * j; + + //A[0] += fi * i; + //A[1] += fi * j; + //A[3] += fj * j; + //b[0] += fi * delta; + //b[1] += fj * delta; + + + // solve + long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1]; + long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1]; + long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1]; + + /// @todo Magic number 1150 is focal length? This is something like + /// f in SXGA mode, but in VGA is more like 530. + float l_nx = static_cast(1150 * l_ddx); + float l_ny = static_cast(1150 * l_ddy); + float l_nz = static_cast(-l_det * l_d); + + //// solve + //double l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1]; + //double l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1]; + //double l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1]; + + ///// @todo Magic number 1150 is focal length? This is something like + ///// f in SXGA mode, but in VGA is more like 530. + //const double dummy_focal_length = 1150.0f; + //double l_nx = l_ddx * dummy_focal_length; + //double l_ny = l_ddy * dummy_focal_length; + //double l_nz = -l_det * l_d; + + float l_sqrt = std::sqrt (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz); + + if (l_sqrt > 0) + { + float l_norminv = 1.0f / (l_sqrt); + + l_nx *= l_norminv; + l_ny *= l_norminv; + l_nz *= l_norminv; + + float angle = 22.5f + std::atan2 (l_ny, l_nx) * 180.0f / 3.14f; + + if (angle < 0.0f) angle += 360.0f; + if (angle >= 360.0f) angle -= 360.0f; + + int bin_index = static_cast (angle*8.0f/360.0f) & 7; + + surface_normal_orientations_ (l_x, l_y) = angle; + + //*lp_norm = std::abs(l_nz)*255; + + //int l_val1 = static_cast(l_nx * l_offsetx + l_offsetx); + //int l_val2 = static_cast(l_ny * l_offsety + l_offsety); + //int l_val3 = static_cast(l_nz * GRANULARITY + GRANULARITY); + + //*lp_norm = NORMAL_LUT[l_val3][l_val2][l_val1]; + *lp_norm = static_cast (0x1 << bin_index); + } + else + { + *lp_norm = 0; // Discard shadows from depth sensor + } + } + else + { + *lp_norm = 0; //out of depth + } + ++lp_line; + ++lp_norm; + } + } + /*cvSmooth(m_dep[0], m_dep[0], CV_MEDIAN, 5, 5);*/ + + unsigned char map[255]; + memset(map, 0, 255); + + map[0x1<<0] = 0; + map[0x1<<1] = 1; + map[0x1<<2] = 2; + map[0x1<<3] = 3; + map[0x1<<4] = 4; + map[0x1<<5] = 5; + map[0x1<<6] = 6; + map[0x1<<7] = 7; + + quantized_surface_normals_.resize (width, height); + for (int row_index = 0; row_index < height; ++row_index) + { + for (int col_index = 0; col_index < width; ++col_index) + { + quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]]; + } + } + + delete[] lp_depth; + delete[] lp_normals; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::extractFeatures (const MaskMap & mask, + const std::size_t nr_features, + const std::size_t modality_index, + std::vector & features) const +{ + const std::size_t width = mask.getWidth (); + const std::size_t height = mask.getHeight (); + + //cv::Mat maskImage(height, width, CV_8U, mask.mask); + //cv::erode(maskImage, maskImage + + // create distance maps for every quantization value + //cv::Mat distance_maps[8]; + //for (int map_index = 0; map_index < 8; ++map_index) + //{ + // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U); + //} + + MaskMap mask_maps[8]; + for (auto &mask_map : mask_maps) + mask_map.resize (width, height); + + unsigned char map[255]; + memset(map, 0, 255); + + map[0x1<<0] = 0; + map[0x1<<1] = 1; + map[0x1<<2] = 2; + map[0x1<<3] = 3; + map[0x1<<4] = 4; + map[0x1<<5] = 5; + map[0x1<<6] = 6; + map[0x1<<7] = 7; + + QuantizedMap distance_map_indices (width, height); + //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height); + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); + const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); + + if (quantized_value == 0) + continue; + const int dist_map_index = map[quantized_value]; + + distance_map_indices (col_index, row_index) = static_cast (dist_map_index); + //distance_maps[dist_map_index].at(row_index, col_index) = 255; + mask_maps[dist_map_index] (col_index, row_index) = 255; + } + } + } + + DistanceMap distance_maps[8]; + for (int map_index = 0; map_index < 8; ++map_index) + computeDistanceMap (mask_maps[map_index], distance_maps[map_index]); + + DistanceMap mask_distance_maps; + computeDistanceMap (mask, mask_distance_maps); + + std::list list1; + std::list list2; + + float weights[8] = {0,0,0,0,0,0,0,0}; + + const std::size_t off = 4; + for (std::size_t row_index = off; row_index < height-off; ++row_index) + { + for (std::size_t col_index = off; col_index < width-off; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); + const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); + + //const float nx = surface_normals_ (col_index, row_index).normal_x; + //const float ny = surface_normals_ (col_index, row_index).normal_y; + //const float nz = surface_normals_ (col_index, row_index).normal_z; + + if (quantized_value != 0)// && !(std::isnan (nx) || std::isnan (ny) || std::isnan (nz))) + { + const int distance_map_index = map[quantized_value]; + + //const float distance = distance_maps[distance_map_index].at (row_index, col_index); + const float distance = distance_maps[distance_map_index] (col_index, row_index); + const float distance_to_border = mask_distance_maps (col_index, row_index); + + if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_) + { + Candidate candidate; + + candidate.distance = distance; + candidate.x = col_index; + candidate.y = row_index; + candidate.bin_index = static_cast (distance_map_index); + + list1.push_back (candidate); + + ++weights[distance_map_index]; + } + } + } + } + } + + for (typename std::list::iterator iter = list1.begin (); iter != list1.end (); ++iter) + iter->distance *= 1.0f / weights[iter->bin_index]; + + list1.sort (); + + if (variable_feature_nr_) + { + int distance = static_cast (list1.size ()); + bool feature_selection_finished = false; + while (!feature_selection_finished) + { + const int sqr_distance = distance*distance; + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + bool candidate_accepted = true; + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + const int dx = static_cast (iter1->x) - static_cast (iter2->x); + const int dy = static_cast (iter1->y) - static_cast (iter2->y); + const int tmp_distance = dx*dx + dy*dy; + + if (tmp_distance < sqr_distance) + { + candidate_accepted = false; + break; + } + } + + + float min_min_sqr_distance = std::numeric_limits::max (); + float max_min_sqr_distance = 0; + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + float min_sqr_distance = std::numeric_limits::max (); + for (typename std::list::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3) + { + if (iter2 == iter3) + continue; + + const float dx = static_cast (iter2->x) - static_cast (iter3->x); + const float dy = static_cast (iter2->y) - static_cast (iter3->y); + + const float sqr_distance = dx*dx + dy*dy; + + if (sqr_distance < min_sqr_distance) + { + min_sqr_distance = sqr_distance; + } + + //std::cerr << min_sqr_distance; + } + //std::cerr << std::endl; + + // check current feature + { + const float dx = static_cast (iter2->x) - static_cast (iter1->x); + const float dy = static_cast (iter2->y) - static_cast (iter1->y); + + const float sqr_distance = dx*dx + dy*dy; + + if (sqr_distance < min_sqr_distance) + { + min_sqr_distance = sqr_distance; + } + } + + if (min_sqr_distance < min_min_sqr_distance) + min_min_sqr_distance = min_sqr_distance; + if (min_sqr_distance > max_min_sqr_distance) + max_min_sqr_distance = min_sqr_distance; + + //std::cerr << min_sqr_distance << ", " << min_min_sqr_distance << ", " << max_min_sqr_distance << std::endl; + } + + if (candidate_accepted) + { + //std::cerr << "feature_index: " << list2.size () << std::endl; + //std::cerr << "min_min_sqr_distance: " << min_min_sqr_distance << std::endl; + //std::cerr << "max_min_sqr_distance: " << max_min_sqr_distance << std::endl; + + if (min_min_sqr_distance < 50) + { + feature_selection_finished = true; + break; + } + + list2.push_back (*iter1); + } + + //if (list2.size () == nr_features) + //{ + // feature_selection_finished = true; + // break; + //} + } + --distance; + } + } + else + { + if (list1.size () <= nr_features) + { + features.reserve (list1.size ()); + for (typename std::list::iterator iter = list1.begin (); iter != list1.end (); ++iter) + { + QuantizedMultiModFeature feature; + + feature.x = static_cast (iter->x); + feature.y = static_cast (iter->y); + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y); + + features.push_back (feature); + } + + return; + } + + int distance = static_cast (list1.size () / nr_features + 1); // ??? @todo:!:!:!:!:!:! + while (list2.size () != nr_features) + { + const int sqr_distance = distance*distance; + for (typename std::list::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1) + { + bool candidate_accepted = true; + + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + const int dx = static_cast (iter1->x) - static_cast (iter2->x); + const int dy = static_cast (iter1->y) - static_cast (iter2->y); + const int tmp_distance = dx*dx + dy*dy; + + if (tmp_distance < sqr_distance) + { + candidate_accepted = false; + break; + } + } + + if (candidate_accepted) + list2.push_back (*iter1); + + if (list2.size () == nr_features) break; + } + --distance; + } + } + + for (typename std::list::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2) + { + QuantizedMultiModFeature feature; + + feature.x = static_cast (iter2->x); + feature.y = static_cast (iter2->y); + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y); + + features.push_back (feature); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::extractAllFeatures ( + const MaskMap & mask, const std::size_t, const std::size_t modality_index, + std::vector & features) const +{ + const std::size_t width = mask.getWidth (); + const std::size_t height = mask.getHeight (); + + //cv::Mat maskImage(height, width, CV_8U, mask.mask); + //cv::erode(maskImage, maskImage + + // create distance maps for every quantization value + //cv::Mat distance_maps[8]; + //for (int map_index = 0; map_index < 8; ++map_index) + //{ + // distance_maps[map_index] = ::cv::Mat::zeros(height, width, CV_8U); + //} + + MaskMap mask_maps[8]; + for (auto &mask_map : mask_maps) + mask_map.resize (width, height); + + unsigned char map[255]; + memset(map, 0, 255); + + map[0x1<<0] = 0; + map[0x1<<1] = 1; + map[0x1<<2] = 2; + map[0x1<<3] = 3; + map[0x1<<4] = 4; + map[0x1<<5] = 5; + map[0x1<<6] = 6; + map[0x1<<7] = 7; + + QuantizedMap distance_map_indices (width, height); + //memset (distance_map_indices.data, 0, sizeof (distance_map_indices.data[0])*width*height); + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); + const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); + + if (quantized_value == 0) + continue; + const int dist_map_index = map[quantized_value]; + + distance_map_indices (col_index, row_index) = static_cast (dist_map_index); + //distance_maps[dist_map_index].at(row_index, col_index) = 255; + mask_maps[dist_map_index] (col_index, row_index) = 255; + } + } + } + + DistanceMap distance_maps[8]; + for (int map_index = 0; map_index < 8; ++map_index) + computeDistanceMap (mask_maps[map_index], distance_maps[map_index]); + + DistanceMap mask_distance_maps; + computeDistanceMap (mask, mask_distance_maps); + + std::list list1; + std::list list2; + + float weights[8] = {0,0,0,0,0,0,0,0}; + + const std::size_t off = 4; + for (std::size_t row_index = off; row_index < height-off; ++row_index) + { + for (std::size_t col_index = off; col_index < width-off; ++col_index) + { + if (mask (col_index, row_index) != 0) + { + //const unsigned char quantized_value = quantized_surface_normals_ (row_index, col_index); + const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index); + + //const float nx = surface_normals_ (col_index, row_index).normal_x; + //const float ny = surface_normals_ (col_index, row_index).normal_y; + //const float nz = surface_normals_ (col_index, row_index).normal_z; + + if (quantized_value != 0)// && !(std::isnan (nx) || std::isnan (ny) || std::isnan (nz))) + { + const int distance_map_index = map[quantized_value]; + + //const float distance = distance_maps[distance_map_index].at (row_index, col_index); + const float distance = distance_maps[distance_map_index] (col_index, row_index); + const float distance_to_border = mask_distance_maps (col_index, row_index); + + if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_) + { + Candidate candidate; + + candidate.distance = distance; + candidate.x = col_index; + candidate.y = row_index; + candidate.bin_index = static_cast (distance_map_index); + + list1.push_back (candidate); + + ++weights[distance_map_index]; + } + } + } + } + } + + for (typename std::list::iterator iter = list1.begin (); iter != list1.end (); ++iter) + iter->distance *= 1.0f / weights[iter->bin_index]; + + list1.sort (); + + features.reserve (list1.size ()); + for (typename std::list::iterator iter = list1.begin (); iter != list1.end (); ++iter) + { + QuantizedMultiModFeature feature; + + feature.x = static_cast (iter->x); + feature.y = static_cast (iter->y); + feature.modality_index = modality_index; + feature.quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y); + + features.push_back (feature); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::quantizeSurfaceNormals () +{ + const std::size_t width = input_->width; + const std::size_t height = input_->height; + + quantized_surface_normals_.resize (width, height); + + for (std::size_t row_index = 0; row_index < height; ++row_index) + { + for (std::size_t col_index = 0; col_index < width; ++col_index) + { + const float normal_x = surface_normals_ (col_index, row_index).normal_x; + const float normal_y = surface_normals_ (col_index, row_index).normal_y; + const float normal_z = surface_normals_ (col_index, row_index).normal_z; + + if (std::isnan(normal_x) || std::isnan(normal_y) || std::isnan(normal_z) || normal_z > 0) + { + quantized_surface_normals_ (col_index, row_index) = 0; + continue; + } + + //quantized_surface_normals_.data[row_index*width+col_index] = + // normal_lookup_(normal_x, normal_y, normal_z); + + float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f; + + if (angle < 0.0f) angle += 360.0f; + if (angle >= 360.0f) angle -= 360.0f; + + int bin_index = static_cast (angle*8.0f/360.0f); + + //quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << bin_index; + quantized_surface_normals_ (col_index, row_index) = static_cast (bin_index); + } + } + + return; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::filterQuantizedSurfaceNormals () +{ + const int width = input_->width; + const int height = input_->height; + + filtered_quantized_surface_normals_.resize (width, height); + + //for (int row_index = 2; row_index < height-2; ++row_index) + //{ + // for (int col_index = 2; col_index < width-2; ++col_index) + // { + // std::list values; + // values.reserve (25); + + // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2; + // values.push_back (dataPtr[0]); + // values.push_back (dataPtr[1]); + // values.push_back (dataPtr[2]); + // values.push_back (dataPtr[3]); + // values.push_back (dataPtr[4]); + // dataPtr += width; + // values.push_back (dataPtr[0]); + // values.push_back (dataPtr[1]); + // values.push_back (dataPtr[2]); + // values.push_back (dataPtr[3]); + // values.push_back (dataPtr[4]); + // dataPtr += width; + // values.push_back (dataPtr[0]); + // values.push_back (dataPtr[1]); + // values.push_back (dataPtr[2]); + // values.push_back (dataPtr[3]); + // values.push_back (dataPtr[4]); + // dataPtr += width; + // values.push_back (dataPtr[0]); + // values.push_back (dataPtr[1]); + // values.push_back (dataPtr[2]); + // values.push_back (dataPtr[3]); + // values.push_back (dataPtr[4]); + // dataPtr += width; + // values.push_back (dataPtr[0]); + // values.push_back (dataPtr[1]); + // values.push_back (dataPtr[2]); + // values.push_back (dataPtr[3]); + // values.push_back (dataPtr[4]); + + // values.sort (); + + // filtered_quantized_surface_normals_ (col_index, row_index) = values[12]; + // } + //} + + + //for (int row_index = 2; row_index < height-2; ++row_index) + //{ + // for (int col_index = 2; col_index < width-2; ++col_index) + // { + // filtered_quantized_surface_normals_ (col_index, row_index) = static_cast (0x1 << (quantized_surface_normals_ (col_index, row_index) - 1)); + // } + //} + + + // filter data + for (int row_index = 2; row_index < height-2; ++row_index) + { + for (int col_index = 2; col_index < width-2; ++col_index) + { + unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0}; + + //{ + // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-1; + // ++histogram[dataPtr[0]]; + // ++histogram[dataPtr[1]]; + // ++histogram[dataPtr[2]]; + //} + //{ + // unsigned char * dataPtr = quantized_surface_normals_.getData () + row_index*width+col_index-1; + // ++histogram[dataPtr[0]]; + // ++histogram[dataPtr[1]]; + // ++histogram[dataPtr[2]]; + //} + //{ + // unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-1; + // ++histogram[dataPtr[0]]; + // ++histogram[dataPtr[1]]; + // ++histogram[dataPtr[2]]; + //} + + { + unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2; + ++histogram[dataPtr[0]]; + ++histogram[dataPtr[1]]; + ++histogram[dataPtr[2]]; + ++histogram[dataPtr[3]]; + ++histogram[dataPtr[4]]; + } + { + unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2; + ++histogram[dataPtr[0]]; + ++histogram[dataPtr[1]]; + ++histogram[dataPtr[2]]; + ++histogram[dataPtr[3]]; + ++histogram[dataPtr[4]]; + } + { + unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2; + ++histogram[dataPtr[0]]; + ++histogram[dataPtr[1]]; + ++histogram[dataPtr[2]]; + ++histogram[dataPtr[3]]; + ++histogram[dataPtr[4]]; + } + { + unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2; + ++histogram[dataPtr[0]]; + ++histogram[dataPtr[1]]; + ++histogram[dataPtr[2]]; + ++histogram[dataPtr[3]]; + ++histogram[dataPtr[4]]; + } + { + unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2; + ++histogram[dataPtr[0]]; + ++histogram[dataPtr[1]]; + ++histogram[dataPtr[2]]; + ++histogram[dataPtr[3]]; + ++histogram[dataPtr[4]]; + } + + + unsigned char max_hist_value = 0; + int max_hist_index = -1; + + if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];} + if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];} + if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];} + if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];} + if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];} + if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];} + if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];} + if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];} + + if (max_hist_index != -1 && max_hist_value >= 1) + { + filtered_quantized_surface_normals_ (col_index, row_index) = static_cast (0x1 << max_hist_index); + } + else + { + filtered_quantized_surface_normals_ (col_index, row_index) = 0; + } + + //filtered_quantized_color_gradients_.data[row_index*width+col_index] = quantized_color_gradients_.data[row_index*width+col_index]; + } + } + + + + //cv::Mat data1(quantized_surface_normals_.height, quantized_surface_normals_.width, CV_8U, quantized_surface_normals_.data); + //cv::Mat data2(filtered_quantized_surface_normals_.height, filtered_quantized_surface_normals_.width, CV_8U, filtered_quantized_surface_normals_.data); + + //cv::medianBlur(data1, data2, 3); + + //for (int row_index = 0; row_index < height; ++row_index) + //{ + // for (int col_index = 0; col_index < width; ++col_index) + // { + // filtered_quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << filtered_quantized_surface_normals_.data[row_index*width+col_index]; + // } + //} +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceNormalModality::computeDistanceMap (const MaskMap & input, DistanceMap & output) const +{ + const std::size_t width = input.getWidth (); + const std::size_t height = input.getHeight (); + + output.resize (width, height); + + // compute distance map + //float *distance_map = new float[input_->points.size ()]; + const unsigned char * mask_map = input.getData (); + float * distance_map = output.getData (); + for (std::size_t index = 0; index < width*height; ++index) + { + if (mask_map[index] == 0) + distance_map[index] = 0.0f; + else + distance_map[index] = static_cast (width + height); + } + + // first pass + float * previous_row = distance_map; + float * current_row = previous_row + width; + for (std::size_t ri = 1; ri < height; ++ri) + { + for (std::size_t ci = 1; ci < width; ++ci) + { + const float up_left = previous_row [ci - 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci-1] + 1.4f; + const float up = previous_row [ci] + 1.0f; //distance_map[(ri-1)*input_->width + ci] + 1.0f; + const float up_right = previous_row [ci + 1] + 1.4f; //distance_map[(ri-1)*input_->width + ci+1] + 1.4f; + const float left = current_row [ci - 1] + 1.0f; //distance_map[ri*input_->width + ci-1] + 1.0f; + const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; + + const float min_value = std::min (std::min (up_left, up), std::min (left, up_right)); + + if (min_value < center) + current_row[ci] = min_value; //distance_map[ri * input_->width + ci] = min_value; + } + previous_row = current_row; + current_row += width; + } + + // second pass + float * next_row = distance_map + width * (height - 1); + current_row = next_row - width; + for (int ri = static_cast (height)-2; ri >= 0; --ri) + { + for (int ci = static_cast (width)-2; ci >= 0; --ci) + { + const float lower_left = next_row [ci - 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci-1] + 1.4f; + const float lower = next_row [ci] + 1.0f; //distance_map[(ri+1)*input_->width + ci] + 1.0f; + const float lower_right = next_row [ci + 1] + 1.4f; //distance_map[(ri+1)*input_->width + ci+1] + 1.4f; + const float right = current_row [ci + 1] + 1.0f; //distance_map[ri*input_->width + ci+1] + 1.0f; + const float center = current_row [ci]; //distance_map[ri*input_->width + ci]; + + const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right)); + + if (min_value < center) + current_row[ci] = min_value; //distance_map[ri*input_->width + ci] = min_value; + } + next_row = current_row; + current_row -= width; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/recognition/trimmed_icp.h b/deps_install/include/pcl-1.10/pcl/recognition/trimmed_icp.h new file mode 100755 index 0000000..de41b11 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/trimmed_icp.h @@ -0,0 +1,184 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +/* + * trimmed_icp.h + * + * Created on: Mar 10, 2013 + * Author: papazov + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace recognition + { + template + class PCL_EXPORTS TrimmedICP: public pcl::registration::TransformationEstimationSVD + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using Matrix4 = typename Eigen::Matrix; + + public: + TrimmedICP () + : new_to_old_energy_ratio_ (0.99f) + {} + + ~TrimmedICP () + {} + + /** \brief Call this method before calling align(). + * + * \param[in] target is target point cloud. The method builds a kd-tree based on 'target' for performing fast closest point search. + * The source point cloud will be registered to 'target' (see align() method). + * */ + inline void + init (const PointCloudConstPtr& target) + { + target_points_ = target; + kdtree_.setInputCloud (target); + } + + /** \brief The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the init() method). + * + * \param[in] source_points is the point cloud to be registered to the target. + * \param[in] num_source_points_to_use gives the number of closest source points taken into account for registration. By closest + * source points we mean the source points closest to the target. These points are computed anew at each iteration. + * \param[in,out] guess_and_result is the estimated rigid transform. IMPORTANT: this matrix is also taken as the initial guess + * for the alignment. If there is no guess, set the matrix to identity! + * */ + inline void + align (const PointCloud& source_points, int num_source_points_to_use, Matrix4& guess_and_result) const + { + int num_trimmed_source_points = num_source_points_to_use, num_source_points = static_cast (source_points.size ()); + + if ( num_trimmed_source_points >= num_source_points ) + { + printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to " + "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set " + "the number of source points to use to a lower value or use standard ICP.\n", __func__); + num_trimmed_source_points = num_source_points; + } + + // These are vectors containing source to target correspondences + pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points); + + // Some variables for the closest point search + pcl::PointXYZ transformed_source_point; + std::vector target_index (1); + std::vector sqr_dist_to_target (1); + float old_energy, energy = std::numeric_limits::max (); + +// printf ("\nalign\n"); + + do + { + // Update the correspondences + for ( int i = 0 ; i < num_source_points ; ++i ) + { + // Transform the i-th source point based on the current transform matrix + aux::transform (guess_and_result, source_points.points[i], transformed_source_point); + + // Perform the closest point search + kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target); + + // Update the i-th correspondence + full_src_to_tgt[i].index_query = i; + full_src_to_tgt[i].index_match = target_index[0]; + full_src_to_tgt[i].distance = sqr_dist_to_target[0]; + } + + // Sort in ascending order according to the squared distance + std::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences); + + old_energy = energy; + energy = 0.0f; + + // Now, setup the trimmed correspondences used for the transform estimation + for ( int i = 0 ; i < num_trimmed_source_points ; ++i ) + { + trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query; + trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match; + energy += full_src_to_tgt[i].distance; + } + + this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result); + +// printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy); + } + while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress + +// printf ("\n"); + } + + inline void + setNewToOldEnergyRatio (float ratio) + { + if ( ratio >= 1 ) + new_to_old_energy_ratio_ = 0.99f; + else + new_to_old_energy_ratio_ = ratio; + } + + protected: + static inline bool + compareCorrespondences (const pcl::Correspondence& a, const pcl::Correspondence& b) + { + return a.distance < b.distance; + } + + protected: + PointCloudConstPtr target_points_; + pcl::KdTreeFLANN kdtree_; + float new_to_old_energy_ratio_; + }; + } // namespace recognition +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/recognition/voxel_structure.h b/deps_install/include/pcl-1.10/pcl/recognition/voxel_structure.h new file mode 100755 index 0000000..026a59b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/recognition/voxel_structure.h @@ -0,0 +1,168 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace recognition + { + /** \brief This class is a box in R3 built of voxels ordered in a regular rectangular grid. Each voxel is of type T. */ + template + class VoxelStructure + { + public: + inline VoxelStructure (): voxels_(nullptr){} + inline virtual ~VoxelStructure (){ this->clear();} + + /** \brief Call this method before using an instance of this class. Parameter meaning is obvious. */ + inline void + build (const REAL bounds[6], int num_of_voxels[3]); + + /** \brief Release the memory allocated for the voxels. */ + inline void + clear (){ if ( voxels_ ){ delete[] voxels_; voxels_ = nullptr;}} + + /** \brief Returns a pointer to the voxel which contains p or NULL if p is not inside the structure. */ + inline T* + getVoxel (const REAL p[3]); + + /** \brief Returns a pointer to the voxel with integer id (x,y,z) or NULL if (x,y,z) is out of bounds. */ + inline T* + getVoxel (int x, int y, int z) const; + + /** \brief Returns the linear voxel array. */ + const inline T* + getVoxels () const + { + return voxels_; + } + + /** \brief Returns the linear voxel array. */ + inline T* + getVoxels () + { + return voxels_; + } + + /** \brief Converts a linear id to a 3D id, i.e., computes the integer 3D coordinates of a voxel from its position in the voxel array. + * + * \param[in] linear_id the position of the voxel in the internal voxel array. + * \param[out] id3d an array of 3 integers for the integer coordinates of the voxel. */ + inline void + compute3dId (int linear_id, int id3d[3]) const + { + // Compute the z axis id + id3d[2] = linear_id / num_of_voxels_xy_plane_; + int proj_xy = linear_id % num_of_voxels_xy_plane_; + // Compute the y axis id + id3d[1] = proj_xy / num_of_voxels_[0]; + // Compute the x axis id + id3d[0] = proj_xy % num_of_voxels_[0]; + } + + /** \brief Returns the number of voxels in x, y and z direction. */ + inline const int* + getNumberOfVoxelsXYZ() const + { + return (num_of_voxels_); + } + + /** \brief Computes the center of the voxel with given integer coordinates. + * + * \param[in] id3 the integer coordinates along the x, y and z axis. + * \param[out] center */ + inline void + computeVoxelCenter (const int id3[3], REAL center[3]) const + { + center[0] = min_center_[0] + static_cast (id3[0])*spacing_[0]; + center[1] = min_center_[1] + static_cast (id3[1])*spacing_[1]; + center[2] = min_center_[2] + static_cast (id3[2])*spacing_[2]; + } + + /** \brief Returns the total number of voxels. */ + inline int + getNumberOfVoxels() const + { + return (total_num_of_voxels_); + } + + /** \brief Returns the bounds of the voxel structure, which is pointer to the internal array of 6 doubles: (min_x, max_x, min_y, max_y, min_z, max_z). */ + inline const float* + getBounds() const + { + return (bounds_); + } + + /** \brief Copies the bounds of the voxel structure to 'b'. */ + inline void + getBounds(REAL b[6]) const + { + b[0] = bounds_[0]; + b[1] = bounds_[1]; + b[2] = bounds_[2]; + b[3] = bounds_[3]; + b[4] = bounds_[4]; + b[5] = bounds_[5]; + } + + /** \brief Returns the voxel spacing in x, y and z direction. That's the same as the voxel size along each axis. */ + const REAL* + getVoxelSpacing() const + { + return (spacing_); + } + + /** \brief Saves pointers to the voxels which are neighbors of the voxels which contains 'p'. The containing voxel is returned too. + * 'neighs' has to be an array of pointers with space for at least 27 pointers (27 = 3^3 which is the max number of neighbors). The */ + inline int + getNeighbors (const REAL* p, T **neighs) const; + + protected: + T *voxels_; + int num_of_voxels_[3], num_of_voxels_xy_plane_, total_num_of_voxels_; + REAL bounds_[6]; + REAL spacing_[3]; // spacing between the voxel in x, y and z direction = voxel size in x, y and z direction + REAL min_center_[3]; // the center of the voxel with integer coordinates (0, 0, 0) + }; + } // namespace recognition +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/register_point_struct.h b/deps_install/include/pcl-1.10/pcl/register_point_struct.h new file mode 100755 index 0000000..32ac7a0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/register_point_struct.h @@ -0,0 +1,360 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#if defined _MSC_VER + #pragma warning (push, 2) + // 4244 : conversion from 'type1' to 'type2', possible loss of data + #pragma warning (disable: 4244) +#endif + +//https://bugreports.qt-project.org/browse/QTBUG-22829 +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include //offsetof +#include + +// Must be used in global namespace with name fully qualified +#define POINT_CLOUD_REGISTER_POINT_STRUCT(name, fseq) \ + POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, \ + BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0)) + /***/ + +#define POINT_CLOUD_REGISTER_POINT_WRAPPER(wrapper, pod) \ + BOOST_MPL_ASSERT_MSG(sizeof(wrapper) == sizeof(pod), POINT_WRAPPER_AND_POD_TYPES_HAVE_DIFFERENT_SIZES, (wrapper&, pod&)); \ + namespace pcl { \ + namespace traits { \ + template<> struct POD { using type = pod; }; \ + } \ + } + /***/ + +// These macros help transform the unusual data structure (type, name, tag)(type, name, tag)... +// into a proper preprocessor sequence of 3-tuples ((type, name, tag))((type, name, tag))... +#define POINT_CLOUD_REGISTER_POINT_STRUCT_X(type, name, tag) \ + ((type, name, tag)) POINT_CLOUD_REGISTER_POINT_STRUCT_Y +#define POINT_CLOUD_REGISTER_POINT_STRUCT_Y(type, name, tag) \ + ((type, name, tag)) POINT_CLOUD_REGISTER_POINT_STRUCT_X +#define POINT_CLOUD_REGISTER_POINT_STRUCT_X0 +#define POINT_CLOUD_REGISTER_POINT_STRUCT_Y0 + +namespace pcl +{ + namespace traits + { + template inline + std::enable_if_t::value> + plus (T &l, const T &r) + { + l += r; + } + + template inline + std::enable_if_t::value> + plus (std::remove_const_t &l, const T &r) + { + using type = std::remove_all_extents_t; + static const std::uint32_t count = sizeof (T) / sizeof (type); + for (std::uint32_t i = 0; i < count; ++i) + l[i] += r[i]; + } + + template inline + std::enable_if_t::value> + plusscalar (T1 &p, const T2 &scalar) + { + p += scalar; + } + + template inline + std::enable_if_t::value> + plusscalar (T1 &p, const T2 &scalar) + { + using type = std::remove_all_extents_t; + static const std::uint32_t count = sizeof (T1) / sizeof (type); + for (std::uint32_t i = 0; i < count; ++i) + p[i] += scalar; + } + + template inline + std::enable_if_t::value> + minus (T &l, const T &r) + { + l -= r; + } + + template inline + std::enable_if_t::value> + minus (std::remove_const_t &l, const T &r) + { + using type = std::remove_all_extents_t; + static const std::uint32_t count = sizeof (T) / sizeof (type); + for (std::uint32_t i = 0; i < count; ++i) + l[i] -= r[i]; + } + + template inline + std::enable_if_t::value> + minusscalar (T1 &p, const T2 &scalar) + { + p -= scalar; + } + + template inline + std::enable_if_t::value> + minusscalar (T1 &p, const T2 &scalar) + { + using type = std::remove_all_extents_t; + static const std::uint32_t count = sizeof (T1) / sizeof (type); + for (std::uint32_t i = 0; i < count; ++i) + p[i] -= scalar; + } + + template inline + std::enable_if_t::value> + mulscalar (T1 &p, const T2 &scalar) + { + p *= scalar; + } + + template inline + std::enable_if_t::value> + mulscalar (T1 &p, const T2 &scalar) + { + using type = std::remove_all_extents_t; + static const std::uint32_t count = sizeof (T1) / sizeof (type); + for (std::uint32_t i = 0; i < count; ++i) + p[i] *= scalar; + } + + template inline + std::enable_if_t::value> + divscalar (T1 &p, const T2 &scalar) + { + p /= scalar; + } + + template inline + std::enable_if_t::value> + divscalar (T1 &p, const T2 &scalar) + { + using type = std::remove_all_extents_t; + static const std::uint32_t count = sizeof (T1) / sizeof (type); + for (std::uint32_t i = 0; i < count; ++i) + p[i] /= scalar; + } + } +} + +// Point operators +#define PCL_PLUSEQ_POINT_TAG(r, data, elem) \ + pcl::traits::plus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ + rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); + /***/ + +#define PCL_PLUSEQSC_POINT_TAG(r, data, elem) \ + pcl::traits::plusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ + scalar); + /***/ + //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) += scalar; + +#define PCL_MINUSEQ_POINT_TAG(r, data, elem) \ + pcl::traits::minus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ + rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); + /***/ + +#define PCL_MINUSEQSC_POINT_TAG(r, data, elem) \ + pcl::traits::minusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ + scalar); + /***/ + //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) -= scalar; + +#define PCL_MULEQSC_POINT_TAG(r, data, elem) \ + pcl::traits::mulscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ + scalar); + /***/ + +#define PCL_DIVEQSC_POINT_TAG(r, data, elem) \ + pcl::traits::divscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ + scalar); + /***/ + +// Construct type traits given full sequence of (type, name, tag) triples +// BOOST_MPL_ASSERT_MSG(std::is_pod::value, +// REGISTERED_POINT_TYPE_MUST_BE_PLAIN_OLD_DATA, (name)); +#define POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, seq) \ + namespace pcl \ + { \ + namespace fields \ + { \ + BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_TAG, name, seq) \ + } \ + namespace traits \ + { \ + BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_NAME, name, seq) \ + BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_OFFSET, name, seq) \ + BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_DATATYPE, name, seq) \ + POINT_CLOUD_REGISTER_POINT_FIELD_LIST(name, POINT_CLOUD_EXTRACT_TAGS(seq)) \ + } \ + namespace common \ + { \ + inline const name& \ + operator+= (name& lhs, const name& rhs) \ + { \ + BOOST_PP_SEQ_FOR_EACH(PCL_PLUSEQ_POINT_TAG, _, seq) \ + return (lhs); \ + } \ + inline const name& \ + operator+= (name& p, const float& scalar) \ + { \ + BOOST_PP_SEQ_FOR_EACH(PCL_PLUSEQSC_POINT_TAG, _, seq) \ + return (p); \ + } \ + inline const name operator+ (const name& lhs, const name& rhs) \ + { name result = lhs; result += rhs; return (result); } \ + inline const name operator+ (const float& scalar, const name& p) \ + { name result = p; result += scalar; return (result); } \ + inline const name operator+ (const name& p, const float& scalar) \ + { name result = p; result += scalar; return (result); } \ + inline const name& \ + operator-= (name& lhs, const name& rhs) \ + { \ + BOOST_PP_SEQ_FOR_EACH(PCL_MINUSEQ_POINT_TAG, _, seq) \ + return (lhs); \ + } \ + inline const name& \ + operator-= (name& p, const float& scalar) \ + { \ + BOOST_PP_SEQ_FOR_EACH(PCL_MINUSEQSC_POINT_TAG, _, seq) \ + return (p); \ + } \ + inline const name operator- (const name& lhs, const name& rhs) \ + { name result = lhs; result -= rhs; return (result); } \ + inline const name operator- (const float& scalar, const name& p) \ + { name result = p; result -= scalar; return (result); } \ + inline const name operator- (const name& p, const float& scalar) \ + { name result = p; result -= scalar; return (result); } \ + inline const name& \ + operator*= (name& p, const float& scalar) \ + { \ + BOOST_PP_SEQ_FOR_EACH(PCL_MULEQSC_POINT_TAG, _, seq) \ + return (p); \ + } \ + inline const name operator* (const float& scalar, const name& p) \ + { name result = p; result *= scalar; return (result); } \ + inline const name operator* (const name& p, const float& scalar) \ + { name result = p; result *= scalar; return (result); } \ + inline const name& \ + operator/= (name& p, const float& scalar) \ + { \ + BOOST_PP_SEQ_FOR_EACH(PCL_DIVEQSC_POINT_TAG, _, seq) \ + return (p); \ + } \ + inline const name operator/ (const float& scalar, const name& p) \ + { name result = p; result /= scalar; return (result); } \ + inline const name operator/ (const name& p, const float& scalar) \ + { name result = p; result /= scalar; return (result); } \ + } \ + } + /***/ + +#define POINT_CLOUD_REGISTER_FIELD_TAG(r, name, elem) \ + struct BOOST_PP_TUPLE_ELEM(3, 2, elem); \ + /***/ + +#define POINT_CLOUD_REGISTER_FIELD_NAME(r, point, elem) \ + template \ + struct name \ + { \ + static const char value[]; \ + }; \ + \ + template \ + const char name::value[] = \ + BOOST_PP_STRINGIZE(BOOST_PP_TUPLE_ELEM(3, 2, elem)); + /***/ + +#define POINT_CLOUD_REGISTER_FIELD_OFFSET(r, name, elem) \ + template<> struct offset \ + { \ + static const std::size_t value = offsetof(name, BOOST_PP_TUPLE_ELEM(3, 1, elem)); \ + }; + /***/ + +#define POINT_CLOUD_REGISTER_FIELD_DATATYPE(r, name, elem) \ + template<> struct datatype \ + { \ + using type = boost::mpl::identity::type; \ + using decomposed = decomposeArray; \ + static const std::uint8_t value = asEnum::value; \ + static const std::uint32_t size = decomposed::value; \ + }; + /***/ + +#define POINT_CLOUD_TAG_OP(s, data, elem) pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem) + +#define POINT_CLOUD_EXTRACT_TAGS(seq) BOOST_PP_SEQ_TRANSFORM(POINT_CLOUD_TAG_OP, _, seq) + +#define POINT_CLOUD_REGISTER_POINT_FIELD_LIST(name, seq) \ + template<> struct fieldList \ + { \ + using type = boost::mpl::vector; \ + }; + /***/ + +#if defined _MSC_VER + #pragma warning (pop) +#endif diff --git a/deps_install/include/pcl-1.10/pcl/registration/bfgs.h b/deps_install/include/pcl-1.10/pcl/registration/bfgs.h new file mode 100755 index 0000000..d6b3074 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/bfgs.h @@ -0,0 +1,615 @@ +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include + +namespace Eigen +{ + template< typename _Scalar > + class PolynomialSolver<_Scalar,2> : public PolynomialSolverBase<_Scalar,2> + { + public: + using PS_Base = PolynomialSolverBase<_Scalar,2>; + EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base ) + + public: + + virtual ~PolynomialSolver () {} + + template< typename OtherPolynomial > + inline PolynomialSolver( const OtherPolynomial& poly, bool& hasRealRoot ) + { + compute( poly, hasRealRoot ); + } + + /** Computes the complex roots of a new polynomial. */ + template< typename OtherPolynomial > + void compute( const OtherPolynomial& poly, bool& hasRealRoot) + { + const Scalar ZERO(0); + Scalar a2(2 * poly[2]); + assert( ZERO != poly[poly.size()-1] ); + Scalar discriminant ((poly[1] * poly[1]) - (4 * poly[0] * poly[2])); + if (ZERO < discriminant) + { + Scalar discriminant_root (std::sqrt (discriminant)); + m_roots[0] = (-poly[1] - discriminant_root) / (a2) ; + m_roots[1] = (-poly[1] + discriminant_root) / (a2) ; + hasRealRoot = true; + } + else { + if (ZERO == discriminant) + { + m_roots.resize (1); + m_roots[0] = -poly[1] / a2; + hasRealRoot = true; + } + else + { + Scalar discriminant_root (std::sqrt (-discriminant)); + m_roots[0] = RootType (-poly[1] / a2, -discriminant_root / a2); + m_roots[1] = RootType (-poly[1] / a2, discriminant_root / a2); + hasRealRoot = false; + } + } + } + + template< typename OtherPolynomial > + void compute( const OtherPolynomial& poly) + { + bool hasRealRoot; + compute(poly, hasRealRoot); + } + + protected: + using PS_Base::m_roots; + }; +} + +template +struct BFGSDummyFunctor +{ + using Scalar = _Scalar; + enum { InputsAtCompileTime = NX }; + using VectorType = Eigen::Matrix; + + const int m_inputs; + + BFGSDummyFunctor() : m_inputs(InputsAtCompileTime) {} + BFGSDummyFunctor(int inputs) : m_inputs(inputs) {} + + virtual ~BFGSDummyFunctor() {} + int inputs() const { return m_inputs; } + + virtual double operator() (const VectorType &x) = 0; + virtual void df(const VectorType &x, VectorType &df) = 0; + virtual void fdf(const VectorType &x, Scalar &f, VectorType &df) = 0; +}; + +namespace BFGSSpace { + enum Status { + NegativeGradientEpsilon = -3, + NotStarted = -2, + Running = -1, + Success = 0, + NoProgress = 1 + }; +} + +/** + * BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving + * unconstrained nonlinear optimization problems. + * For further details please visit: http://en.wikipedia.org/wiki/BFGS_method + * The method provided here is almost similar to the one provided by GSL. + * It reproduces Fletcher's original algorithm in Practical Methods of Optimization + * algorithms : 2.6.2 and 2.6.4 and uses the same politics in GSL with cubic + * interpolation whenever it is possible else falls to quadratic interpolation for + * alpha parameter. + */ +template +class BFGS +{ +public: + using Scalar = typename FunctorType::Scalar; + using FVectorType = typename FunctorType::VectorType; + + BFGS(FunctorType &_functor) + : pnorm(0), g0norm(0), iter(-1), functor(_functor) { } + + using Index = Eigen::DenseIndex; + + struct Parameters { + Parameters() + : max_iters(400) + , bracket_iters(100) + , section_iters(100) + , rho(0.01) + , sigma(0.01) + , tau1(9) + , tau2(0.05) + , tau3(0.5) + , step_size(1) + , order(3) {} + Index max_iters; // maximum number of function evaluation + Index bracket_iters; + Index section_iters; + Scalar rho; + Scalar sigma; + Scalar tau1; + Scalar tau2; + Scalar tau3; + Scalar step_size; + Index order; + }; + + BFGSSpace::Status minimize(FVectorType &x); + BFGSSpace::Status minimizeInit(FVectorType &x); + BFGSSpace::Status minimizeOneStep(FVectorType &x); + BFGSSpace::Status testGradient(Scalar epsilon); + void resetParameters(void) { parameters = Parameters(); } + + Parameters parameters; + Scalar f; + FVectorType gradient; +private: + + BFGS& operator=(const BFGS&); + BFGSSpace::Status lineSearch (Scalar rho, Scalar sigma, + Scalar tau1, Scalar tau2, Scalar tau3, + int order, Scalar alpha1, Scalar &alpha_new); + Scalar interpolate (Scalar a, Scalar fa, Scalar fpa, + Scalar b, Scalar fb, Scalar fpb, Scalar xmin, Scalar xmax, + int order); + void checkExtremum (const Eigen::Matrix& coefficients, Scalar x, Scalar& xmin, Scalar& fmin); + void moveTo (Scalar alpha); + Scalar slope (); + Scalar applyF (Scalar alpha); + Scalar applyDF (Scalar alpha); + void applyFDF (Scalar alpha, Scalar &f, Scalar &df); + void updatePosition (Scalar alpha, FVectorType& x, Scalar& f, FVectorType& g); + void changeDirection (); + + Scalar delta_f, fp0; + FVectorType x0, dx0, dg0, g0, dx, p; + Scalar pnorm, g0norm; + + Scalar f_alpha; + Scalar df_alpha; + FVectorType x_alpha; + FVectorType g_alpha; + + // cache "keys" + Scalar f_cache_key; + Scalar df_cache_key; + Scalar x_cache_key; + Scalar g_cache_key; + + Index iter; + FunctorType &functor; +}; + + +template void +BFGS::checkExtremum(const Eigen::Matrix& coefficients, Scalar x, Scalar& xmin, Scalar& fmin) +{ + Scalar y = Eigen::poly_eval(coefficients, x); + if(y < fmin) { xmin = x; fmin = y; } +} + +template void +BFGS::moveTo(Scalar alpha) +{ + x_alpha = x0 + alpha * p; + x_cache_key = alpha; +} + +template typename BFGS::Scalar +BFGS::slope() +{ + return (g_alpha.dot (p)); +} + +template typename BFGS::Scalar +BFGS::applyF(Scalar alpha) +{ + if (alpha == f_cache_key) return f_alpha; + moveTo (alpha); + f_alpha = functor (x_alpha); + f_cache_key = alpha; + return (f_alpha); +} + +template typename BFGS::Scalar +BFGS::applyDF(Scalar alpha) +{ + if (alpha == df_cache_key) return df_alpha; + moveTo (alpha); + if(alpha != g_cache_key) + { + functor.df (x_alpha, g_alpha); + g_cache_key = alpha; + } + df_alpha = slope (); + df_cache_key = alpha; + return (df_alpha); +} + +template void +BFGS::applyFDF(Scalar alpha, Scalar& f, Scalar& df) +{ + if(alpha == f_cache_key && alpha == df_cache_key) + { + f = f_alpha; + df = df_alpha; + return; + } + + if(alpha == f_cache_key || alpha == df_cache_key) + { + f = applyF (alpha); + df = applyDF (alpha); + return; + } + + moveTo (alpha); + functor.fdf (x_alpha, f_alpha, g_alpha); + f_cache_key = alpha; + g_cache_key = alpha; + df_alpha = slope (); + df_cache_key = alpha; + f = f_alpha; + df = df_alpha; +} + +template void +BFGS::updatePosition (Scalar alpha, FVectorType &x, Scalar &f, FVectorType &g) +{ + { + Scalar f_alpha, df_alpha; + applyFDF (alpha, f_alpha, df_alpha); + } ; + + f = f_alpha; + x = x_alpha; + g = g_alpha; +} + +template void +BFGS::changeDirection () +{ + x_alpha = x0; + x_cache_key = 0.0; + f_cache_key = 0.0; + g_alpha = g0; + g_cache_key = 0.0; + df_alpha = slope (); + df_cache_key = 0.0; +} + +template BFGSSpace::Status +BFGS::minimize(FVectorType &x) +{ + BFGSSpace::Status status = minimizeInit(x); + do { + status = minimizeOneStep(x); + iter++; + } while (status==BFGSSpace::Success && iter < parameters.max_iters); + return status; +} + +template BFGSSpace::Status +BFGS::minimizeInit(FVectorType &x) +{ + iter = 0; + delta_f = 0; + dx.setZero (); + functor.fdf(x, f, gradient); + x0 = x; + g0 = gradient; + g0norm = g0.norm (); + p = gradient * -1/g0norm; + pnorm = p.norm (); + fp0 = -g0norm; + + { + x_alpha = x0; x_cache_key = 0; + + f_alpha = f; f_cache_key = 0; + + g_alpha = g0; g_cache_key = 0; + + df_alpha = slope (); df_cache_key = 0; + } + + return BFGSSpace::NotStarted; +} + +template BFGSSpace::Status +BFGS::minimizeOneStep(FVectorType &x) +{ + Scalar alpha = 0.0, alpha1; + Scalar f0 = f; + if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0) + { + dx.setZero (); + return BFGSSpace::NoProgress; + } + + if (delta_f < 0) + { + Scalar del = std::max (-delta_f, 10 * std::numeric_limits::epsilon() * std::abs(f0)); + alpha1 = std::min (1.0, 2.0 * del / (-fp0)); + } + else + alpha1 = std::abs(parameters.step_size); + + BFGSSpace::Status status = lineSearch(parameters.rho, parameters.sigma, + parameters.tau1, parameters.tau2, parameters.tau3, + parameters.order, alpha1, alpha); + + if(status != BFGSSpace::Success) + return status; + + updatePosition(alpha, x, f, gradient); + + delta_f = f - f0; + + /* Choose a new direction for the next step */ + { + /* This is the BFGS update: */ + /* p' = g1 - A dx - B dg */ + /* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg */ + /* B = dx.g/dx.dg */ + + Scalar dxg, dgg, dxdg, dgnorm, A, B; + + /* dx0 = x - x0 */ + dx0 = x - x0; + dx = dx0; /* keep a copy */ + + /* dg0 = g - g0 */ + dg0 = gradient - g0; + dxg = dx0.dot (gradient); + dgg = dg0.dot (gradient); + dxdg = dx0.dot (dg0); + dgnorm = dg0.norm (); + + if (dxdg != 0) + { + B = dxg / dxdg; + A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg; + } + else + { + B = 0; + A = 0; + } + + p = -A * dx0; + p+= gradient; + p+= -B * dg0 ; + } + + g0 = gradient; + x0 = x; + g0norm = g0.norm (); + pnorm = p.norm (); + + Scalar dir = ((p.dot (gradient)) > 0) ? -1.0 : 1.0; + p*= dir / pnorm; + pnorm = p.norm (); + fp0 = p.dot (g0); + + changeDirection(); + return BFGSSpace::Success; +} + +template typename BFGSSpace::Status +BFGS::testGradient(Scalar epsilon) +{ + if(epsilon < 0) + return BFGSSpace::NegativeGradientEpsilon; + else + { + if(gradient.norm () < epsilon) + return BFGSSpace::Success; + else + return BFGSSpace::Running; + } +} + +template typename BFGS::Scalar +BFGS::interpolate (Scalar a, Scalar fa, Scalar fpa, + Scalar b, Scalar fb, Scalar fpb, + Scalar xmin, Scalar xmax, + int order) +{ + /* Map [a,b] to [0,1] */ + Scalar y, alpha, ymin, ymax, fmin; + + ymin = (xmin - a) / (b - a); + ymax = (xmax - a) / (b - a); + + // Ensure ymin <= ymax + if (ymin > ymax) { Scalar tmp = ymin; ymin = ymax; ymax = tmp; }; + + if (order > 2 && !(fpb != fpa) && fpb != std::numeric_limits::infinity ()) + { + fpa = fpa * (b - a); + fpb = fpb * (b - a); + + Scalar eta = 3 * (fb - fa) - 2 * fpa - fpb; + Scalar xi = fpa + fpb - 2 * (fb - fa); + Scalar c0 = fa, c1 = fpa, c2 = eta, c3 = xi; + Scalar y0, y1; + Eigen::Matrix coefficients; + coefficients << c0, c1, c2, c3; + + y = ymin; + // Evaluate the cubic polyinomial at ymin; + fmin = Eigen::poly_eval (coefficients, ymin); + checkExtremum (coefficients, ymax, y, fmin); + { + // Solve quadratic polynomial for the derivate + Eigen::Matrix coefficients2; + coefficients2 << c1, 2 * c2, 3 * c3; + bool real_roots; + Eigen::PolynomialSolver solver (coefficients2, real_roots); + if(real_roots) + { + if ((solver.roots ()).size () == 2) /* found 2 roots */ + { + y0 = std::real (solver.roots () [0]); + y1 = std::real (solver.roots () [1]); + if(y0 > y1) { Scalar tmp (y0); y0 = y1; y1 = tmp; } + if (y0 > ymin && y0 < ymax) + checkExtremum (coefficients, y0, y, fmin); + if (y1 > ymin && y1 < ymax) + checkExtremum (coefficients, y1, y, fmin); + } + else if ((solver.roots ()).size () == 1) /* found 1 root */ + { + y0 = std::real (solver.roots () [0]); + if (y0 > ymin && y0 < ymax) + checkExtremum (coefficients, y0, y, fmin); + } + } + } + } + else + { + fpa = fpa * (b - a); + Scalar fl = fa + ymin*(fpa + ymin*(fb - fa -fpa)); + Scalar fh = fa + ymax*(fpa + ymax*(fb - fa -fpa)); + Scalar c = 2 * (fb - fa - fpa); /* curvature */ + y = ymin; fmin = fl; + + if (fh < fmin) { y = ymax; fmin = fh; } + + if (c > a) /* positive curvature required for a minimum */ + { + Scalar z = -fpa / c; /* location of minimum */ + if (z > ymin && z < ymax) { + Scalar f = fa + z*(fpa + z*(fb - fa -fpa)); + if (f < fmin) { y = z; fmin = f; }; + } + } + } + + alpha = a + y * (b - a); + return alpha; +} + +template BFGSSpace::Status +BFGS::lineSearch(Scalar rho, Scalar sigma, + Scalar tau1, Scalar tau2, Scalar tau3, + int order, Scalar alpha1, Scalar &alpha_new) +{ + Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next; + Scalar alpha = alpha1, alpha_prev = 0.0; + Scalar a, b, fa, fb, fpa, fpb; + Index i = 0; + + applyFDF (0.0, f0, fp0); + + falpha_prev = f0; + fpalpha_prev = fp0; + + /* Avoid uninitialized variables morning */ + a = 0.0; b = alpha; + fa = f0; fb = 0.0; + fpa = fp0; fpb = 0.0; + + /* Begin bracketing */ + + while (i++ < parameters.bracket_iters) + { + falpha = applyF (alpha); + + if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev) + { + a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev; + b = alpha; fb = falpha; fpb = std::numeric_limits::quiet_NaN (); + break; + } + + fpalpha = applyDF (alpha); + + /* Fletcher's sigma test */ + if (std::abs (fpalpha) <= -sigma * fp0) + { + alpha_new = alpha; + return BFGSSpace::Success; + } + + if (fpalpha >= 0) + { + a = alpha; fa = falpha; fpa = fpalpha; + b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev; + break; /* goto sectioning */ + } + + delta = alpha - alpha_prev; + + { + Scalar lower = alpha + delta; + Scalar upper = alpha + tau1 * delta; + + alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev, + alpha, falpha, fpalpha, lower, upper, order); + + } + + alpha_prev = alpha; + falpha_prev = falpha; + fpalpha_prev = fpalpha; + alpha = alpha_next; + } + /* Sectioning of bracket [a,b] */ + while (i++ < parameters.section_iters) + { + delta = b - a; + + { + Scalar lower = a + tau2 * delta; + Scalar upper = b - tau3 * delta; + + alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order); + } + falpha = applyF (alpha); + if ((a-alpha)*fpa <= std::numeric_limits::epsilon ()) { + /* roundoff prevents progress */ + return BFGSSpace::NoProgress; + }; + + if (falpha > f0 + rho * alpha * fp0 || falpha >= fa) + { + /* a_next = a; */ + b = alpha; fb = falpha; fpb = std::numeric_limits::quiet_NaN (); + } + else + { + fpalpha = applyDF (alpha); + + if (std::abs(fpalpha) <= -sigma * fp0) + { + alpha_new = alpha; + return BFGSSpace::Success; /* terminate */ + } + + if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0)) + { + b = a; fb = fa; fpb = fpa; + a = alpha; fa = falpha; fpa = fpalpha; + } + else + { + a = alpha; fa = falpha; fpa = fpalpha; + } + } + } + return BFGSSpace::Success; +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/boost.h b/deps_install/include/pcl-1.10/pcl/registration/boost.h new file mode 100755 index 0000000..962e588 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/boost.h @@ -0,0 +1,52 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +//#include +#include +#include +#include + +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/boost_graph.h b/deps_install/include/pcl-1.10/pcl/registration/boost_graph.h new file mode 100755 index 0000000..0c18798 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/boost_graph.h @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace boost +{ + + struct eigen_vecS + { + }; + + template + struct container_gen + { + using type = std::vector >; + }; + + template <> + struct parallel_edge_traits + { + using type = allow_parallel_edge_tag; + }; + + namespace detail + { + template <> + struct is_random_access + { + enum { value = true }; + using type = mpl::true_; + }; + } + + struct eigen_listS + { + }; + + template + struct container_gen + { + using type = std::list >; + }; + + template <> + struct parallel_edge_traits + { + using type = allow_parallel_edge_tag; + }; + + namespace detail + { + template <> + struct is_random_access + { + enum { value = false }; + using type = mpl::false_; + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/convergence_criteria.h b/deps_install/include/pcl-1.10/pcl/registration/convergence_criteria.h new file mode 100755 index 0000000..efe0532 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/convergence_criteria.h @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b ConvergenceCriteria represents an abstract base class for + * different convergence criteria used in registration loops. + * + * This should be used as part of an Iterative Closest Point (ICP)-like + * method, to verify if the algorithm has reached convergence. + * + * Typical convergence criteria that could inherit from this include: + * + * * a maximum number of iterations has been reached + * * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold) + * * the Mean Squared Error (MSE) between the current set of correspondences and the previous one is smaller than some threshold + * + * \author Radu B. Rusu + * \ingroup registration + */ + class PCL_EXPORTS ConvergenceCriteria + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + ConvergenceCriteria () {} + + /** \brief Empty destructor. */ + virtual ~ConvergenceCriteria () {} + + /** \brief Check if convergence has been reached. Pure virtual. */ + virtual bool + hasConverged () = 0; + + /** \brief Bool operator. */ + operator bool () + { + return (hasConverged ()); + } + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation.h new file mode 100755 index 0000000..696804c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation.h @@ -0,0 +1,435 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include +#include +#include +#include + +#include + +namespace pcl +{ + namespace registration + { + /** \brief Abstract @b CorrespondenceEstimationBase class. + * All correspondence estimation methods should inherit from this. + * \author Radu B. Rusu + * \ingroup registration + */ + template + class CorrespondenceEstimationBase: public PCLBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + // using PCLBase::initCompute; + using PCLBase::deinitCompute; + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::setIndices; + + using KdTree = pcl::search::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using KdTreeReciprocal = pcl::search::KdTree; + using KdTreeReciprocalPtr = typename KdTree::Ptr; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr; + + /** \brief Empty constructor. */ + CorrespondenceEstimationBase () + : corr_name_ ("CorrespondenceEstimationBase") + , tree_ (new pcl::search::KdTree) + , tree_reciprocal_ (new pcl::search::KdTree) + , target_ () + , point_representation_ () + , input_transformed_ () + , target_cloud_updated_ (true) + , source_cloud_updated_ (true) + , force_no_recompute_ (false) + , force_no_recompute_reciprocal_ (false) + { + } + + /** \brief Empty destructor */ + ~CorrespondenceEstimationBase () {} + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud source + */ + inline void + setInputSource (const PointCloudSourceConstPtr &cloud) + { + source_cloud_updated_ = true; + PCLBase::setInputCloud (cloud); + input_fields_ = pcl::getFields (); + } + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudSourceConstPtr const + getInputSource () + { + return (input_ ); + } + + /** \brief Provide a pointer to the input target + * (e.g., the point cloud that we want to align the input source to) + * \param[in] cloud the input point cloud target + */ + inline void + setInputTarget (const PointCloudTargetConstPtr &cloud); + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudTargetConstPtr const + getInputTarget () { return (target_ ); } + + + /** \brief See if this rejector requires source normals */ + virtual bool + requiresSourceNormals () const + { return (false); } + + /** \brief Abstract method for setting the source normals */ + virtual void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ()); + } + + /** \brief See if this rejector requires target normals */ + virtual bool + requiresTargetNormals () const + { return (false); } + + /** \brief Abstract method for setting the target normals */ + virtual void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ()); + } + + /** \brief Provide a pointer to the vector of indices that represent the + * input source point cloud. + * \param[in] indices a pointer to the vector of indices + */ + inline void + setIndicesSource (const IndicesPtr &indices) + { + setIndices (indices); + } + + /** \brief Get a pointer to the vector of indices used for the source dataset. */ + inline IndicesPtr const + getIndicesSource () { return (indices_); } + + /** \brief Provide a pointer to the vector of indices that represent the input target point cloud. + * \param[in] indices a pointer to the vector of indices + */ + inline void + setIndicesTarget (const IndicesPtr &indices) + { + target_cloud_updated_ = true; + target_indices_ = indices; + } + + /** \brief Get a pointer to the vector of indices used for the target dataset. */ + inline IndicesPtr const + getIndicesTarget () { return (target_indices_); } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + inline void + setSearchMethodTarget (const KdTreePtr &tree, + bool force_no_recompute = false) + { + tree_ = tree; + if (force_no_recompute) + { + force_no_recompute_ = true; + } + // Since we just set a new tree, we need to check for updates + target_cloud_updated_ = true; + } + + /** \brief Get a pointer to the search method used to find correspondences in the + * target cloud. */ + inline KdTreePtr + getSearchMethodTarget () const + { + return (tree_); + } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the source cloud (usually used by reciprocal correspondence finding). + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputSource. Only use if you are + * extremely confident that the tree will be set correctly. + */ + inline void + setSearchMethodSource (const KdTreeReciprocalPtr &tree, + bool force_no_recompute = false) + { + tree_reciprocal_ = tree; + if ( force_no_recompute ) + { + force_no_recompute_reciprocal_ = true; + } + // Since we just set a new tree, we need to check for updates + source_cloud_updated_ = true; + } + + /** \brief Get a pointer to the search method used to find correspondences in the + * source cloud. */ + inline KdTreeReciprocalPtr + getSearchMethodSource () const + { + return (tree_reciprocal_); + } + + /** \brief Determine the correspondences between input and target cloud. + * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) + * \param[in] max_distance maximum allowed distance between correspondences + */ + virtual void + determineCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()) = 0; + + /** \brief Determine the reciprocal correspondences between input and target cloud. + * A correspondence is considered reciprocal if both Src_i has Tgt_i as a + * correspondence, and Tgt_i has Src_i as one. + * + * \param[out] correspondences the found correspondences (index of query and target point, distance) + * \param[in] max_distance maximum allowed distance between correspondences + */ + virtual void + determineReciprocalCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()) = 0; + + /** \brief Provide a boost shared pointer to the PointRepresentation to be used + * when searching for nearest neighbors. + * + * \param[in] point_representation the PointRepresentation to be used by the + * k-D tree for nearest neighbor search + */ + inline void + setPointRepresentation (const PointRepresentationConstPtr &point_representation) + { + point_representation_ = point_representation; + } + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual typename CorrespondenceEstimationBase::Ptr clone () const = 0; + + protected: + /** \brief The correspondence estimation method name. */ + std::string corr_name_; + + /** \brief A pointer to the spatial search object used for the target dataset. */ + KdTreePtr tree_; + + /** \brief A pointer to the spatial search object used for the source dataset. */ + KdTreeReciprocalPtr tree_reciprocal_; + + + + /** \brief The input point cloud dataset target. */ + PointCloudTargetConstPtr target_; + + /** \brief The target point cloud dataset indices. */ + IndicesPtr target_indices_; + + /** \brief The point representation used (internal). */ + PointRepresentationConstPtr point_representation_; + + /** \brief The transformed input source point cloud dataset. */ + PointCloudTargetPtr input_transformed_; + + /** \brief The types of input point fields available. */ + std::vector input_fields_; + + /** \brief Abstract class get name method. */ + inline const std::string& + getClassName () const { return (corr_name_); } + + /** \brief Internal computation initialization. */ + bool + initCompute (); + + /** \brief Internal computation initialization for reciprocal correspondences. */ + bool + initComputeReciprocal (); + + /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. + * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method + * is called. */ + bool target_cloud_updated_; + /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again. + * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method + * is called. */ + bool source_cloud_updated_; + /** \brief A flag which, if set, means the tree operating on the target cloud + * will never be recomputed*/ + bool force_no_recompute_; + + /** \brief A flag which, if set, means the tree operating on the source cloud + * will never be recomputed*/ + bool force_no_recompute_reciprocal_; + + }; + + /** \brief @b CorrespondenceEstimation represents the base class for + * determining correspondences between target and query point + * sets/features. + * + * Code example: + * + * \code + * pcl::PointCloud::Ptr source, target; + * // ... read or fill in source and target + * pcl::CorrespondenceEstimation est; + * est.setInputSource (source); + * est.setInputTarget (target); + * + * pcl::Correspondences all_correspondences; + * // Determine all reciprocal correspondences + * est.determineReciprocalCorrespondences (all_correspondences); + * \endcode + * + * \author Radu B. Rusu, Michael Dixon, Dirk Holz + * \ingroup registration + */ + template + class CorrespondenceEstimation : public CorrespondenceEstimationBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using CorrespondenceEstimationBase::point_representation_; + using CorrespondenceEstimationBase::input_transformed_; + using CorrespondenceEstimationBase::tree_; + using CorrespondenceEstimationBase::tree_reciprocal_; + using CorrespondenceEstimationBase::target_; + using CorrespondenceEstimationBase::corr_name_; + using CorrespondenceEstimationBase::target_indices_; + using CorrespondenceEstimationBase::getClassName; + using CorrespondenceEstimationBase::initCompute; + using CorrespondenceEstimationBase::initComputeReciprocal; + using CorrespondenceEstimationBase::input_; + using CorrespondenceEstimationBase::indices_; + using CorrespondenceEstimationBase::input_fields_; + using PCLBase::deinitCompute; + + using KdTree = pcl::search::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr; + + /** \brief Empty constructor. */ + CorrespondenceEstimation () + { + corr_name_ = "CorrespondenceEstimation"; + } + + /** \brief Empty destructor */ + ~CorrespondenceEstimation () {} + + /** \brief Determine the correspondences between input and target cloud. + * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) + * \param[in] max_distance maximum allowed distance between correspondences + */ + void + determineCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()) override; + + /** \brief Determine the reciprocal correspondences between input and target cloud. + * A correspondence is considered reciprocal if both Src_i has Tgt_i as a + * correspondence, and Tgt_i has Src_i as one. + * + * \param[out] correspondences the found correspondences (index of query and target point, distance) + * \param[in] max_distance maximum allowed distance between correspondences + */ + void + determineReciprocalCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()) override; + + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + typename CorrespondenceEstimationBase::Ptr + clone () const override + { + Ptr copy (new CorrespondenceEstimation (*this)); + return (copy); + } + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_backprojection.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_backprojection.h new file mode 100755 index 0000000..526e81b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_backprojection.h @@ -0,0 +1,225 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b CorrespondenceEstimationBackprojection computes + * correspondences as points in the target cloud which have minimum + * \author Suat Gedikli + * \ingroup registration + */ + template + class CorrespondenceEstimationBackProjection : public CorrespondenceEstimationBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using CorrespondenceEstimationBase::initCompute; + using CorrespondenceEstimationBase::initComputeReciprocal; + using CorrespondenceEstimationBase::input_transformed_; + using PCLBase::deinitCompute; + using PCLBase::input_; + using PCLBase::indices_; + using CorrespondenceEstimationBase::getClassName; + using CorrespondenceEstimationBase::point_representation_; + using CorrespondenceEstimationBase::target_indices_; + + using KdTree = pcl::search::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointCloudNormals = pcl::PointCloud; + using NormalsPtr = typename PointCloudNormals::Ptr; + using NormalsConstPtr = typename PointCloudNormals::ConstPtr; + + /** \brief Empty constructor. + * + * \note + * Sets the number of neighbors to be considered in the target point cloud (k_) to 10. + */ + CorrespondenceEstimationBackProjection () + : source_normals_ () + , source_normals_transformed_ () + , target_normals_ () + , k_ (10) + { + corr_name_ = "CorrespondenceEstimationBackProjection"; + } + + /** \brief Empty destructor */ + virtual ~CorrespondenceEstimationBackProjection () {} + + /** \brief Set the normals computed on the source point cloud + * \param[in] normals the normals computed for the source cloud + */ + inline void + setSourceNormals (const NormalsConstPtr &normals) { source_normals_ = normals; } + + /** \brief Get the normals of the source point cloud + */ + inline NormalsConstPtr + getSourceNormals () const { return (source_normals_); } + + /** \brief Set the normals computed on the target point cloud + * \param[in] normals the normals computed for the target cloud + */ + inline void + setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; } + + /** \brief Get the normals of the target point cloud + */ + inline NormalsConstPtr + getTargetNormals () const { return (target_normals_); } + + + /** \brief See if this rejector requires source normals */ + bool + requiresSourceNormals () const + { return (true); } + + /** \brief Blob method for setting the source normals */ + void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) + { + NormalsPtr cloud (new PointCloudNormals); + fromPCLPointCloud2 (*cloud2, *cloud); + setSourceNormals (cloud); + } + + /** \brief See if this rejector requires target normals*/ + bool + requiresTargetNormals () const + { return (true); } + + /** \brief Method for setting the target normals */ + void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr cloud2) + { + NormalsPtr cloud (new PointCloudNormals); + fromPCLPointCloud2 (*cloud2, *cloud); + setTargetNormals (cloud); + } + + /** \brief Determine the correspondences between input and target cloud. + * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) + * \param[in] max_distance maximum distance between the normal on the source point cloud and the corresponding point in the target + * point cloud + */ + void + determineCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()); + + /** \brief Determine the reciprocal correspondences between input and target cloud. + * A correspondence is considered reciprocal if both Src_i has Tgt_i as a + * correspondence, and Tgt_i has Src_i as one. + * + * \param[out] correspondences the found correspondences (index of query and target point, distance) + * \param[in] max_distance maximum allowed distance between correspondences + */ + virtual void + determineReciprocalCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()); + + /** \brief Set the number of nearest neighbours to be considered in the target + * point cloud. By default, we use k = 10 nearest neighbors. + * + * \param[in] k the number of nearest neighbours to be considered + */ + inline void + setKSearch (unsigned int k) { k_ = k; } + + /** \brief Get the number of nearest neighbours considered in the target point + * cloud for computing correspondences. By default we use k = 10 nearest + * neighbors. + */ + inline unsigned int + getKSearch () const { return (k_); } + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual typename CorrespondenceEstimationBase::Ptr + clone () const + { + Ptr copy (new CorrespondenceEstimationBackProjection (*this)); + return (copy); + } + + protected: + + using CorrespondenceEstimationBase::corr_name_; + using CorrespondenceEstimationBase::tree_; + using CorrespondenceEstimationBase::tree_reciprocal_; + using CorrespondenceEstimationBase::target_; + + /** \brief Internal computation initialization. */ + bool + initCompute (); + + private: + + /** \brief The normals computed at each point in the source cloud */ + NormalsConstPtr source_normals_; + + /** \brief The normals computed at each point in the source cloud */ + NormalsPtr source_normals_transformed_; + + /** \brief The normals computed at each point in the target cloud */ + NormalsConstPtr target_normals_; + + /** \brief The number of neighbours to be considered in the target point cloud */ + unsigned int k_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_normal_shooting.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_normal_shooting.h new file mode 100755 index 0000000..4e8f4b8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_normal_shooting.h @@ -0,0 +1,218 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b CorrespondenceEstimationNormalShooting computes + * correspondences as points in the target cloud which have minimum + * distance to normals computed on the input cloud + * + * Code example: + * + * \code + * pcl::PointCloud::Ptr source, target; + * // ... read or fill in source and target + * pcl::CorrespondenceEstimationNormalShooting est; + * est.setInputSource (source); + * est.setSourceNormals (source); + * + * est.setInputTarget (target); + * + * // Test the first 10 correspondences for each point in source, and return the best + * est.setKSearch (10); + * + * pcl::Correspondences all_correspondences; + * // Determine all correspondences + * est.determineCorrespondences (all_correspondences); + * \endcode + * + * \author Aravindhan K. Krishnan, Radu B. Rusu + * \ingroup registration + */ + template + class CorrespondenceEstimationNormalShooting : public CorrespondenceEstimationBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using CorrespondenceEstimationBase::initCompute; + using CorrespondenceEstimationBase::initComputeReciprocal; + using CorrespondenceEstimationBase::input_transformed_; + using PCLBase::deinitCompute; + using PCLBase::input_; + using PCLBase::indices_; + using CorrespondenceEstimationBase::getClassName; + using CorrespondenceEstimationBase::point_representation_; + using CorrespondenceEstimationBase::target_indices_; + + using KdTree = pcl::search::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointCloudNormals = pcl::PointCloud; + using NormalsPtr = typename PointCloudNormals::Ptr; + using NormalsConstPtr = typename PointCloudNormals::ConstPtr; + + /** \brief Empty constructor. + * + * \note + * Sets the number of neighbors to be considered in the target point cloud (k_) to 10. + */ + CorrespondenceEstimationNormalShooting () + : source_normals_ () + , source_normals_transformed_ () + , k_ (10) + { + corr_name_ = "CorrespondenceEstimationNormalShooting"; + } + + /** \brief Empty destructor */ + ~CorrespondenceEstimationNormalShooting () {} + + /** \brief Set the normals computed on the source point cloud + * \param[in] normals the normals computed for the source cloud + */ + inline void + setSourceNormals (const NormalsConstPtr &normals) { source_normals_ = normals; } + + /** \brief Get the normals of the source point cloud + */ + inline NormalsConstPtr + getSourceNormals () const { return (source_normals_); } + + + /** \brief See if this rejector requires source normals */ + bool + requiresSourceNormals () const override + { return (true); } + + /** \brief Blob method for setting the source normals */ + void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + NormalsPtr cloud (new PointCloudNormals); + fromPCLPointCloud2 (*cloud2, *cloud); + setSourceNormals (cloud); + } + + /** \brief Determine the correspondences between input and target cloud. + * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) + * \param[in] max_distance maximum distance between the normal on the source point cloud and the corresponding point in the target + * point cloud + */ + void + determineCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()) override; + + /** \brief Determine the reciprocal correspondences between input and target cloud. + * A correspondence is considered reciprocal if both Src_i has Tgt_i as a + * correspondence, and Tgt_i has Src_i as one. + * + * \param[out] correspondences the found correspondences (index of query and target point, distance) + * \param[in] max_distance maximum allowed distance between correspondences + */ + void + determineReciprocalCorrespondences (pcl::Correspondences &correspondences, + double max_distance = std::numeric_limits::max ()) override; + + /** \brief Set the number of nearest neighbours to be considered in the target + * point cloud. By default, we use k = 10 nearest neighbors. + * + * \param[in] k the number of nearest neighbours to be considered + */ + inline void + setKSearch (unsigned int k) { k_ = k; } + + /** \brief Get the number of nearest neighbours considered in the target point + * cloud for computing correspondences. By default we use k = 10 nearest + * neighbors. + */ + inline unsigned int + getKSearch () const { return (k_); } + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + typename CorrespondenceEstimationBase::Ptr + clone () const override + { + Ptr copy (new CorrespondenceEstimationNormalShooting (*this)); + return (copy); + } + + protected: + + using CorrespondenceEstimationBase::corr_name_; + using CorrespondenceEstimationBase::tree_; + using CorrespondenceEstimationBase::tree_reciprocal_; + using CorrespondenceEstimationBase::target_; + + /** \brief Internal computation initialization. */ + bool + initCompute (); + + private: + + /** \brief The normals computed at each point in the source cloud */ + NormalsConstPtr source_normals_; + + /** \brief The normals computed at each point in the source cloud */ + NormalsPtr source_normals_transformed_; + + /** \brief The number of neighbours to be considered in the target point cloud */ + unsigned int k_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_organized_projection.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_organized_projection.h new file mode 100755 index 0000000..220ca64 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_estimation_organized_projection.h @@ -0,0 +1,207 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point cloud + * onto the target point cloud using the camera intrinsic and extrinsic parameters. The correspondences can be trimmed + * by a depth threshold and by a distance threshold. + * It is not as precise as a nearest neighbor search, but it is much faster, as it avoids the usage of any additional + * structures (i.e., kd-trees). + * \note The target point cloud must be organized (no restrictions on the source) and the target point cloud must be + * given in the camera coordinate frame. Any other transformation is specified by the src_to_tgt_transformation_ + * variable. + * \author Alex Ichim + */ + template + class CorrespondenceEstimationOrganizedProjection : public CorrespondenceEstimationBase + { + public: + using CorrespondenceEstimationBase::initCompute; + using CorrespondenceEstimationBase::input_transformed_; + using PCLBase::deinitCompute; + using PCLBase::input_; + using PCLBase::indices_; + using CorrespondenceEstimationBase::getClassName; + using CorrespondenceEstimationBase::point_representation_; + using CorrespondenceEstimationBase::target_cloud_updated_; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using Ptr = shared_ptr< CorrespondenceEstimationOrganizedProjection >; + using ConstPtr = shared_ptr< const CorrespondenceEstimationOrganizedProjection >; + + + + /** \brief Empty constructor that sets all the intrinsic calibration to the default Kinect values. */ + CorrespondenceEstimationOrganizedProjection () + : fx_ (525.f) + , fy_ (525.f) + , cx_ (319.5f) + , cy_ (239.5f) + , src_to_tgt_transformation_ (Eigen::Matrix4f::Identity ()) + , depth_threshold_ (std::numeric_limits::max ()) + , projection_matrix_ (Eigen::Matrix3f::Identity ()) + { } + + + /** \brief Sets the focal length parameters of the target camera. + * \param[in] fx the focal length in pixels along the x-axis of the image + * \param[in] fy the focal length in pixels along the y-axis of the image + */ + inline void + setFocalLengths (const float fx, const float fy) + { fx_ = fx; fy_ = fy; } + + /** \brief Reads back the focal length parameters of the target camera. + * \param[out] fx the focal length in pixels along the x-axis of the image + * \param[out] fy the focal length in pixels along the y-axis of the image + */ + inline void + getFocalLengths (float &fx, float &fy) const + { fx = fx_; fy = fy_; } + + + /** \brief Sets the camera center parameters of the target camera. + * \param[in] cx the x-coordinate of the camera center + * \param[in] cy the y-coordinate of the camera center + */ + inline void + setCameraCenters (const float cx, const float cy) + { cx_ = cx; cy_ = cy; } + + /** \brief Reads back the camera center parameters of the target camera. + * \param[out] cx the x-coordinate of the camera center + * \param[out] cy the y-coordinate of the camera center + */ + inline void + getCameraCenters (float &cx, float &cy) const + { cx = cx_; cy = cy_; } + + /** \brief Sets the transformation from the source point cloud to the target point cloud. + * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct + * for that. + * \param[in] src_to_tgt_transformation the transformation + */ + inline void + setSourceTransformation (const Eigen::Matrix4f &src_to_tgt_transformation) + { src_to_tgt_transformation_ = src_to_tgt_transformation; } + + /** \brief Reads back the transformation from the source point cloud to the target point cloud. + * \note The target point cloud must be in its local camera coordinates, so use this transformation to correct + * for that. + * \return the transformation + */ + inline Eigen::Matrix4f + getSourceTransformation () const + { return (src_to_tgt_transformation_); } + + /** \brief Sets the depth threshold; after projecting the source points in the image space of the target camera, + * this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too far from + * each other. + * \param[in] depth_threshold the depth threshold + */ + inline void + setDepthThreshold (const float depth_threshold) + { depth_threshold_ = depth_threshold; } + + /** \brief Reads back the depth threshold; after projecting the source points in the image space of the target + * camera, this threshold is applied on the depths of corresponding dexels to eliminate the ones that are too + * far from each other. + * \return the depth threshold + */ + inline float + getDepthThreshold () const + { return (depth_threshold_); } + + /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold. + * \param correspondences + * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected + */ + void + determineCorrespondences (Correspondences &correspondences, double max_distance); + + /** \brief Computes the correspondences, applying a maximum Euclidean distance threshold. + * \param correspondences + * \param[in] max_distance Euclidean distance threshold above which correspondences will be rejected + */ + void + determineReciprocalCorrespondences (Correspondences &correspondences, double max_distance); + + /** \brief Clone and cast to CorrespondenceEstimationBase */ + virtual typename CorrespondenceEstimationBase::Ptr + clone () const + { + Ptr copy (new CorrespondenceEstimationOrganizedProjection (*this)); + return (copy); + } + + protected: + using CorrespondenceEstimationBase::target_; + + bool + initCompute (); + + float fx_, fy_; + float cx_, cy_; + Eigen::Matrix4f src_to_tgt_transformation_; + float depth_threshold_; + + Eigen::Matrix3f projection_matrix_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection.h new file mode 100755 index 0000000..ca9371f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection.h @@ -0,0 +1,405 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** @b CorrespondenceRejector represents the base class for correspondence rejection methods + * \author Dirk Holz + * \ingroup registration + */ + class CorrespondenceRejector + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + CorrespondenceRejector () + {} + + /** \brief Empty destructor. */ + virtual ~CorrespondenceRejector () {} + + /** \brief Provide a pointer to the vector of the input correspondences. + * \param[in] correspondences the const boost shared pointer to a correspondence vector + */ + virtual inline void + setInputCorrespondences (const CorrespondencesConstPtr &correspondences) + { + input_correspondences_ = correspondences; + }; + + /** \brief Get a pointer to the vector of the input correspondences. + * \return correspondences the const boost shared pointer to a correspondence vector + */ + inline CorrespondencesConstPtr + getInputCorrespondences () { return input_correspondences_; }; + + /** \brief Run correspondence rejection + * \param[out] correspondences Vector of correspondences that have not been rejected. + */ + inline void + getCorrespondences (pcl::Correspondences &correspondences) + { + if (!input_correspondences_ || (input_correspondences_->empty ())) + return; + + applyRejection (correspondences); + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * Pure virtual. Compared to \a getCorrespondences this function is + * stateless, i.e., input correspondences do not need to be provided beforehand, + * but are directly provided in the function call. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + virtual inline void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) = 0; + + /** \brief Determine the indices of query points of + * correspondences that have been rejected, i.e., the difference + * between the input correspondences (set via \a setInputCorrespondences) + * and the given correspondence vector. + * \param[in] correspondences Vector of correspondences after rejection + * \param[out] indices Vector of query point indices of those correspondences + * that have been rejected. + */ + inline void + getRejectedQueryIndices (const pcl::Correspondences &correspondences, + std::vector& indices) + { + if (!input_correspondences_ || input_correspondences_->empty ()) + { + PCL_WARN ("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences not set (lookup of rejected correspondences _not_ possible).\n", getClassName ().c_str ()); + return; + } + + pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices); + } + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const { return (rejection_name_); } + + + /** \brief See if this rejector requires source points */ + virtual bool + requiresSourcePoints () const + { return (false); } + + /** \brief Abstract method for setting the source cloud */ + virtual void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setSourcePoints] This class does not require an input source cloud", getClassName ().c_str ()); + } + + /** \brief See if this rejector requires source normals */ + virtual bool + requiresSourceNormals () const + { return (false); } + + /** \brief Abstract method for setting the source normals */ + virtual void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ()); + } + /** \brief See if this rejector requires a target cloud */ + virtual bool + requiresTargetPoints () const + { return (false); } + + /** \brief Abstract method for setting the target cloud */ + virtual void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setTargetPoints] This class does not require an input target cloud", getClassName ().c_str ()); + } + + /** \brief See if this rejector requires target normals */ + virtual bool + requiresTargetNormals () const + { return (false); } + + /** \brief Abstract method for setting the target normals */ + virtual void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr /*cloud2*/) + { + PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ()); + } + + protected: + + /** \brief The name of the rejection method. */ + std::string rejection_name_; + + /** \brief The input correspondences. */ + CorrespondencesConstPtr input_correspondences_; + + /** \brief Abstract rejection method. */ + virtual void + applyRejection (Correspondences &correspondences) = 0; + }; + + /** @b DataContainerInterface provides a generic interface for computing correspondence scores between correspondent + * points in the input and target clouds + * \ingroup registration + */ + class DataContainerInterface + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + virtual ~DataContainerInterface () = default; + virtual double getCorrespondenceScore (int index) = 0; + virtual double getCorrespondenceScore (const pcl::Correspondence &) = 0; + virtual double getCorrespondenceScoreFromNormals (const pcl::Correspondence &) = 0; + }; + + /** @b DataContainer is a container for the input and target point clouds and implements the interface + * to compute correspondence scores between correspondent points in the input and target clouds + * \ingroup registration + */ + template + class DataContainer : public DataContainerInterface + { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using KdTreePtr = typename pcl::search::KdTree::Ptr; + + using Normals = pcl::PointCloud; + using NormalsPtr = typename Normals::Ptr; + using NormalsConstPtr = typename Normals::ConstPtr; + + public: + + /** \brief Empty constructor. */ + DataContainer (bool needs_normals = false) + : input_ () + , input_transformed_ () + , target_ () + , input_normals_ () + , input_normals_transformed_ () + , target_normals_ () + , tree_ (new pcl::search::KdTree) + , class_name_ ("DataContainer") + , needs_normals_ (needs_normals) + , target_cloud_updated_ (true) + , force_no_recompute_ (false) + { + } + + /** \brief Empty destructor */ + ~DataContainer () {} + + /** \brief Provide a source point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + inline void + setInputSource (const PointCloudConstPtr &cloud) + { + input_ = cloud; + } + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudConstPtr const + getInputSource () { return (input_); } + + /** \brief Provide a target point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] target a cloud containing XYZ data + */ + inline void + setInputTarget (const PointCloudConstPtr &target) + { + target_ = target; + target_cloud_updated_ = true; + } + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudConstPtr const + getInputTarget () { return (target_); } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + inline void + setSearchMethodTarget (const KdTreePtr &tree, + bool force_no_recompute = false) + { + tree_ = tree; + if (force_no_recompute) + { + force_no_recompute_ = true; + } + target_cloud_updated_ = true; + } + + /** \brief Set the normals computed on the input point cloud + * \param[in] normals the normals computed for the input cloud + */ + inline void + setInputNormals (const NormalsConstPtr &normals) { input_normals_ = normals; } + + /** \brief Get the normals computed on the input point cloud */ + inline NormalsConstPtr + getInputNormals () { return (input_normals_); } + + /** \brief Set the normals computed on the target point cloud + * \param[in] normals the normals computed for the input cloud + */ + inline void + setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; } + + /** \brief Get the normals computed on the target point cloud */ + inline NormalsConstPtr + getTargetNormals () { return (target_normals_); } + + /** \brief Get the correspondence score for a point in the input cloud + * \param[in] index index of the point in the input cloud + */ + inline double + getCorrespondenceScore (int index) override + { + if ( target_cloud_updated_ && !force_no_recompute_ ) + { + tree_->setInputCloud (target_); + } + std::vector indices (1); + std::vector distances (1); + if (tree_->nearestKSearch (input_->points[index], 1, indices, distances)) + return (distances[0]); + return (std::numeric_limits::max ()); + } + + /** \brief Get the correspondence score for a given pair of correspondent points + * \param[in] corr Correspondent points + */ + inline double + getCorrespondenceScore (const pcl::Correspondence &corr) override + { + // Get the source and the target feature from the list + const PointT &src = input_->points[corr.index_query]; + const PointT &tgt = target_->points[corr.index_match]; + + return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ()); + } + + /** \brief Get the correspondence score for a given pair of correspondent points based on + * the angle between the normals. The normmals for the in put and target clouds must be + * set before using this function + * \param[in] corr Correspondent points + */ + inline double + getCorrespondenceScoreFromNormals (const pcl::Correspondence &corr) override + { + //assert ( (input_normals_->points.size () != 0) && (target_normals_->points.size () != 0) && "Normals are not set for the input and target point clouds"); + assert (input_normals_ && target_normals_ && "Normals are not set for the input and target point clouds"); + const NormalT &src = input_normals_->points[corr.index_query]; + const NormalT &tgt = target_normals_->points[corr.index_match]; + return (double ((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) + (src.normal[2] * tgt.normal[2]))); + } + + private: + /** \brief The input point cloud dataset */ + PointCloudConstPtr input_; + + /** \brief The input transformed point cloud dataset */ + PointCloudPtr input_transformed_; + + /** \brief The target point cloud datase. */ + PointCloudConstPtr target_; + + /** \brief Normals to the input point cloud */ + NormalsConstPtr input_normals_; + + /** \brief Normals to the input point cloud */ + NormalsPtr input_normals_transformed_; + + /** \brief Normals to the target point cloud */ + NormalsConstPtr target_normals_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The name of the rejection method. */ + std::string class_name_; + + /** \brief Should the current data container use normals? */ + bool needs_normals_; + + /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. + * This way, we avoid rebuilding the kd-tree */ + bool target_cloud_updated_; + + /** \brief A flag which, if set, means the tree operating on the target cloud + * will never be recomputed*/ + bool force_no_recompute_; + + + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const { return (class_name_); } + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_distance.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_distance.h new file mode 100755 index 0000000..2655244 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_distance.h @@ -0,0 +1,207 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** + * @b CorrespondenceRejectorDistance implements a simple correspondence + * rejection method based on thresholding the distances between the + * correspondences. + * + * \note If \ref setInputCloud and \ref setInputTarget are given, then the + * distances between correspondences will be estimated using the given XYZ + * data, and not read from the set of input correspondences. + * + * \author Dirk Holz, Radu B. Rusu + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectorDistance: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + CorrespondenceRejectorDistance () : max_distance_(std::numeric_limits::max ()) + { + rejection_name_ = "CorrespondenceRejectorDistance"; + } + + /** \brief Empty destructor */ + ~CorrespondenceRejectorDistance () {} + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + /** \brief Set the maximum distance used for thresholding in correspondence rejection. + * \param[in] distance Distance to be used as maximum distance between correspondences. + * Correspondences with larger distances are rejected. + * \note Internally, the distance will be stored squared. + */ + virtual inline void + setMaximumDistance (float distance) { max_distance_ = distance * distance; }; + + /** \brief Get the maximum distance used for thresholding in correspondence rejection. */ + inline float + getMaximumDistance () { return std::sqrt (max_distance_); }; + + /** \brief Provide a source point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + template inline void + setInputCloud (const typename pcl::PointCloud::ConstPtr &cloud) + { + PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + } + + /** \brief Provide a source point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + template inline void + setInputSource (const typename pcl::PointCloud::ConstPtr &cloud) + { + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + } + + /** \brief Provide a target point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] target a cloud containing XYZ data + */ + template inline void + setInputTarget (const typename pcl::PointCloud::ConstPtr &target) + { + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputTarget (target); + } + + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const override + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const override + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + template inline void + setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, + bool force_no_recompute = false) + { + boost::static_pointer_cast< DataContainer > + (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); + } + + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the + * distance is larger than this threshold, the points will not be ignored in the alignment process. + */ + float max_distance_; + + using DataContainerPtr = DataContainerInterface::Ptr; + + /** \brief A pointer to the DataContainer object containing the input and target point clouds */ + DataContainerPtr data_container_; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_features.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_features.h new file mode 100755 index 0000000..7d562a5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_features.h @@ -0,0 +1,300 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature + * descriptors. Given an input feature space, the method checks if each feature in the source cloud has a + * correspondence in the target cloud, either by checking the first K (given) point correspondences, or + * by defining a tolerance threshold via a radius in feature space. + * \todo explain this better. + * \author Radu B. Rusu + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectorFeatures: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + CorrespondenceRejectorFeatures () : max_distance_ (std::numeric_limits::max ()) + { + rejection_name_ = "CorrespondenceRejectorFeatures"; + } + + /** \brief Empty destructor. */ + ~CorrespondenceRejectorFeatures () {} + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + /** \brief Provide a pointer to a cloud of feature descriptors associated with the source point cloud + * \param[in] source_feature a cloud of feature descriptors associated with the source point cloud + * \param[in] key a string that uniquely identifies the feature + */ + template inline void + setSourceFeature (const typename pcl::PointCloud::ConstPtr &source_feature, + const std::string &key); + + /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key + * \param[in] key a string that uniquely identifies the feature (must match the key provided by setSourceFeature) + */ + template inline typename pcl::PointCloud::ConstPtr + getSourceFeature (const std::string &key); + + /** \brief Provide a pointer to a cloud of feature descriptors associated with the target point cloud + * \param[in] target_feature a cloud of feature descriptors associated with the target point cloud + * \param[in] key a string that uniquely identifies the feature + */ + template inline void + setTargetFeature (const typename pcl::PointCloud::ConstPtr &target_feature, + const std::string &key); + + /** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key + * \param[in] key a string that uniquely identifies the feature (must match the key provided by setTargetFeature) + */ + template inline typename pcl::PointCloud::ConstPtr + getTargetFeature (const std::string &key); + + /** \brief Set a hard distance threshold in the feature \a FeatureT space, between source and target + * features. Any feature correspondence that is above this threshold will be considered bad and will be + * filtered out. + * \param[in] thresh the distance threshold + * \param[in] key a string that uniquely identifies the feature + */ + template inline void + setDistanceThreshold (double thresh, const std::string &key); + + /** \brief Test that all features are valid (i.e., does each key have a valid source cloud, target cloud, + * and search method) + */ + inline bool + hasValidFeatures (); + + /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features + * \param[in] key a string that uniquely identifies the feature + * \param[in] fr the point feature representation to be used + */ + template inline void + setFeatureRepresentation (const typename pcl::PointRepresentation::ConstPtr &fr, + const std::string &key); + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the + * distance is larger than this threshold, the points will not be ignored in the alignment process. + */ + float max_distance_; + + class FeatureContainerInterface + { + public: + /** \brief Empty destructor */ + virtual ~FeatureContainerInterface () = default; + virtual bool isValid () = 0; + virtual double getCorrespondenceScore (int index) = 0; + virtual bool isCorrespondenceValid (int index) = 0; + + using Ptr = shared_ptr; + }; + + using FeaturesMap = std::unordered_map; + + /** \brief An STL map containing features to use when performing the correspondence search.*/ + FeaturesMap features_map_; + + /** \brief An inner class containing pointers to the source and target feature clouds + * and the parameters needed to perform the correspondence search. This class extends + * FeatureContainerInterface, which contains abstract methods for any methods that do not depend on the + * FeatureT --- these methods can thus be called from a pointer to FeatureContainerInterface without + * casting to the derived class. + */ + template + class FeatureContainer : public pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface + { + public: + using FeatureCloudConstPtr = typename pcl::PointCloud::ConstPtr; + using SearchMethod = std::function &, int, std::vector &, std::vector &)>; + + using PointRepresentationConstPtr = typename pcl::PointRepresentation::ConstPtr; + + FeatureContainer () : thresh_(std::numeric_limits::max ()), feature_representation_() + { + } + + /** \brief Empty destructor */ + ~FeatureContainer () {} + + inline void + setSourceFeature (const FeatureCloudConstPtr &source_features) + { + source_features_ = source_features; + } + + inline FeatureCloudConstPtr + getSourceFeature () + { + return (source_features_); + } + + inline void + setTargetFeature (const FeatureCloudConstPtr &target_features) + { + target_features_ = target_features; + } + + inline FeatureCloudConstPtr + getTargetFeature () + { + return (target_features_); + } + + inline void + setDistanceThreshold (double thresh) + { + thresh_ = thresh; + } + + inline bool + isValid () override + { + if (!source_features_ || !target_features_) + return (false); + return (source_features_->points.size () > 0 && + target_features_->points.size () > 0); + } + + /** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features + * \param[in] fr the point feature representation to be used + */ + inline void + setFeatureRepresentation (const PointRepresentationConstPtr &fr) + { + feature_representation_ = fr; + } + + /** \brief Obtain a score between a pair of correspondences. + * \param[in] index the index to check in the list of correspondences + * \return score the resultant computed score + */ + inline double + getCorrespondenceScore (int index) override + { + // If no feature representation was given, reset to the default implementation for FeatureT + if (!feature_representation_) + feature_representation_.reset (new DefaultFeatureRepresentation); + + // Get the source and the target feature from the list + const FeatureT &feat_src = source_features_->points[index]; + const FeatureT &feat_tgt = target_features_->points[index]; + + // Check if the representations are valid + if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt)) + { + PCL_ERROR ("[pcl::registration::%s::getCorrespondenceScore] Invalid feature representation given!\n", this->getClassName ().c_str ()); + return (std::numeric_limits::max ()); + } + + // Set the internal feature point representation of choice + Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ()); + feature_representation_->vectorize (FeatureT (feat_src), feat_src_ptr); + Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ()); + feature_representation_->vectorize (FeatureT (feat_tgt), feat_tgt_ptr); + + // Compute the L2 norm + return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ()); + } + + /** \brief Check whether the correspondence pair at the given index is valid + * by computing the score and testing it against the user given threshold + * \param[in] index the index to check in the list of correspondences + * \return true if the correspondence is good, false otherwise + */ + inline bool + isCorrespondenceValid (int index) override + { + return (getCorrespondenceScore (index) < thresh_ * thresh_); + } + + private: + FeatureCloudConstPtr source_features_, target_features_; + SearchMethod search_method_; + + /** \brief The L2 squared Euclidean threshold. */ + double thresh_; + + /** \brief The internal point feature representation used. */ + PointRepresentationConstPtr feature_representation_; + }; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_median_distance.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_median_distance.h new file mode 100755 index 0000000..08104c7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_median_distance.h @@ -0,0 +1,210 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceRejectorMedianDistance implements a simple correspondence + * rejection method based on thresholding based on the median distance between the + * correspondences. + * + * \note If \ref setInputCloud and \ref setInputTarget are given, then the + * distances between correspondences will be estimated using the given XYZ + * data, and not read from the set of input correspondences. + * + * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher) + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectorMedianDistance: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + CorrespondenceRejectorMedianDistance () + : median_distance_ (0) + , factor_ (1.0) + { + rejection_name_ = "CorrespondenceRejectorMedianDistance"; + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + /** \brief Get the median distance used for thresholding in correspondence rejection. */ + inline double + getMedianDistance () const { return (median_distance_); }; + + /** \brief Provide a source point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + template inline void + setInputSource (const typename pcl::PointCloud::ConstPtr &cloud) + { + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + } + + /** \brief Provide a source point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + template inline void + setInputCloud (const typename pcl::PointCloud::ConstPtr &cloud) + { + PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + } + + /** \brief Provide a target point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] target a cloud containing XYZ data + */ + template inline void + setInputTarget (const typename pcl::PointCloud::ConstPtr &target) + { + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputTarget (target); + } + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const override + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const override + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + template inline void + setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, + bool force_no_recompute = false) + { + boost::static_pointer_cast< DataContainer > + (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); + } + + /** \brief Set the factor for correspondence rejection. Points with distance greater than median times factor + * will be rejected + * \param[in] factor value + */ + inline void + setMedianFactor (double factor) { factor_ = factor; }; + + /** \brief Get the factor used for thresholding in correspondence rejection. */ + inline double + getMedianFactor () const { return factor_; }; + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** \brief The median distance threshold between two correspondent points in source <-> target. + */ + double median_distance_; + + /** \brief The factor for correspondence rejection. Points with distance greater than median times factor + * will be rejected + */ + double factor_; + + using DataContainerPtr = DataContainerInterface::Ptr; + + /** \brief A pointer to the DataContainer object containing the input and target point clouds */ + DataContainerPtr data_container_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_one_to_one.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_one_to_one.h new file mode 100755 index 0000000..ea5e382 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_one_to_one.h @@ -0,0 +1,96 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceRejectorOneToOne implements a correspondence + * rejection method based on eliminating duplicate match indices in + * the correspondences. Correspondences with the same match index are + * removed and only the one with smallest distance between query and + * match are kept. That is, considering match->query 1-m correspondences + * are removed leaving only 1-1 correspondences. + * \author Dirk Holz + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectorOneToOne: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + CorrespondenceRejectorOneToOne () + { + rejection_name_ = "CorrespondenceRejectorOneToOne"; + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + protected: + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_organized_boundary.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_organized_boundary.h new file mode 100755 index 0000000..cfa20ec --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_organized_boundary.h @@ -0,0 +1,144 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) Alexandru-Eugen Ichim + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include + + +namespace pcl +{ + namespace registration + { + /** + * @brief The CorrespondenceRejectionOrganizedBoundary class implements a simple correspondence rejection measure. + * For each pair of points in correspondence, it checks whether they are on the boundary of a silhouette. This is + * done by counting the number of NaN dexels in a window around the points (the threshold and window size can be set + * by the user). + * \note Both the source and the target clouds need to be organized, otherwise all the correspondences will be rejected. + * + * \author Alexandru E. Ichim + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectionOrganizedBoundary : public CorrespondenceRejector + { + public: + /** @brief Empty constructor. */ + CorrespondenceRejectionOrganizedBoundary () + : boundary_nans_threshold_ (8) + , window_size_ (5) + , depth_step_threshold_ (0.025f) + { } + + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + inline void + setNumberOfBoundaryNaNs (int val) + { boundary_nans_threshold_ = val; } + + + template inline void + setInputSource (const typename pcl::PointCloud::ConstPtr &cloud) + { + if (!data_container_) + data_container_.reset (new pcl::registration::DataContainer); + boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + } + + template inline void + setInputTarget (const typename pcl::PointCloud::ConstPtr &cloud) + { + if (!data_container_) + data_container_.reset (new pcl::registration::DataContainer); + boost::static_pointer_cast > (data_container_)->setInputTarget (cloud); + } + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const override + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const override + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + virtual bool + updateSource (const Eigen::Matrix4d &) + { return (true); } + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { getRemainingCorrespondences (*input_correspondences_, correspondences); } + + int boundary_nans_threshold_; + int window_size_; + float depth_step_threshold_; + + using DataContainerPtr = DataContainerInterface::Ptr; + DataContainerPtr data_container_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_poly.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_poly.h new file mode 100755 index 0000000..593bd30 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_poly.h @@ -0,0 +1,384 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and + * pose-invariant geometric constraints between two point sets by forming virtual polygons of a user-specifiable + * cardinality on each model using the input correspondences. + * These polygons are then checked in a pose-invariant manner (i.e. the side lengths must be approximately equal), + * and rejection is performed by thresholding these edge lengths. + * + * If you use this in academic work, please cite: + * + * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger. + * Pose Estimation using Local Structure-Specific Shape and Appearance Context. + * International Conference on Robotics and Automation (ICRA), 2013. + * + * \author Anders Glent Buch + * \ingroup registration + */ + template + class PCL_EXPORTS CorrespondenceRejectorPoly: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + /** \brief Empty constructor */ + CorrespondenceRejectorPoly () + : iterations_ (10000) + , cardinality_ (3) + , similarity_threshold_ (0.75f) + , similarity_threshold_squared_ (0.75f * 0.75f) + { + rejection_name_ = "CorrespondenceRejectorPoly"; + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + inline void + setInputSource (const PointCloudSourceConstPtr &cloud) + { + input_ = cloud; + } + + /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + inline void + setInputCloud (const PointCloudSourceConstPtr &cloud) + { + PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", + getClassName ().c_str ()); + input_ = cloud; + } + + /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] target a cloud containing XYZ data + */ + inline void + setInputTarget (const PointCloudTargetConstPtr &target) + { + target_ = target; + } + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const override + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloudSourcePtr cloud (new PointCloudSource); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const override + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloudTargetPtr cloud (new PointCloudTarget); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + /** \brief Set the polygon cardinality + * \param cardinality polygon cardinality + */ + inline void + setCardinality (int cardinality) + { + cardinality_ = cardinality; + } + + /** \brief Get the polygon cardinality + * \return polygon cardinality + */ + inline int + getCardinality () + { + return (cardinality_); + } + + /** \brief Set the similarity threshold in [0,1[ between edge lengths, + * where 1 is a perfect match + * \param similarity_threshold similarity threshold + */ + inline void + setSimilarityThreshold (float similarity_threshold) + { + similarity_threshold_ = similarity_threshold; + similarity_threshold_squared_ = similarity_threshold * similarity_threshold; + } + + /** \brief Get the similarity threshold between edge lengths + * \return similarity threshold + */ + inline float + getSimilarityThreshold () + { + return (similarity_threshold_); + } + + /** \brief Set the number of iterations + * \param iterations number of iterations + */ + inline void + setIterations (int iterations) + { + iterations_ = iterations; + } + + /** \brief Get the number of iterations + * \return number of iterations + */ + inline int + getIterations () + { + return (iterations_); + } + + /** \brief Polygonal rejection of a single polygon, indexed by a subset of correspondences + * \param corr all correspondences into \ref input_ and \ref target_ + * \param idx sampled indices into \b correspondences, must have a size equal to \ref cardinality_ + * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_ + */ + inline bool + thresholdPolygon (const pcl::Correspondences& corr, const std::vector& idx) + { + if (cardinality_ == 2) // Special case: when two points are considered, we only have one edge + { + return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query, + corr[ idx[0] ].index_match, corr[ idx[1] ].index_match, + cardinality_)); + } + // Otherwise check all edges + for (int i = 0; i < cardinality_; ++i) + { + if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query, + corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match, + similarity_threshold_squared_)) + { + return (false); + } + } + return (true); + } + + /** \brief Polygonal rejection of a single polygon, indexed by two point index vectors + * \param source_indices indices of polygon points in \ref input_, must have a size equal to \ref cardinality_ + * \param target_indices corresponding indices of polygon points in \ref target_, must have a size equal to \ref cardinality_ + * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_ + */ + inline bool + thresholdPolygon (const std::vector& source_indices, const std::vector& target_indices) + { + // Convert indices to correspondences and an index vector pointing to each element + pcl::Correspondences corr (cardinality_); + std::vector idx (cardinality_); + for (int i = 0; i < cardinality_; ++i) + { + corr[i].index_query = source_indices[i]; + corr[i].index_match = target_indices[i]; + idx[i] = i; + } + + return (thresholdPolygon (corr, idx)); + } + + protected: + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** \brief Get k unique random indices in range {0,...,n-1} (sampling without replacement) + * \note No check is made to ensure that k <= n. + * \param n upper index range, exclusive + * \param k number of unique indices to sample + * \return k unique random indices in range {0,...,n-1} + */ + inline std::vector + getUniqueRandomIndices (int n, int k) + { + // Marked sampled indices and sample counter + std::vector sampled (n, false); + int samples = 0; + // Resulting unique indices + std::vector result; + result.reserve (k); + do + { + // Pick a random index in the range + const int idx = (std::rand () % n); + // If unique + if (!sampled[idx]) + { + // Mark as sampled and increment result counter + sampled[idx] = true; + ++samples; + // Store + result.push_back (idx); + } + } + while (samples < k); + + return (result); + } + + /** \brief Squared Euclidean distance between two points using the members x, y and z + * \param p1 first point + * \param p2 second point + * \return squared Euclidean distance + */ + inline float + computeSquaredDistance (const SourceT& p1, const TargetT& p2) + { + const float dx = p2.x - p1.x; + const float dy = p2.y - p1.y; + const float dz = p2.z - p1.z; + + return (dx*dx + dy*dy + dz*dz); + } + + /** \brief Edge length similarity thresholding + * \param index_query_1 index of first source vertex + * \param index_query_2 index of second source vertex + * \param index_match_1 index of first target vertex + * \param index_match_2 index of second target vertex + * \param simsq squared similarity threshold in [0,1] + * \return true if edge length ratio is larger than or equal to threshold + */ + inline bool + thresholdEdgeLength (int index_query_1, + int index_query_2, + int index_match_1, + int index_match_2, + float simsq) + { + // Distance between source points + const float dist_src = computeSquaredDistance ((*input_)[index_query_1], (*input_)[index_query_2]); + // Distance between target points + const float dist_tgt = computeSquaredDistance ((*target_)[index_match_1], (*target_)[index_match_2]); + // Edge length similarity [0,1] where 1 is a perfect match + const float edge_sim = (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src); + + return (edge_sim >= simsq); + } + + /** \brief Compute a linear histogram. This function is equivalent to the MATLAB function \b histc, with the + * edges set as follows: lower:(upper-lower)/bins:upper + * \param data input samples + * \param lower lower bound of input samples + * \param upper upper bound of input samples + * \param bins number of bins in output + * \return linear histogram + */ + std::vector + computeHistogram (const std::vector& data, float lower, float upper, int bins); + + /** \brief Find the optimal value for binary histogram thresholding using Otsu's method + * \param histogram input histogram + * \return threshold value according to Otsu's criterion + */ + int + findThresholdOtsu (const std::vector& histogram); + + /** \brief The input point cloud dataset */ + PointCloudSourceConstPtr input_; + + /** \brief The input point cloud dataset target */ + PointCloudTargetConstPtr target_; + + /** \brief Number of iterations to run */ + int iterations_; + + /** \brief The polygon cardinality used during rejection */ + int cardinality_; + + /** \brief Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect match */ + float similarity_threshold_; + + /** \brief Squared value if \ref similarity_threshold_, only for internal use */ + float similarity_threshold_squared_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_sample_consensus.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_sample_consensus.h new file mode 100755 index 0000000..41dcf50 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_sample_consensus.h @@ -0,0 +1,248 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + + +#include +#include + +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceRejectorSampleConsensus implements a correspondence rejection + * using Random Sample Consensus to identify inliers (and reject outliers) + * \author Dirk Holz + * \ingroup registration + */ + template + class CorrespondenceRejectorSampleConsensus: public CorrespondenceRejector + { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m), + * and the maximum number of iterations to 1000. + */ + CorrespondenceRejectorSampleConsensus () + : inlier_threshold_ (0.05) + , max_iterations_ (1000) // std::numeric_limits::max () + , input_ () + , input_transformed_ () + , target_ () + , refine_ (false) + , save_inliers_ (false) + { + rejection_name_ = "CorrespondenceRejectorSampleConsensus"; + } + + /** \brief Empty destructor. */ + ~CorrespondenceRejectorSampleConsensus () {} + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + inline void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + /** \brief Provide a source point cloud dataset (must contain XYZ data!) + * \param[in] cloud a cloud containing XYZ data + */ + virtual inline void + setInputSource (const PointCloudConstPtr &cloud) + { + input_ = cloud; + } + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudConstPtr const + getInputSource () { return (input_); } + + /** \brief Provide a target point cloud dataset (must contain XYZ data!) + * \param[in] cloud a cloud containing XYZ data + */ + virtual inline void + setInputTarget (const PointCloudConstPtr &cloud) { target_ = cloud; } + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudConstPtr const + getInputTarget () { return (target_ ); } + + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const override + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloudPtr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const override + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloudPtr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + /** \brief Set the maximum distance between corresponding points. + * Correspondences with distances below the threshold are considered as inliers. + * \param[in] threshold Distance threshold in the same dimension as source and target data sets. + */ + inline void + setInlierThreshold (double threshold) { inlier_threshold_ = threshold; }; + + /** \brief Get the maximum distance between corresponding points. + * \return Distance threshold in the same dimension as source and target data sets. + */ + inline double + getInlierThreshold () { return inlier_threshold_; }; + + /** \brief Set the maximum number of iterations. + * \param[in] max_iterations Maximum number if iterations to run + */ + inline void + setMaximumIterations (int max_iterations) { max_iterations_ = std::max (max_iterations, 0); } + + /** \brief Get the maximum number of iterations. + * \return max_iterations Maximum number if iterations to run + */ + inline int + getMaximumIterations () { return (max_iterations_); } + + /** \brief Get the best transformation after RANSAC rejection. + * \return The homogeneous 4x4 transformation yielding the largest number of inliers. + */ + inline Eigen::Matrix4f + getBestTransformation () { return best_transformation_; }; + + /** \brief Specify whether the model should be refined internally using the variance of the inliers + * \param[in] refine true if the model should be refined, false otherwise + */ + inline void + setRefineModel (const bool refine) + { + refine_ = refine; + } + + /** \brief Get the internal refine parameter value as set by the user using setRefineModel */ + inline bool + getRefineModel () const + { + return (refine_); + } + + /** \brief Get the inlier indices found by the correspondence rejector. This information is only saved if setSaveInliers(true) was called in advance. + * \param[out] inlier_indices Indices for the inliers + */ + inline void + getInliersIndices (std::vector &inlier_indices) { inlier_indices = inlier_indices_; } + + /** \brief Set whether to save inliers or not + * \param[in] s True to save inliers / False otherwise + */ + inline void + setSaveInliers (bool s) { save_inliers_ = s; } + + /** \brief Get whether the rejector is configured to save inliers */ + inline bool + getSaveInliers () { return save_inliers_; } + + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + double inlier_threshold_; + + int max_iterations_; + + PointCloudConstPtr input_; + PointCloudPtr input_transformed_; + PointCloudConstPtr target_; + + Eigen::Matrix4f best_transformation_; + + bool refine_; + std::vector inlier_indices_; + bool save_inliers_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_sample_consensus_2d.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_sample_consensus_2d.h new file mode 100755 index 0000000..d415cd4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_sample_consensus_2d.h @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceRejectorSampleConsensus2D implements a pixel-based + * correspondence rejection using Random Sample Consensus to identify inliers + * (and reject outliers) + * \author Radu B. Rusu + * \ingroup registration + */ + template + class CorrespondenceRejectorSampleConsensus2D: public CorrespondenceRejectorSampleConsensus + { + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + using CorrespondenceRejectorSampleConsensus::refine_; + using CorrespondenceRejectorSampleConsensus::input_; + using CorrespondenceRejectorSampleConsensus::target_; + using CorrespondenceRejectorSampleConsensus::input_correspondences_; + using CorrespondenceRejectorSampleConsensus::rejection_name_; + using CorrespondenceRejectorSampleConsensus::getClassName; + using CorrespondenceRejectorSampleConsensus::inlier_threshold_; + using CorrespondenceRejectorSampleConsensus::max_iterations_; + using CorrespondenceRejectorSampleConsensus::best_transformation_; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. Sets the inlier threshold to 5cm (0.05m), + * and the maximum number of iterations to 1000. + */ + CorrespondenceRejectorSampleConsensus2D () + : projection_matrix_ (Eigen::Matrix3f::Identity ()) + { + rejection_name_ = "CorrespondenceRejectorSampleConsensus2D"; + // Put the projection matrix together + //projection_matrix_ (0, 0) = 525.f; + //projection_matrix_ (1, 1) = 525.f; + //projection_matrix_ (0, 2) = 320.f; + //projection_matrix_ (1, 2) = 240.f; + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + inline void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences); + + /** \brief Sets the focal length parameters of the target camera. + * \param[in] fx the focal length in pixels along the x-axis of the image + * \param[in] fy the focal length in pixels along the y-axis of the image + */ + inline void + setFocalLengths (const float fx, const float fy) + { + projection_matrix_ (0, 0) = fx; + projection_matrix_ (1, 1) = fy; + } + + /** \brief Reads back the focal length parameters of the target camera. + * \param[out] fx the focal length in pixels along the x-axis of the image + * \param[out] fy the focal length in pixels along the y-axis of the image + */ + inline void + getFocalLengths (float &fx, float &fy) const + { + fx = projection_matrix_ (0, 0); + fy = projection_matrix_ (1, 1); + } + + + /** \brief Sets the camera center parameters of the target camera. + * \param[in] cx the x-coordinate of the camera center + * \param[in] cy the y-coordinate of the camera center + */ + inline void + setCameraCenters (const float cx, const float cy) + { + projection_matrix_ (0, 2) = cx; + projection_matrix_ (1, 2) = cy; + } + + /** \brief Reads back the camera center parameters of the target camera. + * \param[out] cx the x-coordinate of the camera center + * \param[out] cy the y-coordinate of the camera center + */ + inline void + getCameraCenters (float &cx, float &cy) const + { + cx = projection_matrix_ (0, 2); + cy = projection_matrix_ (1, 2); + } + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** \brief Camera projection matrix. */ + Eigen::Matrix3f projection_matrix_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_surface_normal.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_surface_normal.h new file mode 100755 index 0000000..0c9fba3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_surface_normal.h @@ -0,0 +1,322 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** + * @b CorrespondenceRejectorSurfaceNormal implements a simple correspondence + * rejection method based on the angle between the normals at correspondent points. + * + * \note If \ref setInputCloud and \ref setInputTarget are given, then the + * distances between correspondences will be estimated using the given XYZ + * data, and not read from the set of input correspondences. + * + * \author Aravindhan K Krishnan (original code from libpointmatcher: https://github.com/ethz-asl/libpointmatcher) + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectorSurfaceNormal: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. Sets the threshold to 1.0. */ + CorrespondenceRejectorSurfaceNormal () + : threshold_ (1.0) + { + rejection_name_ = "CorrespondenceRejectorSurfaceNormal"; + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + /** \brief Set the thresholding angle between the normals for correspondence rejection. + * \param[in] threshold cosine of the thresholding angle between the normals for rejection + */ + inline void + setThreshold (double threshold) { threshold_ = threshold; }; + + /** \brief Get the thresholding angle between the normals for correspondence rejection. */ + inline double + getThreshold () const { return threshold_; }; + + /** \brief Initialize the data container object for the point type and the normal type. */ + template inline void + initializeDataContainer () + { + data_container_.reset (new DataContainer); + } + + /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] input a cloud containing XYZ data + */ + template inline void + setInputCloud (const typename pcl::PointCloud::ConstPtr &input) + { + PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + boost::static_pointer_cast > (data_container_)->setInputSource (input); + } + + /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] input a cloud containing XYZ data + */ + template inline void + setInputSource (const typename pcl::PointCloud::ConstPtr &input) + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + boost::static_pointer_cast > (data_container_)->setInputSource (input); + } + + /** \brief Get the target input point cloud */ + template inline typename pcl::PointCloud::ConstPtr + getInputSource () const + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::getInputSource] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + return (boost::static_pointer_cast > (data_container_)->getInputSource ()); + } + + /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] target a cloud containing XYZ data + */ + template inline void + setInputTarget (const typename pcl::PointCloud::ConstPtr &target) + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::setInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + boost::static_pointer_cast > (data_container_)->setInputTarget (target); + } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + template inline void + setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, + bool force_no_recompute = false) + { + boost::static_pointer_cast< DataContainer > + (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); + } + + /** \brief Get the target input point cloud */ + template inline typename pcl::PointCloud::ConstPtr + getInputTarget () const + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::getInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + return (boost::static_pointer_cast > (data_container_)->getInputTarget ()); + } + + /** \brief Set the normals computed on the input point cloud + * \param[in] normals the normals computed for the input cloud + */ + template inline void + setInputNormals (const typename pcl::PointCloud::ConstPtr &normals) + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::setInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + boost::static_pointer_cast > (data_container_)->setInputNormals (normals); + } + + /** \brief Get the normals computed on the input point cloud */ + template inline typename pcl::PointCloud::Ptr + getInputNormals () const + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::getInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + return (boost::static_pointer_cast > (data_container_)->getInputNormals ()); + } + + /** \brief Set the normals computed on the target point cloud + * \param[in] normals the normals computed for the input cloud + */ + template inline void + setTargetNormals (const typename pcl::PointCloud::ConstPtr &normals) + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + boost::static_pointer_cast > (data_container_)->setTargetNormals (normals); + } + + /** \brief Get the normals computed on the target point cloud */ + template inline typename pcl::PointCloud::Ptr + getTargetNormals () const + { + if (!data_container_) + { + PCL_ERROR ("[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); + return; + } + return (boost::static_pointer_cast > (data_container_)->getTargetNormals ()); + } + + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const override + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const override + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + /** \brief See if this rejector requires source normals */ + bool + requiresSourceNormals () const override + { return (true); } + + /** \brief Blob method for setting the source normals */ + void + setSourceNormals (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputNormals (cloud); + } + + /** \brief See if this rejector requires target normals*/ + bool + requiresTargetNormals () const override + { return (true); } + + /** \brief Method for setting the target normals */ + void + setTargetNormals (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + if (!data_container_) + initializeDataContainer (); + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setTargetNormals (cloud); + } + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** \brief The median distance threshold between two correspondent points in source <-> target. */ + double threshold_; + + using DataContainerPtr = DataContainerInterface::Ptr; + /** \brief A pointer to the DataContainer object containing the input and target point clouds */ + DataContainerPtr data_container_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_trimmed.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_trimmed.h new file mode 100755 index 0000000..a88e522 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_trimmed.h @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** \brief CorrespondenceRejectorTrimmed implements a correspondence + * rejection for ICP-like registration algorithms that uses only the best + * 'k' correspondences where 'k' is some estimate of the overlap between + * the two point clouds being registered. + * + * Reference: + * 'The Trimmed Iterative Closest Point Algorithm' by + * D. Chetverikov, D. Svirko, D. Stepanov, and Pavel Krsek. + * In Proceedings of the 16th International Conference on Pattern + * Recognition (ICPR 2002). + * + * \author Dirk Holz + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectorTrimmed: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + CorrespondenceRejectorTrimmed () : + overlap_ratio_ (0.5f), + nr_min_correspondences_ (0) + { + rejection_name_ = "CorrespondenceRejectorTrimmed"; + } + + /** \brief Destructor. */ + ~CorrespondenceRejectorTrimmed () {} + + /** \brief Set the expected ratio of overlap between point clouds (in + * terms of correspondences). + * \param[in] ratio ratio of overlap between 0 (no overlap, no + * correspondences) and 1 (full overlap, all correspondences) + */ + virtual inline void + setOverlapRatio (float ratio) { overlap_ratio_ = std::min (1.0f, std::max (0.0f, ratio)); }; + + /** \brief Get the maximum distance used for thresholding in correspondence rejection. */ + inline float + getOverlapRatio () { return overlap_ratio_; }; + + /** \brief Set a minimum number of correspondences. If the specified overlap ratio causes to have + * less correspondences, \a CorrespondenceRejectorTrimmed will try to return at least + * \a nr_min_correspondences_ correspondences (or all correspondences in case \a nr_min_correspondences_ + * is less than the number of given correspondences). + * \param[in] min_correspondences the minimum number of correspondences + */ + inline void + setMinCorrespondences (unsigned int min_correspondences) { nr_min_correspondences_ = min_correspondences; }; + + /** \brief Get the minimum number of correspondences. */ + inline unsigned int + getMinCorrespondences () { return nr_min_correspondences_; }; + + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** Overlap Ratio in [0..1] */ + float overlap_ratio_; + + /** Minimum number of correspondences. */ + unsigned int nr_min_correspondences_; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_var_trimmed.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_var_trimmed.h new file mode 100755 index 0000000..c3750c2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_rejection_var_trimmed.h @@ -0,0 +1,252 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Perception, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +#include + +namespace pcl +{ + namespace registration + { + /** + * @b CorrespondenceRejectoVarTrimmed implements a simple correspondence + * rejection method by considering as inliers a certain percentage of correspondences + * with the least distances. The percentage of inliers is computed internally as mentioned + * in the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al' + * + * \note If \ref setInputCloud and \ref setInputTarget are given, then the + * distances between correspondences will be estimated using the given XYZ + * data, and not read from the set of input correspondences. + * + * \author Aravindhan K Krishnan. This code is ported from libpointmatcher (https://github.com/ethz-asl/libpointmatcher) + * \ingroup registration + */ + class PCL_EXPORTS CorrespondenceRejectorVarTrimmed: public CorrespondenceRejector + { + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + using CorrespondenceRejector::getClassName; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Empty constructor. */ + CorrespondenceRejectorVarTrimmed () : + trimmed_distance_ (0), + factor_ (), + min_ratio_ (0.05), + max_ratio_ (0.95), + lambda_ (0.95) + { + rejection_name_ = "CorrespondenceRejectorVarTrimmed"; + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + void + getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) override; + + /** \brief Get the trimmed distance used for thresholding in correspondence rejection. */ + inline double + getTrimmedDistance () const { return trimmed_distance_; }; + + /** \brief Provide a source point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + template inline void + setInputSource (const typename pcl::PointCloud::ConstPtr &cloud) + { + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + } + + /** \brief Provide a source point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] cloud a cloud containing XYZ data + */ + template inline void + setInputCloud (const typename pcl::PointCloud::ConstPtr &cloud) + { + PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + } + + /** \brief Provide a target point cloud dataset (must contain XYZ + * data!), used to compute the correspondence distance. + * \param[in] target a cloud containing XYZ data + */ + template inline void + setInputTarget (const typename pcl::PointCloud::ConstPtr &target) + { + if (!data_container_) + data_container_.reset (new DataContainer); + boost::static_pointer_cast > (data_container_)->setInputTarget (target); + } + + + + /** \brief See if this rejector requires source points */ + bool + requiresSourcePoints () const override + { return (true); } + + /** \brief Blob method for setting the source cloud */ + void + setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputSource (cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool + requiresTargetPoints () const override + { return (true); } + + /** \brief Method for setting the target cloud */ + void + setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override + { + PointCloud::Ptr cloud (new PointCloud); + fromPCLPointCloud2 (*cloud2, *cloud); + setInputTarget (cloud); + } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + template inline void + setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, + bool force_no_recompute = false) + { + boost::static_pointer_cast< DataContainer > + (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); + } + + /** \brief Get the computed inlier ratio used for thresholding in correspondence rejection. */ + inline double + getTrimFactor () const { return factor_; } + + /** brief set the minimum overlap ratio + * \param[in] ratio the overlap ratio [0..1] + */ + inline void + setMinRatio (double ratio) { min_ratio_ = ratio; } + + /** brief get the minimum overlap ratio + */ + inline double + getMinRatio () const { return min_ratio_; } + + /** brief set the maximum overlap ratio + * \param[in] ratio the overlap ratio [0..1] + */ + inline void + setMaxRatio (double ratio) { max_ratio_ = ratio; } + + /** brief get the maximum overlap ratio + */ + inline double + getMaxRatio () const { return max_ratio_; } + + protected: + + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void + applyRejection (pcl::Correspondences &correspondences) override + { + getRemainingCorrespondences (*input_correspondences_, correspondences); + } + + /** \brief The inlier distance threshold (based on the computed trim factor) between two correspondent points in source <-> target. + */ + double trimmed_distance_; + + /** \brief The factor for correspondence rejection. Only factor times the total points sorted based on + * the correspondence distances will be considered as inliers. Remaining points are rejected. This factor is + * computed internally + */ + double factor_; + + /** \brief The minimum overlap ratio between the input and target clouds + */ + double min_ratio_; + + /** \brief The maximum overlap ratio between the input and target clouds + */ + double max_ratio_; + + /** \brief part of the term that balances the root mean square difference. This is an internal parameter + */ + double lambda_; + + using DataContainerPtr = DataContainerInterface::Ptr; + + /** \brief A pointer to the DataContainer object containing the input and target point clouds */ + DataContainerPtr data_container_; + + private: + + /** \brief finds the optimal inlier ratio. This is based on the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al' + */ + inline float optimizeInlierRatio (std::vector &dists); + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_sorting.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_sorting.h new file mode 100755 index 0000000..fcb724f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_sorting.h @@ -0,0 +1,142 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include + +namespace pcl +{ + namespace registration + { + /** @b sortCorrespondencesByQueryIndex : a functor for sorting correspondences by query index + * \author Dirk Holz + * \ingroup registration + */ + struct sortCorrespondencesByQueryIndex + { + using first_argument_type = pcl::Correspondence; + using second_argument_type = pcl::Correspondence; + using result_type = bool; + bool + operator()( pcl::Correspondence a, pcl::Correspondence b) + { + return (a.index_query < b.index_query); + } + }; + + /** @b sortCorrespondencesByMatchIndex : a functor for sorting correspondences by match index + * \author Dirk Holz + * \ingroup registration + */ + struct sortCorrespondencesByMatchIndex + { + using first_argument_type = pcl::Correspondence; + using second_argument_type = pcl::Correspondence; + using result_type = bool; + bool + operator()( pcl::Correspondence a, pcl::Correspondence b) + { + return (a.index_match < b.index_match); + } + }; + + /** @b sortCorrespondencesByDistance : a functor for sorting correspondences by distance + * \author Dirk Holz + * \ingroup registration + */ + struct sortCorrespondencesByDistance + { + using first_argument_type = pcl::Correspondence; + using second_argument_type = pcl::Correspondence; + using result_type = bool; + bool + operator()( pcl::Correspondence a, pcl::Correspondence b) + { + return (a.distance < b.distance); + } + }; + + /** @b sortCorrespondencesByQueryIndexAndDistance : a functor for sorting correspondences by query index _and_ distance + * \author Dirk Holz + * \ingroup registration + */ + struct sortCorrespondencesByQueryIndexAndDistance + { + using first_argument_type = pcl::Correspondence; + using second_argument_type = pcl::Correspondence; + using result_type = bool; + inline bool + operator()( pcl::Correspondence a, pcl::Correspondence b) + { + if (a.index_query < b.index_query) + return (true); + else if ( (a.index_query == b.index_query) && (a.distance < b.distance) ) + return (true); + return (false); + } + }; + + /** @b sortCorrespondencesByMatchIndexAndDistance : a functor for sorting correspondences by match index _and_ distance + * \author Dirk Holz + * \ingroup registration + */ + struct sortCorrespondencesByMatchIndexAndDistance + { + using first_argument_type = pcl::Correspondence; + using second_argument_type = pcl::Correspondence; + using result_type = bool; + inline bool + operator()( pcl::Correspondence a, pcl::Correspondence b) + { + if (a.index_match < b.index_match) + return (true); + else if ( (a.index_match == b.index_match) && (a.distance < b.distance) ) + return (true); + return (false); + } + }; + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/correspondence_types.h b/deps_install/include/pcl-1.10/pcl/registration/correspondence_types.h new file mode 100755 index 0000000..7a3ad20 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/correspondence_types.h @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** \brief calculates the mean and standard deviation of descriptor distances from correspondences + * \param[in] correspondences list of correspondences + * \param[out] mean the mean descriptor distance of correspondences + * \param[out] stddev the standard deviation of descriptor distances. + * \note The sample variance is used to determine the standard deviation + */ + inline void + getCorDistMeanStd (const pcl::Correspondences& correspondences, double &mean, double &stddev); + + /** \brief extracts the query indices + * \param[in] correspondences list of correspondences + * \param[out] indices array of extracted indices. + * \note order of indices corresponds to input list of descriptor correspondences + */ + inline void + getQueryIndices (const pcl::Correspondences& correspondences, std::vector& indices); + + /** \brief extracts the match indices + * \param[in] correspondences list of correspondences + * \param[out] indices array of extracted indices. + * \note order of indices corresponds to input list of descriptor correspondences + */ + inline void + getMatchIndices (const pcl::Correspondences& correspondences, std::vector& indices); + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/default_convergence_criteria.h b/deps_install/include/pcl-1.10/pcl/registration/default_convergence_criteria.h new file mode 100755 index 0000000..64c0932 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/default_convergence_criteria.h @@ -0,0 +1,279 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b DefaultConvergenceCriteria represents an instantiation of + * ConvergenceCriteria, and implements the following criteria for registration loop + * evaluation: + * + * * a maximum number of iterations has been reached + * * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold) + * * the Mean Squared Error (MSE) between the current set of correspondences and the previous one is smaller than some threshold (both relative and absolute tests) + * + * \note Convergence is considered reached if ANY of the above criteria are met. + * + * \author Radu B. Rusu + * \ingroup registration + */ + template + class DefaultConvergenceCriteria : public ConvergenceCriteria + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = Eigen::Matrix; + + enum ConvergenceState + { + CONVERGENCE_CRITERIA_NOT_CONVERGED, + CONVERGENCE_CRITERIA_ITERATIONS, + CONVERGENCE_CRITERIA_TRANSFORM, + CONVERGENCE_CRITERIA_ABS_MSE, + CONVERGENCE_CRITERIA_REL_MSE, + CONVERGENCE_CRITERIA_NO_CORRESPONDENCES, + CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS + }; + + /** \brief Empty constructor. + * Sets: + * * the maximum number of iterations to 1000 + * * the rotation threshold to 0.256 degrees (0.99999) + * * the translation threshold to 0.0003 meters (3e-4^2) + * * the MSE relative / absolute thresholds to 0.001% and 1e-12 + * + * \param[in] iterations a reference to the number of iterations the loop has ran so far + * \param[in] transform a reference to the current transformation obtained by the transformation evaluation + * \param[in] correspondences a reference to the current set of point correspondences between source and target + */ + DefaultConvergenceCriteria (const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences) + : iterations_ (iterations) + , transformation_ (transform) + , correspondences_ (correspondences) + , correspondences_prev_mse_ (std::numeric_limits::max ()) + , correspondences_cur_mse_ (std::numeric_limits::max ()) + , max_iterations_ (100) // 100 iterations + , failure_after_max_iter_ (false) + , rotation_threshold_ (0.99999) // 0.256 degrees + , translation_threshold_ (3e-4 * 3e-4) // 0.0003 meters + , mse_threshold_relative_ (0.00001) // 0.001% of the previous MSE (relative error) + , mse_threshold_absolute_ (1e-12) // MSE (absolute error) + , iterations_similar_transforms_ (0) + , max_iterations_similar_transforms_ (0) + , convergence_state_ (CONVERGENCE_CRITERIA_NOT_CONVERGED) + { + } + + /** \brief Empty destructor */ + ~DefaultConvergenceCriteria () {} + + /** \brief Set the maximum number of consecutive iterations that the internal rotation, + * translation, and MSE differences are allowed to be similar. + * \param[in] nr_iterations the maximum number of iterations + */ + inline void + setMaximumIterationsSimilarTransforms (const int nr_iterations) { max_iterations_similar_transforms_ = nr_iterations; } + + /** \brief Get the maximum number of consecutive iterations that the internal rotation, + * translation, and MSE differences are allowed to be similar, as set by the user. + */ + inline int + getMaximumIterationsSimilarTransforms () const { return (max_iterations_similar_transforms_); } + + /** \brief Set the maximum number of iterations the internal optimization should run for. + * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for + */ + inline void + setMaximumIterations (const int nr_iterations) { max_iterations_ = nr_iterations; } + + /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */ + inline int + getMaximumIterations () const { return (max_iterations_); } + + /** \brief Specifies if the registration fails or converges when the maximum number of iterations is reached. + * \param[in] failure_after_max_iter If true, the registration fails. If false, the registration is assumed to have converged. + */ + inline void + setFailureAfterMaximumIterations (const bool failure_after_max_iter) { failure_after_max_iter_ = failure_after_max_iter; } + + /** \brief Get whether the registration will fail or converge when the maximum number of iterations is reached. */ + inline bool + getFailureAfterMaximumIterations () const { return (failure_after_max_iter_); } + + /** \brief Set the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. + * \param[in] threshold the rotation threshold in order for an optimization to be considered as having converged to the final solution. + */ + inline void + setRotationThreshold (const double threshold) { rotation_threshold_ = threshold; } + + /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user. + */ + inline double + getRotationThreshold () const { return (rotation_threshold_); } + + /** \brief Set the translation threshold (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. + * \param[in] threshold the translation threshold in order for an optimization to be considered as having converged to the final solution. + */ + inline void + setTranslationThreshold (const double threshold) { translation_threshold_ = threshold; } + + /** \brief Get the rotation threshold cosine angle (maximum allowable difference between two consecutive transformations) as set by the user. + */ + inline double + getTranslationThreshold () const { return (translation_threshold_); } + + /** \brief Set the relative MSE between two consecutive sets of correspondences. + * \param[in] mse_relative the relative MSE threshold + */ + inline void + setRelativeMSE (const double mse_relative) { mse_threshold_relative_ = mse_relative; } + + /** \brief Get the relative MSE between two consecutive sets of correspondences. */ + inline double + getRelativeMSE () const { return (mse_threshold_relative_); } + + /** \brief Set the absolute MSE between two consecutive sets of correspondences. + * \param[in] mse_absolute the relative MSE threshold + */ + inline void + setAbsoluteMSE (const double mse_absolute) { mse_threshold_absolute_ = mse_absolute; } + + /** \brief Get the absolute MSE between two consecutive sets of correspondences. */ + inline double + getAbsoluteMSE () const { return (mse_threshold_absolute_); } + + + /** \brief Check if convergence has been reached. */ + bool + hasConverged () override; + + /** \brief Return the convergence state after hasConverged () */ + ConvergenceState + getConvergenceState () + { + return (convergence_state_); + } + + /** \brief Sets the convergence state externally (for example, when ICP does not find + * enough correspondences to estimate a transformation, the function is called setting + * the convergence state to ConvergenceState::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES) + * \param[in] c the convergence state + */ + inline void + setConvergenceState(ConvergenceState c) + { + convergence_state_ = c; + } + + protected: + + /** \brief Calculate the mean squared error (MSE) of the distance for a given set of correspondences. + * \param[in] correspondences the given set of correspondences + */ + inline double + calculateMSE (const pcl::Correspondences &correspondences) const + { + double mse = 0; + for (const auto &correspondence : correspondences) + mse += correspondence.distance; + mse /= double (correspondences.size ()); + return (mse); + } + + /** \brief The number of iterations done by the registration loop so far. */ + const int &iterations_; + + /** \brief The current transformation obtained by the transformation estimation method. */ + const Matrix4 &transformation_; + + /** \brief The current set of point correspondences between the source and the target. */ + const pcl::Correspondences &correspondences_; + + /** \brief The MSE for the previous set of correspondences. */ + double correspondences_prev_mse_; + + /** \brief The MSE for the current set of correspondences. */ + double correspondences_cur_mse_; + + /** \brief The maximum nuyyGmber of iterations that the registration loop is to be executed. */ + int max_iterations_; + + /** \brief Specifys if the registration fails or converges when the maximum number of iterations is reached. */ + bool failure_after_max_iter_; + + /** \brief The rotation threshold is the relative rotation between two iterations (as angle cosine). */ + double rotation_threshold_; + + /** \brief The translation threshold is the relative translation between two iterations (0 if no translation). */ + double translation_threshold_; + + /** \brief The relative change from the previous MSE for the current set of correspondences, e.g. .1 means 10% change. */ + double mse_threshold_relative_; + + /** \brief The absolute change from the previous MSE for the current set of correspondences. */ + double mse_threshold_absolute_; + + /** \brief Internal counter for the number of iterations that the internal + * rotation, translation, and MSE differences are allowed to be similar. */ + int iterations_similar_transforms_; + + /** \brief The maximum number of iterations that the internal rotation, + * translation, and MSE differences are allowed to be similar. */ + int max_iterations_similar_transforms_; + + /** \brief The state of the convergence (e.g., why did the registration converge). */ + ConvergenceState convergence_state_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/distances.h b/deps_install/include/pcl-1.10/pcl/registration/distances.h new file mode 100755 index 0000000..b367c6b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/distances.h @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace distances + { + + /** \brief Compute the median value from a set of doubles + * \param[in] fvec the set of doubles + * \param[in] m the number of doubles in the set + */ + inline double + computeMedian (double *fvec, int m) + { + // Copy the values to vectors for faster sorting + std::vector data (m); + memcpy (&data[0], fvec, sizeof (double) * m); + + std::nth_element(data.begin(), data.begin() + (data.size () >> 1), data.end()); + return (data[data.size () >> 1]); + } + + /** \brief Use a Huber kernel to estimate the distance between two vectors + * \param[in] p_src the first eigen vector + * \param[in] p_tgt the second eigen vector + * \param[in] sigma the sigma value + */ + inline double + huber (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt, double sigma) + { + Eigen::Array4f diff = (p_tgt.array () - p_src.array ()).abs (); + double norm = 0.0; + for (int i = 0; i < 3; ++i) + { + if (diff[i] < sigma) + norm += diff[i] * diff[i]; + else + norm += 2.0 * sigma * diff[i] - sigma * sigma; + } + return (norm); + } + + /** \brief Use a Huber kernel to estimate the distance between two vectors + * \param[in] diff the norm difference between two vectors + * \param[in] sigma the sigma value + */ + inline double + huber (double diff, double sigma) + { + double norm = 0.0; + if (diff < sigma) + norm += diff * diff; + else + norm += 2.0 * sigma * diff - sigma * sigma; + return (norm); + } + + /** \brief Use a Gedikli kernel to estimate the distance between two vectors + * (for more information, see + * \param[in] val the norm difference between two vectors + * \param[in] clipping the clipping value + * \param[in] slope the slope. Default: 4 + */ + inline double + gedikli (double val, double clipping, double slope = 4) + { + return (1.0 / (1.0 + pow (std::abs(val) / clipping, slope))); + } + + /** \brief Compute the Manhattan distance between two eigen vectors. + * \param[in] p_src the first eigen vector + * \param[in] p_tgt the second eigen vector + */ + inline double + l1 (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) + { + return ((p_src.array () - p_tgt.array ()).abs ().sum ()); + } + + /** \brief Compute the Euclidean distance between two eigen vectors. + * \param[in] p_src the first eigen vector + * \param[in] p_tgt the second eigen vector + */ + inline double + l2 (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) + { + return ((p_src - p_tgt).norm ()); + } + + /** \brief Compute the squared Euclidean distance between two eigen vectors. + * \param[in] p_src the first eigen vector + * \param[in] p_tgt the second eigen vector + */ + inline double + l2Sqr (const Eigen::Vector4f &p_src, const Eigen::Vector4f &p_tgt) + { + return ((p_src - p_tgt).squaredNorm ()); + } + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/eigen.h b/deps_install/include/pcl-1.10/pcl/registration/eigen.h new file mode 100755 index 0000000..3f1c831 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/eigen.h @@ -0,0 +1,49 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/elch.h b/deps_install/include/pcl-1.10/pcl/registration/elch.h new file mode 100755 index 0000000..67f61ec --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/elch.h @@ -0,0 +1,254 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b ELCH (Explicit Loop Closing Heuristic) class + * \author Jochen Sprickerhof + * \ingroup registration + */ + template + class ELCH : public PCLBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + struct Vertex + { + Vertex () : cloud () {} + PointCloudPtr cloud; + Eigen::Affine3f transform; + }; + + /** \brief graph structure to hold the SLAM graph */ + using LoopGraph = boost::adjacency_list< + boost::listS, boost::eigen_vecS, boost::undirectedS, + Vertex, + boost::no_property>; + + using LoopGraphPtr = shared_ptr; + + using Registration = pcl::Registration; + using RegistrationPtr = typename Registration::Ptr; + using RegistrationConstPtr = typename Registration::ConstPtr; + + /** \brief Empty constructor. */ + ELCH () : + loop_graph_ (new LoopGraph), + loop_start_ (0), + loop_end_ (0), + reg_ (new pcl::IterativeClosestPoint), + compute_loop_ (true), + vd_ () + {}; + + /** \brief Empty destructor */ + ~ELCH () {} + + /** \brief Add a new point cloud to the internal graph. + * \param[in] cloud the new point cloud + */ + inline void + addPointCloud (PointCloudPtr cloud) + { + typename boost::graph_traits::vertex_descriptor vd = add_vertex (*loop_graph_); + (*loop_graph_)[vd].cloud = cloud; + if (num_vertices (*loop_graph_) > 1) + add_edge (vd_, vd, *loop_graph_); + vd_ = vd; + } + + /** \brief Getter for the internal graph. */ + inline LoopGraphPtr + getLoopGraph () + { + return (loop_graph_); + } + + /** \brief Setter for a new internal graph. + * \param[in] loop_graph the new graph + */ + inline void + setLoopGraph (LoopGraphPtr loop_graph) + { + loop_graph_ = loop_graph; + } + + /** \brief Getter for the first scan of a loop. */ + inline typename boost::graph_traits::vertex_descriptor + getLoopStart () + { + return (loop_start_); + } + + /** \brief Setter for the first scan of a loop. + * \param[in] loop_start the scan that starts the loop + */ + inline void + setLoopStart (const typename boost::graph_traits::vertex_descriptor &loop_start) + { + loop_start_ = loop_start; + } + + /** \brief Getter for the last scan of a loop. */ + inline typename boost::graph_traits::vertex_descriptor + getLoopEnd () + { + return (loop_end_); + } + + /** \brief Setter for the last scan of a loop. + * \param[in] loop_end the scan that ends the loop + */ + inline void + setLoopEnd (const typename boost::graph_traits::vertex_descriptor &loop_end) + { + loop_end_ = loop_end; + } + + /** \brief Getter for the registration algorithm. */ + inline RegistrationPtr + getReg () + { + return (reg_); + } + + /** \brief Setter for the registration algorithm. + * \param[in] reg the registration algorithm used to compute the transformation between the start and the end of the loop + */ + inline void + setReg (RegistrationPtr reg) + { + reg_ = reg; + } + + /** \brief Getter for the transformation between the first and the last scan. */ + inline Eigen::Matrix4f + getLoopTransform () + { + return (loop_transform_); + } + + /** \brief Setter for the transformation between the first and the last scan. + * \param[in] loop_transform the transformation between the first and the last scan + */ + inline void + setLoopTransform (const Eigen::Matrix4f &loop_transform) + { + loop_transform_ = loop_transform; + compute_loop_ = false; + } + + /** \brief Computes new poses for all point clouds by closing the loop + * between start and end point cloud. This will transform all given point + * clouds for now! + */ + void + compute (); + + protected: + using PCLBase::deinitCompute; + + /** \brief This method should get called before starting the actual computation. */ + virtual bool + initCompute (); + + private: + /** \brief graph structure for the internal optimization graph */ + using LOAGraph = boost::adjacency_list< + boost::listS, boost::vecS, boost::undirectedS, + boost::no_property, + boost::property< boost::edge_weight_t, double > >; + + /** + * graph balancer algorithm computes the weights + * @param[in] g the graph + * @param[in] f index of the first node + * @param[in] l index of the last node + * @param[out] weights array for the weights + */ + void + loopOptimizerAlgorithm (LOAGraph &g, double *weights); + + /** \brief The internal loop graph. */ + LoopGraphPtr loop_graph_; + + /** \brief The first scan of the loop. */ + typename boost::graph_traits::vertex_descriptor loop_start_; + + /** \brief The last scan of the loop. */ + typename boost::graph_traits::vertex_descriptor loop_end_; + + /** \brief The registration object used to close the loop. */ + RegistrationPtr reg_; + + /** \brief The transformation between that start and end of the loop. */ + Eigen::Matrix4f loop_transform_; + bool compute_loop_; + + /** \brief previously added node in the loop_graph_. */ + typename boost::graph_traits::vertex_descriptor vd_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/exceptions.h b/deps_install/include/pcl-1.10/pcl/registration/exceptions.h new file mode 100755 index 0000000..85fb8b2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/exceptions.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \class SolverDidntConvergeException + * \brief An exception that is thrown when the non linear solver didn't converge + */ + class PCL_EXPORTS SolverDidntConvergeException : public PCLException + { + public: + + SolverDidntConvergeException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; + + /** \class NotEnoughPointsException + * \brief An exception that is thrown when the number of correspondents is not equal + * to the minimum required + */ + class PCL_EXPORTS NotEnoughPointsException : public PCLException + { + public: + + NotEnoughPointsException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + } ; +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/gicp.h b/deps_install/include/pcl-1.10/pcl/registration/gicp.h new file mode 100755 index 0000000..887b004 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/gicp.h @@ -0,0 +1,370 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the + * generalized iterative closest point algorithm as described by Alex Segal et al. in + * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf + * The approach is based on using anistropic cost functions to optimize the alignment + * after closest point assignments have been made. + * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and + * FLANN. + * \author Nizar Sallem + * \ingroup registration + */ + template + class GeneralizedIterativeClosestPoint : public IterativeClosestPoint + { + public: + using IterativeClosestPoint::reg_name_; + using IterativeClosestPoint::getClassName; + using IterativeClosestPoint::indices_; + using IterativeClosestPoint::target_; + using IterativeClosestPoint::input_; + using IterativeClosestPoint::tree_; + using IterativeClosestPoint::tree_reciprocal_; + using IterativeClosestPoint::nr_iterations_; + using IterativeClosestPoint::max_iterations_; + using IterativeClosestPoint::previous_transformation_; + using IterativeClosestPoint::final_transformation_; + using IterativeClosestPoint::transformation_; + using IterativeClosestPoint::transformation_epsilon_; + using IterativeClosestPoint::converged_; + using IterativeClosestPoint::corr_dist_threshold_; + using IterativeClosestPoint::inlier_threshold_; + using IterativeClosestPoint::min_number_correspondences_; + using IterativeClosestPoint::update_visualizer_; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + using MatricesVector = std::vector< Eigen::Matrix3d, Eigen::aligned_allocator >; + using MatricesVectorPtr = shared_ptr; + using MatricesVectorConstPtr = shared_ptr; + + using InputKdTree = typename Registration::KdTree; + using InputKdTreePtr = typename Registration::KdTreePtr; + + using Ptr = shared_ptr< GeneralizedIterativeClosestPoint >; + using ConstPtr = shared_ptr< const GeneralizedIterativeClosestPoint >; + + + using Vector6d = Eigen::Matrix; + + /** \brief Empty constructor. */ + GeneralizedIterativeClosestPoint () + : k_correspondences_(20) + , gicp_epsilon_(0.001) + , rotation_epsilon_(2e-3) + , mahalanobis_(0) + , max_inner_iterations_(20) + { + min_number_correspondences_ = 4; + reg_name_ = "GeneralizedIterativeClosestPoint"; + max_iterations_ = 200; + transformation_epsilon_ = 5e-4; + corr_dist_threshold_ = 5.; + rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src, + const std::vector& indices_src, + const PointCloudTarget& cloud_tgt, + const std::vector& indices_tgt, + Eigen::Matrix4f& transformation_matrix) + { + estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); + }; + } + + /** \brief Provide a pointer to the input dataset + * \param cloud the const boost shared pointer to a PointCloud message + */ + inline void + setInputSource (const PointCloudSourceConstPtr &cloud) override + { + + if (cloud->points.empty ()) + { + PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); + return; + } + PointCloudSource input = *cloud; + // Set all the point.data[3] values to 1 to aid the rigid transformation + for (std::size_t i = 0; i < input.size (); ++i) + input[i].data[3] = 1.0; + + pcl::IterativeClosestPoint::setInputSource (cloud); + input_covariances_.reset (); + } + + /** \brief Provide a pointer to the covariances of the input source (if computed externally!). + * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. + * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). + * \param[in] target the input point cloud target + */ + inline void + setSourceCovariances (const MatricesVectorPtr& covariances) + { + input_covariances_ = covariances; + } + + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) + * \param[in] target the input point cloud target + */ + inline void + setInputTarget (const PointCloudTargetConstPtr &target) override + { + pcl::IterativeClosestPoint::setInputTarget(target); + target_covariances_.reset (); + } + + /** \brief Provide a pointer to the covariances of the input target (if computed externally!). + * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. + * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). + * \param[in] target the input point cloud target + */ + inline void + setTargetCovariances (const MatricesVectorPtr& covariances) + { + target_covariances_ = covariances; + } + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative + * non-linear Levenberg-Marquardt approach. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, + const std::vector &indices_src, + const PointCloudTarget &cloud_tgt, + const std::vector &indices_tgt, + Eigen::Matrix4f &transformation_matrix); + + /** \brief \return Mahalanobis distance matrix for the given point index */ + inline const Eigen::Matrix3d& mahalanobis(std::size_t index) const + { + assert(index < mahalanobis_.size()); + return mahalanobis_[index]; + } + + /** \brief Computes rotation matrix derivative. + * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5] + * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5] + * param x array representing 3D transformation + * param R rotation matrix + * param g gradient vector + */ + void + computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const; + + /** \brief Set the rotation epsilon (maximum allowable difference between two + * consecutive rotations) in order for an optimization to be considered as having + * converged to the final solution. + * \param epsilon the rotation epsilon + */ + inline void + setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; } + + /** \brief Get the rotation epsilon (maximum allowable difference between two + * consecutive rotations) as set by the user. + */ + inline double + getRotationEpsilon () { return (rotation_epsilon_); } + + /** \brief Set the number of neighbors used when selecting a point neighbourhood + * to compute covariances. + * A higher value will bring more accurate covariance matrix but will make + * covariances computation slower. + * \param k the number of neighbors to use when computing covariances + */ + void + setCorrespondenceRandomness (int k) { k_correspondences_ = k; } + + /** \brief Get the number of neighbors used when computing covariances as set by + * the user + */ + int + getCorrespondenceRandomness () { return (k_correspondences_); } + + /** set maximum number of iterations at the optimization step + * \param[in] max maximum number of iterations for the optimizer + */ + void + setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; } + + ///\return maximum number of iterations at the optimization step + int + getMaximumOptimizerIterations () { return (max_inner_iterations_); } + + protected: + + /** \brief The number of neighbors used for covariances computation. + * default: 20 + */ + int k_correspondences_; + + /** \brief The epsilon constant for gicp paper; this is NOT the convergence + * tolerance + * default: 0.001 + */ + double gicp_epsilon_; + + /** The epsilon constant for rotation error. (In GICP the transformation epsilon + * is split in rotation part and translation part). + * default: 2e-3 + */ + double rotation_epsilon_; + + /** \brief base transformation */ + Eigen::Matrix4f base_transformation_; + + /** \brief Temporary pointer to the source dataset. */ + const PointCloudSource *tmp_src_; + + /** \brief Temporary pointer to the target dataset. */ + const PointCloudTarget *tmp_tgt_; + + /** \brief Temporary pointer to the source dataset indices. */ + const std::vector *tmp_idx_src_; + + /** \brief Temporary pointer to the target dataset indices. */ + const std::vector *tmp_idx_tgt_; + + + /** \brief Input cloud points covariances. */ + MatricesVectorPtr input_covariances_; + + /** \brief Target cloud points covariances. */ + MatricesVectorPtr target_covariances_; + + /** \brief Mahalanobis matrices holder. */ + std::vector mahalanobis_; + + /** \brief maximum number of optimizations */ + int max_inner_iterations_; + + /** \brief compute points covariances matrices according to the K nearest + * neighbors. K is set via setCorrespondenceRandomness() method. + * \param cloud pointer to point cloud + * \param tree KD tree performer for nearest neighbors search + * \param[out] cloud_covariances covariances matrices for each point in the cloud + */ + template + void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, + const typename pcl::search::KdTree::Ptr tree, + MatricesVector& cloud_covariances); + + /** \return trace of mat1^t . mat2 + * \param mat1 matrix of dimension nxm + * \param mat2 matrix of dimension nxp + */ + inline double + matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const + { + double r = 0.; + std::size_t n = mat1.rows(); + // tr(mat1^t.mat2) + for(std::size_t i = 0; i < n; i++) + for(std::size_t j = 0; j < n; j++) + r += mat1 (j, i) * mat2 (i,j); + return r; + } + + /** \brief Rigid transformation computation method with initial guess. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess the initial guess of the transformation to compute + */ + void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override; + + /** \brief Search for the closest nearest neighbor of a given point. + * \param query the point to search a nearest neighbour for + * \param index vector of size 1 to store the index of the nearest neighbour found + * \param distance vector of size 1 to store the distance to nearest neighbour found + */ + inline bool + searchForNeighbors (const PointSource &query, std::vector& index, std::vector& distance) + { + int k = tree_->nearestKSearch (query, 1, index, distance); + if (k == 0) + return (false); + return (true); + } + + /// \brief compute transformation matrix from transformation matrix + void applyState(Eigen::Matrix4f &t, const Vector6d& x) const; + + /// \brief optimization functor structure + struct OptimizationFunctorWithIndices : public BFGSDummyFunctor + { + OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp) + : BFGSDummyFunctor (), gicp_(gicp) {} + double operator() (const Vector6d& x) override; + void df(const Vector6d &x, Vector6d &df) override; + void fdf(const Vector6d &x, double &f, Vector6d &df) override; + + const GeneralizedIterativeClosestPoint *gicp_; + }; + + std::function &cloud_src, + const std::vector &src_indices, + const pcl::PointCloud &cloud_tgt, + const std::vector &tgt_indices, + Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/gicp6d.h b/deps_install/include/pcl-1.10/pcl/registration/gicp6d.h new file mode 100755 index 0000000..590d8a6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/gicp6d.h @@ -0,0 +1,208 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + struct EIGEN_ALIGN16 _PointXYZLAB + { + PCL_ADD_POINT4D; // this adds the members x,y,z + union + { + struct + { + float L; + float a; + float b; + }; + float data_lab[4]; + }; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief A custom point type for position and CIELAB color value */ + struct PointXYZLAB : public _PointXYZLAB + { + inline PointXYZLAB () + { + x = y = z = 0.0f; data[3] = 1.0f; // important for homogeneous coordinates + L = a = b = 0.0f; data_lab[3] = 0.0f; + } + }; +} + +// register the custom point type in PCL +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::_PointXYZLAB, + (float, x, x) + (float, y, y) + (float, z, z) + (float, L, L) + (float, a, a) + (float, b, b) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB) + +namespace pcl +{ + /** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information into the + * Generalized Iterative Closest Point (GICP) algorithm. + * + * The suggested input is PointXYZRGBA. + * + * \note If you use this code in any academic work, please cite: + * + * - M. Korn, M. Holzkothen, J. Pauli + * Color Supported Generalized-ICP. + * In Proceedings of VISAPP 2014 - International Conference on Computer Vision Theory and Applications, + * Lisbon, Portugal, January 2014. + * + * \author Martin Holzkothen, Michael Korn + * \ingroup registration + */ + class PCL_EXPORTS GeneralizedIterativeClosestPoint6D : public GeneralizedIterativeClosestPoint + { + using PointSource = PointXYZRGBA; + using PointTarget = PointXYZRGBA; + + public: + + /** \brief constructor. + * + * \param[in] lab_weight the color weight + */ + GeneralizedIterativeClosestPoint6D (float lab_weight = 0.032f); + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud source + */ + void + setInputSource (const PointCloudSourceConstPtr& cloud) override; + + /** \brief Provide a pointer to the input target + * (e.g., the point cloud that we want to align the input source to) + * + * \param[in] cloud the input point cloud target + */ + void + setInputTarget (const PointCloudTargetConstPtr& target) override; + + protected: + + /** \brief Rigid transformation computation method with initial guess. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess the initial guess of the transformation to compute + */ + void + computeTransformation (PointCloudSource& output, + const Eigen::Matrix4f& guess) override; + + /** \brief Search for the closest nearest neighbor of a given point. + * \param query the point to search a nearest neighbour for + * \param index vector of size 1 to store the index of the nearest neighbour found + * \param distance vector of size 1 to store the distance to nearest neighbour found + */ + inline bool + searchForNeighbors (const PointXYZLAB& query, std::vector& index, std::vector& distance); + + protected: + /** \brief Holds the converted (LAB) data cloud. */ + pcl::PointCloud::Ptr cloud_lab_; + + /** \brief Holds the converted (LAB) model cloud. */ + pcl::PointCloud::Ptr target_lab_; + + /** \brief 6d-tree to search in model cloud. */ + KdTreeFLANN target_tree_lab_; + + /** \brief The color weight. */ + float lab_weight_; + + /** \brief Custom point representation to perform kdtree searches in more than 3 (i.e. in all 6) dimensions. */ + class MyPointRepresentation : public PointRepresentation + { + using PointRepresentation::nr_dimensions_; + using PointRepresentation::trivial_; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + MyPointRepresentation () + { + nr_dimensions_ = 6; + trivial_ = false; + } + + + ~MyPointRepresentation () + { + } + + inline Ptr + makeShared () const + { + return Ptr (new MyPointRepresentation (*this)); + } + + void + copyToFloatArray (const PointXYZLAB &p, float * out) const override + { + // copy all of the six values + out[0] = p.x; + out[1] = p.y; + out[2] = p.z; + out[3] = p.L; + out[4] = p.a; + out[5] = p.b; + } + }; + + /** \brief Enables 6d searches with kd-tree class using the color weight. */ + MyPointRepresentation point_rep_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/ia_fpcs.h b/deps_install/include/pcl-1.10/pcl/registration/ia_fpcs.h new file mode 100755 index 0000000..4a24ffd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/ia_fpcs.h @@ -0,0 +1,567 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel. + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief Compute the mean point density of a given point cloud. + * \param[in] cloud pointer to the input point cloud + * \param[in] max_dist maximum distance of a point to be considered as a neighbor + * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set) + * \return the mean point density of a given point cloud + */ + template inline float + getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, float max_dist, int nr_threads = 1); + + /** \brief Compute the mean point density of a given point cloud. + * \param[in] cloud pointer to the input point cloud + * \param[in] indices the vector of point indices to use from \a cloud + * \param[in] max_dist maximum distance of a point to be considered as a neighbor + * \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag is set) + * \return the mean point density of a given point cloud + */ + template inline float + getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, const std::vector &indices, + float max_dist, int nr_threads = 1); + + + namespace registration + { + /** \brief FPCSInitialAlignment computes corresponding four point congruent sets as described in: + * "4-points congruent sets for robust pairwise surface registration", Dror Aiger, Niloy Mitra, Daniel Cohen-Or. + * ACM Transactions on Graphics, vol. 27(3), 2008 + * \author P.W.Theiler + * \ingroup registration + */ + template + class FPCSInitialAlignment : public Registration + { + public: + /** \cond */ + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using KdTreeReciprocal = pcl::search::KdTree; + using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceIterator = typename PointCloudSource::iterator; + + using Normals = pcl::PointCloud; + using NormalsConstPtr = typename Normals::ConstPtr; + + using MatchingCandidate = pcl::registration::MatchingCandidate; + using MatchingCandidates = pcl::registration::MatchingCandidates; + /** \endcond */ + + + /** \brief Constructor. + * Resets the maximum number of iterations to 0 thus forcing an internal computation if not set by the user. + * Sets the number of RANSAC iterations to 1000 and the standard transformation estimation to TransformationEstimation3Point. + */ + FPCSInitialAlignment (); + + /** \brief Destructor. */ + ~FPCSInitialAlignment () + {}; + + + /** \brief Provide a pointer to the vector of target indices. + * \param[in] target_indices a pointer to the target indices + */ + inline void + setTargetIndices (const IndicesPtr &target_indices) + { + target_indices_ = target_indices; + }; + + /** \return a pointer to the vector of target indices. */ + inline IndicesPtr + getTargetIndices () const + { + return (target_indices_); + }; + + + /** \brief Provide a pointer to the normals of the source point cloud. + * \param[in] source_normals pointer to the normals of the source pointer cloud. + */ + inline void + setSourceNormals (const NormalsConstPtr &source_normals) + { + source_normals_ = source_normals; + }; + + /** \return the normals of the source point cloud. */ + inline NormalsConstPtr + getSourceNormals () const + { + return (source_normals_); + }; + + + /** \brief Provide a pointer to the normals of the target point cloud. + * \param[in] target_normals point to the normals of the target point cloud. + */ + inline void + setTargetNormals (const NormalsConstPtr &target_normals) + { + target_normals_ = target_normals; + }; + + /** \return the normals of the target point cloud. */ + inline NormalsConstPtr + getTargetNormals () const + { + return (target_normals_); + }; + + + /** \brief Set the number of used threads if OpenMP is activated. + * \param[in] nr_threads the number of used threads + */ + inline void + setNumberOfThreads (int nr_threads) + { + nr_threads_ = nr_threads; + }; + + /** \return the number of threads used if OpenMP is activated. */ + inline int + getNumberOfThreads () const + { + return (nr_threads_); + }; + + + /** \brief Set the constant factor delta which weights the internally calculated parameters. + * \param[in] delta the weight factor delta + * \param[in] normalize flag if delta should be normalized according to point cloud density + */ + inline void + setDelta (float delta, bool normalize = false) + { + delta_ = delta; + normalize_delta_ = normalize; + }; + + /** \return the constant factor delta which weights the internally calculated parameters. */ + inline float + getDelta () const + { + return (delta_); + }; + + + /** \brief Set the approximate overlap between source and target. + * \param[in] approx_overlap the estimated overlap + */ + inline void + setApproxOverlap (float approx_overlap) + { + approx_overlap_ = approx_overlap; + }; + + /** \return the approximated overlap between source and target. */ + inline float + getApproxOverlap () const + { + return (approx_overlap_); + }; + + + /** \brief Set the scoring threshold used for early finishing the method. + * \param[in] score_threshold early terminating score criteria + */ + inline void + setScoreThreshold (float score_threshold) + { + score_threshold_ = score_threshold; + }; + + /** \return the scoring threshold used for early finishing the method. */ + inline float + getScoreThreshold () const + { + return (score_threshold_); + }; + + + /** \brief Set the number of source samples to use during alignment. + * \param[in] nr_samples the number of source samples + */ + inline void + setNumberOfSamples (int nr_samples) + { + nr_samples_ = nr_samples; + }; + + /** \return the number of source samples to use during alignment. */ + inline int + getNumberOfSamples () const + { + return (nr_samples_); + }; + + + /** \brief Set the maximum normal difference between valid point correspondences in degree. + * \param[in] max_norm_diff the maximum difference in degree + */ + inline void + setMaxNormalDifference (float max_norm_diff) + { + max_norm_diff_ = max_norm_diff; + }; + + /** \return the maximum normal difference between valid point correspondences in degree. */ + inline float + getMaxNormalDifference () const + { + return (max_norm_diff_); + }; + + + /** \brief Set the maximum computation time in seconds. + * \param[in] max_runtime the maximum runtime of the method in seconds + */ + inline void + setMaxComputationTime (int max_runtime) + { + max_runtime_ = max_runtime; + }; + + /** \return the maximum computation time in seconds. */ + inline int + getMaxComputationTime () const + { + return (max_runtime_); + }; + + + /** \return the fitness score of the best scored four-point match. */ + inline float + getFitnessScore () const + { + return (fitness_score_); + }; + + protected: + + using PCLBase ::deinitCompute; + using PCLBase ::input_; + using PCLBase ::indices_; + + using Registration ::reg_name_; + using Registration ::target_; + using Registration ::tree_; + using Registration ::correspondences_; + using Registration ::target_cloud_updated_; + using Registration ::final_transformation_; + using Registration ::max_iterations_; + using Registration ::ransac_iterations_; + using Registration ::transformation_estimation_; + using Registration ::converged_; + + + /** \brief Rigid transformation computation method. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess The computed transforamtion + */ + void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override; + + + /** \brief Internal computation initialization. */ + virtual bool + initCompute (); + + /** \brief Select an approximately coplanar set of four points from the source cloud. + * \param[out] base_indices selected source cloud indices, further used as base (B) + * \param[out] ratio the two diagonal intersection ratios (r1,r2) of the base points + * \return + * * < 0 no coplanar four point sets with large enough sampling distance was found + * * = 0 a set of four congruent points was selected + */ + int + selectBase (std::vector &base_indices, float (&ratio)[2]); + + /** \brief Select randomly a triplet of points with large point-to-point distances. The minimum point + * sampling distance is calculated based on the estimated point cloud overlap during initialization. + * + * \param[out] base_indices indices of base B + * \return + * * < 0 no triangle with large enough base lines could be selected + * * = 0 base triangle succesully selected + */ + int + selectBaseTriangle (std::vector &base_indices); + + /** \brief Setup the base (four coplanar points) by ordering the points and computing intersection + * ratios and segment to segment distances of base diagonal. + * + * \param[in,out] base_indices indices of base B (will be reordered) + * \param[out] ratio diagonal intersection ratios of base points + */ + void + setupBase (std::vector &base_indices, float (&ratio)[2]); + + /** \brief Calculate intersection ratios and segment to segment distances of base diagonals. + * \param[in] base_indices indices of base B + * \param[out] ratio diagonal intersection ratios of base points + * \return quality value of diagonal intersection + */ + float + segmentToSegmentDist (const std::vector &base_indices, float (&ratio)[2]); + + /** \brief Search for corresponding point pairs given the distance between two base points. + * + * \param[in] idx1 first index of current base segment (in source cloud) + * \param[in] idx2 second index of current base segment (in source cloud) + * \param[out] pairs resulting point pairs with point-to-point distance close to ref_dist + * \return + * * < 0 no corresponding point pair was found + * * = 0 at least one point pair candidate was found + */ + virtual int + bruteForceCorrespondences (int idx1, int idx2, pcl::Correspondences &pairs); + + /** \brief Determine base matches by combining the point pair candidate and search for coinciding + * intersection points using the diagonal segment ratios of base B. The coincidation threshold is + * calculated during initialization (coincidation_limit_). + * + * \param[in] base_indices indices of base B + * \param[out] matches vector of candidate matches w.r.t the base B + * \param[in] pairs_a point pairs corresponding to points of 1st diagonal of base B + * \param[in] pairs_b point pairs corresponding to points of 2nd diagonal of base B + * \param[in] ratio diagonal intersection ratios of base points + * \return + * * < 0 no base match could be found + * * = 0 at least one base match was found + */ + virtual int + determineBaseMatches ( + const std::vector &base_indices, + std::vector > &matches, + const pcl::Correspondences &pairs_a, + const pcl::Correspondences &pairs_b, + const float (&ratio)[2]); + + /** \brief Check if outer rectangle distance of matched points fit with the base rectangle. + * + * \param[in] match_indices indices of match M + * \param[in] ds edge lengths of base B + * \return + * * < 0 at least one edge of the match M has no corresponding one in the base B + * * = 0 edges of match M fits to the ones of base B + */ + int + checkBaseMatch (const std::vector &match_indices, const float (&ds)[4]); + + /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the + * base and store the best fitting match (together with its score and estimated transformation). + * \note For forwards compatibility the results are stored in 'vectors of size 1'. + * + * \param[in] base_indices indices of base B + * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are + * reordered during this step. + * \param[out] candidates vector which contains the candidates matches M + */ + virtual void + handleMatches ( + const std::vector &base_indices, + std::vector > &matches, + MatchingCandidates &candidates); + + /** \brief Sets the correspondences between the base B and the match M by using the distance of each point + * to the centroid of the rectangle. + * + * \param[in] base_indices indices of base B + * \param[in] match_indices indices of match M + * \param[out] correspondences resulting correspondences + */ + virtual void + linkMatchWithBase ( + const std::vector &base_indices, + std::vector &match_indices, + pcl::Correspondences &correspondences); + + /** \brief Validate the matching by computing the transformation between the source and target based on the + * four matched points and by comparing the mean square error (MSE) to a threshold. The MSE limit was + * calculated during initialization (max_mse_). + * + * \param[in] base_indices indices of base B + * \param[in] match_indices indices of match M + * \param[in] correspondences corresondences between source and target + * \param[out] transformation resulting transformation matrix + * \return + * * < 0 MSE bigger than max_mse_ + * * = 0 MSE smaller than max_mse_ + */ + virtual int + validateMatch ( + const std::vector &base_indices, + const std::vector &match_indices, + const pcl::Correspondences &correspondences, + Eigen::Matrix4f &transformation); + + /** \brief Validate the transformation by calculating the number of inliers after transforming the source cloud. + * The resulting fitness score is later used as the decision criteria of the best fitting match. + * + * \param[out] transformation updated orientation matrix using all inliers + * \param[out] fitness_score current best fitness_score + * \note fitness score is only updated if the score of the current transformation exceeds the input one. + * \return + * * < 0 if previous result is better than the current one (score remains) + * * = 0 current result is better than the previous one (score updated) + */ + virtual int + validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score); + + /** \brief Final computation of best match out of vector of best matches. To avoid cross thread dependencies + * during parallel running, a best match for each try was calculated. + * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'. + * \param[in] candidates vector of candidate matches + */ + virtual void + finalCompute (const std::vector &candidates); + + + /** \brief Normals of source point cloud. */ + NormalsConstPtr source_normals_; + + /** \brief Normals of target point cloud. */ + NormalsConstPtr target_normals_; + + + /** \brief Number of threads for parallelization (standard = 1). + * \note Only used if run compiled with OpenMP. + */ + int nr_threads_; + + /** \brief Estimated overlap between source and target (standard = 0.5). */ + float approx_overlap_; + + /** \brief Delta value of 4pcs algorithm (standard = 1.0). + * It can be used as: + * * absolute value (normalization = false), value should represent the point accuracy to ensure finding neighbors between source <-> target + * * relative value (normalization = true), to adjust the internally calculated point accuracy (= point density) + */ + float delta_; + + /** \brief Score threshold to stop calculation with success. + * If not set by the user it is equal to the approximated overlap + */ + float score_threshold_; + + /** \brief The number of points to uniformly sample the source point cloud. (standard = 0 => full cloud). */ + int nr_samples_; + + /** \brief Maximum normal difference of corresponding point pairs in degrees (standard = 90). */ + float max_norm_diff_; + + /** \brief Maximum allowed computation time in seconds (standard = 0 => ~unlimited). */ + int max_runtime_; + + + /** \brief Resulting fitness score of the best match. */ + float fitness_score_; + + + /** \brief Estimated diamter of the target point cloud. */ + float diameter_; + + /** \brief Estimated squared metric overlap between source and target. + * \note Internally calculated using the estimated overlap and the extent of the source cloud. + * It is used to derive the minimum sampling distance of the base points as well as to calculated + * the number of tries to reliably find a correct match. + */ + float max_base_diameter_sqr_; + + /** \brief Use normals flag. */ + bool use_normals_; + + /** \brief Normalize delta flag. */ + bool normalize_delta_; + + + /** \brief A pointer to the vector of source point indices to use after sampling. */ + pcl::IndicesPtr source_indices_; + + /** \brief A pointer to the vector of target point indices to use after sampling. */ + pcl::IndicesPtr target_indices_; + + /** \brief Maximal difference between corresponding point pairs in source and target. + * \note Internally calculated using an estimation of the point density. + */ + float max_pair_diff_; + + /** \brief Maximal difference between the length of the base edges and valid match edges. + * \note Internally calculated using an estimation of the point density. + */ + float max_edge_diff_; + + /** \brief Maximal distance between coinciding intersection points to find valid matches. + * \note Internally calculated using an estimation of the point density. + */ + float coincidation_limit_; + + /** \brief Maximal mean squared errors of a transformation calculated from a candidate match. + * \note Internally calculated using an estimation of the point density. + */ + float max_mse_; + + /** \brief Maximal squared point distance between source and target points to count as inlier. + * \note Internally calculated using an estimation of the point density. + */ + float max_inlier_dist_sqr_; + + + /** \brief Definition of a small error. */ + const float small_error_; + + }; + }; // namespace registration +}; // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/ia_kfpcs.h b/deps_install/include/pcl-1.10/pcl/registration/ia_kfpcs.h new file mode 100755 index 0000000..400e602 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/ia_kfpcs.h @@ -0,0 +1,259 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** \brief KFPCSInitialAlignment computes corresponding four point congruent sets based on keypoints + * as described in: "Markerless point cloud registration with keypoint-based 4-points congruent sets", + * Pascal Theiler, Jan Dirk Wegner, Konrad Schindler. ISPRS Annals II-5/W2, 2013. Presented at ISPRS Workshop + * Laser Scanning, Antalya, Turkey, 2013. + * \note Method has since been improved and some variations to the paper exist. + * \author P.W.Theiler + * \ingroup registration + */ + template + class KFPCSInitialAlignment : public virtual FPCSInitialAlignment + { + public: + /** \cond */ + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceIterator = typename PointCloudSource::iterator; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetIterator = typename PointCloudTarget::iterator; + + using MatchingCandidate = pcl::registration::MatchingCandidate; + using MatchingCandidates = pcl::registration::MatchingCandidates; + /** \endcond */ + + + /** \brief Constructor. */ + KFPCSInitialAlignment (); + + /** \brief Destructor. */ + ~KFPCSInitialAlignment () + {}; + + + /** \brief Set the upper translation threshold used for score evaluation. + * \param[in] upper_trl_boundary upper translation threshold + */ + inline void + setUpperTranslationThreshold (float upper_trl_boundary) + { + upper_trl_boundary_ = upper_trl_boundary; + }; + + /** \return the upper translation threshold used for score evaluation. */ + inline float + getUpperTranslationThreshold () const + { + return (upper_trl_boundary_); + }; + + + /** \brief Set the lower translation threshold used for score evaluation. + * \param[in] lower_trl_boundary lower translation threshold + */ + inline void + setLowerTranslationThreshold (float lower_trl_boundary) + { + lower_trl_boundary_ = lower_trl_boundary; + }; + + /** \return the lower translation threshold used for score evaluation. */ + inline float + getLowerTranslationThreshold () const + { + return (lower_trl_boundary_); + }; + + + /** \brief Set the weighting factor of the translation cost term. + * \param[in] lambda the weighting factor of the translation cost term + */ + inline void + setLambda (float lambda) + { + lambda_ = lambda; + }; + + /** \return the weighting factor of the translation cost term. */ + inline float + getLambda () const + { + return (lambda_); + }; + + + /** \brief Get the N best unique candidate matches according to their fitness score. + * The method only returns unique transformations comparing the translation + * and the 3D rotation to already returned transformations. + * + * \note The method may return less than N candidates, if the number of unique candidates + * is smaller than N + * + * \param[in] n number of best candidates to return + * \param[in] min_angle3d minimum 3D angle difference in radian + * \param[in] min_translation3d minimum 3D translation difference + * \param[out] candidates vector of unique candidates + */ + void + getNBestCandidates (int n, float min_angle3d, float min_translation3d, MatchingCandidates &candidates); + + /** \brief Get all unique candidate matches with fitness scores above a threshold t. + * The method only returns unique transformations comparing the translation + * and the 3D rotation to already returned transformations. + * + * \param[in] t fitness score threshold + * \param[in] min_angle3d minimum 3D angle difference in radian + * \param[in] min_translation3d minimum 3D translation difference + * \param[out] candidates vector of unique candidates + */ + void + getTBestCandidates (float t, float min_angle3d, float min_translation3d, MatchingCandidates &candidates); + + + protected: + + using PCLBase ::deinitCompute; + using PCLBase ::input_; + using PCLBase ::indices_; + + using Registration ::reg_name_; + using Registration ::tree_; + using Registration ::final_transformation_; + using Registration ::ransac_iterations_; + using Registration ::correspondences_; + using Registration ::converged_; + + using FPCSInitialAlignment ::delta_; + using FPCSInitialAlignment ::approx_overlap_; + using FPCSInitialAlignment ::max_pair_diff_; + using FPCSInitialAlignment ::max_edge_diff_; + using FPCSInitialAlignment ::coincidation_limit_; + using FPCSInitialAlignment ::max_mse_; + using FPCSInitialAlignment ::max_inlier_dist_sqr_; + using FPCSInitialAlignment ::diameter_; + using FPCSInitialAlignment ::normalize_delta_; + using FPCSInitialAlignment ::fitness_score_; + using FPCSInitialAlignment ::score_threshold_; + using FPCSInitialAlignment ::linkMatchWithBase; + using FPCSInitialAlignment ::validateMatch; + + + /** \brief Internal computation initialization. */ + bool + initCompute () override; + + /** \brief Method to handle current candidate matches. Here we validate and evaluate the matches w.r.t the + * base and store the sorted matches (together with score values and estimated transformations). + * + * \param[in] base_indices indices of base B + * \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate matches are + * reordered during this step. + * \param[out] candidates vector which contains the candidates matches M + */ + void + handleMatches ( + const std::vector &base_indices, + std::vector > &matches, + MatchingCandidates &candidates) override; + + /** \brief Validate the transformation by calculating the score value after transforming the input source cloud. + * The resulting score is later used as the decision criteria of the best fitting match. + * + * \param[out] transformation updated orientation matrix using all inliers + * \param[out] fitness_score current best score + * \note fitness score is only updated if the score of the current transformation exceeds the input one. + * \return + * * < 0 if previous result is better than the current one (score remains) + * * = 0 current result is better than the previous one (score updated) + */ + int + validateTransformation (Eigen::Matrix4f &transformation, float &fitness_score) override; + + /** \brief Final computation of best match out of vector of matches. To avoid cross thread dependencies + * during parallel running, a best match for each try was calculated. + * \note For forwards compatibility the candidates are stored in vectors of 'vectors of size 1'. + * \param[in] candidates vector of candidate matches + */ + void + finalCompute (const std::vector &candidates) override; + + + /** \brief Lower boundary for translation costs calculation. + * \note If not set by the user, the translation costs are not used during evaluation. + */ + float lower_trl_boundary_; + + /** \brief Upper boundary for translation costs calculation. + * \note If not set by the user, it is calculated from the estimated overlap and the diameter + * of the point cloud. + */ + float upper_trl_boundary_; + + /** \brief Weighting factor for translation costs (standard = 0.5). */ + float lambda_; + + + /** \brief Container for resulting vector of registration candidates. */ + MatchingCandidates candidates_; + + /** \brief Flag if translation score should be used in validation (internal calculation). */ + bool use_trl_score_; + + /** \brief Subset of input indices on which we evaluate candidates. + * To speed up the evaluation, we only use a fix number of indices defined during initialization. + */ + pcl::IndicesPtr indices_validation_; + + }; + }; // namespace registration +}; // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/ia_ransac.h b/deps_install/include/pcl-1.10/pcl/registration/ia_ransac.h new file mode 100755 index 0000000..b920fde --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/ia_ransac.h @@ -0,0 +1,282 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in + * section IV of "Fast Point Feature Histograms (FPFH) for 3D Registration," Rusu et al. + * \author Michael Dixon, Radu B. Rusu + * \ingroup registration + */ + template + class SampleConsensusInitialAlignment : public Registration + { + public: + using Registration::reg_name_; + using Registration::input_; + using Registration::indices_; + using Registration::target_; + using Registration::final_transformation_; + using Registration::transformation_; + using Registration::corr_dist_threshold_; + using Registration::min_number_correspondences_; + using Registration::max_iterations_; + using Registration::tree_; + using Registration::transformation_estimation_; + using Registration::converged_; + using Registration::getClassName; + + using PointCloudSource = typename Registration::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename Registration::PointCloudTarget; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + using FeatureCloud = pcl::PointCloud; + using FeatureCloudPtr = typename FeatureCloud::Ptr; + using FeatureCloudConstPtr = typename FeatureCloud::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + class ErrorFunctor + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + virtual ~ErrorFunctor () = default; + virtual float operator () (float d) const = 0; + }; + + class HuberPenalty : public ErrorFunctor + { + private: + HuberPenalty () {} + public: + HuberPenalty (float threshold) : threshold_ (threshold) {} + virtual float operator () (float e) const + { + if (e <= threshold_) + return (0.5 * e*e); + return (0.5 * threshold_ * (2.0 * std::fabs (e) - threshold_)); + } + protected: + float threshold_; + }; + + class TruncatedError : public ErrorFunctor + { + private: + TruncatedError () {} + public: + ~TruncatedError () {} + + TruncatedError (float threshold) : threshold_ (threshold) {} + float operator () (float e) const override + { + if (e <= threshold_) + return (e / threshold_); + return (1.0); + } + protected: + float threshold_; + }; + + using ErrorFunctorPtr = typename ErrorFunctor::Ptr; + + using FeatureKdTreePtr = typename KdTreeFLANN::Ptr; + /** \brief Constructor. */ + SampleConsensusInitialAlignment () : + input_features_ (), target_features_ (), + nr_samples_(3), min_sample_distance_ (0.0f), k_correspondences_ (10), + feature_tree_ (new pcl::KdTreeFLANN), + error_functor_ () + { + reg_name_ = "SampleConsensusInitialAlignment"; + max_iterations_ = 1000; + + // Setting a non-std::numeric_limits::max () value to corr_dist_threshold_ to make it play nicely with TruncatedError + corr_dist_threshold_ = 100.0f; + transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD); + }; + + /** \brief Provide a shared pointer to the source point cloud's feature descriptors + * \param features the source point cloud's features + */ + void + setSourceFeatures (const FeatureCloudConstPtr &features); + + /** \brief Get a pointer to the source point cloud's features */ + inline FeatureCloudConstPtr const + getSourceFeatures () { return (input_features_); } + + /** \brief Provide a shared pointer to the target point cloud's feature descriptors + * \param features the target point cloud's features + */ + void + setTargetFeatures (const FeatureCloudConstPtr &features); + + /** \brief Get a pointer to the target point cloud's features */ + inline FeatureCloudConstPtr const + getTargetFeatures () { return (target_features_); } + + /** \brief Set the minimum distances between samples + * \param min_sample_distance the minimum distances between samples + */ + void + setMinSampleDistance (float min_sample_distance) { min_sample_distance_ = min_sample_distance; } + + /** \brief Get the minimum distances between samples, as set by the user */ + float + getMinSampleDistance () { return (min_sample_distance_); } + + /** \brief Set the number of samples to use during each iteration + * \param nr_samples the number of samples to use during each iteration + */ + void + setNumberOfSamples (int nr_samples) { nr_samples_ = nr_samples; } + + /** \brief Get the number of samples to use during each iteration, as set by the user */ + int + getNumberOfSamples () { return (nr_samples_); } + + /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will + * add more randomness to the feature matching. + * \param k the number of neighbors to use when selecting a random feature correspondence. + */ + void + setCorrespondenceRandomness (int k) { k_correspondences_ = k; } + + /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */ + int + getCorrespondenceRandomness () { return (k_correspondences_); } + + /** \brief Specify the error function to minimize + * \note This call is optional. TruncatedError will be used by default + * \param[in] error_functor a shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor + */ + void + setErrorFunction (const ErrorFunctorPtr & error_functor) { error_functor_ = error_functor; } + + /** \brief Get a shared pointer to the ErrorFunctor that is to be minimized + * \return A shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor + */ + ErrorFunctorPtr + getErrorFunction () { return (error_functor_); } + + protected: + /** \brief Choose a random index between 0 and n-1 + * \param n the number of possible indices to choose from + */ + inline int + getRandomIndex (int n) { return (static_cast (n * (rand () / (RAND_MAX + 1.0)))); }; + + /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are + * greater than a user-defined minimum distance, \a min_sample_distance. + * \param cloud the input point cloud + * \param nr_samples the number of samples to select + * \param min_sample_distance the minimum distance between any two samples + * \param sample_indices the resulting sample indices + */ + void + selectSamples (const PointCloudSource &cloud, int nr_samples, float min_sample_distance, + std::vector &sample_indices); + + /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to + * the sample points' features. From these, select one randomly which will be considered that sample point's + * correspondence. + * \param input_features a cloud of feature descriptors + * \param sample_indices the indices of each sample point + * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud + */ + void + findSimilarFeatures (const FeatureCloud &input_features, const std::vector &sample_indices, + std::vector &corresponding_indices); + + /** \brief An error metric for that computes the quality of the alignment between the given cloud and the target. + * \param cloud the input cloud + * \param threshold distances greater than this value are capped + */ + float + computeErrorMetric (const PointCloudSource &cloud, float threshold); + + /** \brief Rigid transformation computation method. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess The computed transforamtion + */ + void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override; + + /** \brief The source point cloud's feature descriptors. */ + FeatureCloudConstPtr input_features_; + + /** \brief The target point cloud's feature descriptors. */ + FeatureCloudConstPtr target_features_; + + /** \brief The number of samples to use during each iteration. */ + int nr_samples_; + + /** \brief The minimum distances between samples. */ + float min_sample_distance_; + + /** \brief The number of neighbors to use when selecting a random feature correspondence. */ + int k_correspondences_; + + /** \brief The KdTree used to compare feature descriptors. */ + FeatureKdTreePtr feature_tree_; + + ErrorFunctorPtr error_functor_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/icp.h b/deps_install/include/pcl-1.10/pcl/registration/icp.h new file mode 100755 index 0000000..7114cc3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/icp.h @@ -0,0 +1,404 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +// PCL includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. + * The transformation is estimated based on Singular Value Decomposition (SVD). + * + * The algorithm has several termination criteria: + * + *
    + *
  1. Number of iterations has reached the maximum user imposed number of iterations (via \ref setMaximumIterations)
    2. + *
  2. The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \ref setTransformationEpsilon)
    3. + *
  3. The sum of Euclidean squared errors is smaller than a user defined threshold (via \ref setEuclideanFitnessEpsilon)
    4. + *
+ * + * + * Usage example: + * \code + * IterativeClosestPoint icp; + * // Set the input source and target + * icp.setInputCloud (cloud_source); + * icp.setInputTarget (cloud_target); + * + * // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored) + * icp.setMaxCorrespondenceDistance (0.05); + * // Set the maximum number of iterations (criterion 1) + * icp.setMaximumIterations (50); + * // Set the transformation epsilon (criterion 2) + * icp.setTransformationEpsilon (1e-8); + * // Set the euclidean distance difference epsilon (criterion 3) + * icp.setEuclideanFitnessEpsilon (1); + * + * // Perform the alignment + * icp.align (cloud_source_registered); + * + * // Obtain the transformation that aligned cloud_source to cloud_source_registered + * Eigen::Matrix4f transformation = icp.getFinalTransformation (); + * \endcode + * + * \author Radu B. Rusu, Michael Dixon + * \ingroup registration + */ + template + class IterativeClosestPoint : public Registration + { + public: + using PointCloudSource = typename Registration::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename Registration::PointCloudTarget; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Registration::reg_name_; + using Registration::getClassName; + using Registration::input_; + using Registration::indices_; + using Registration::target_; + using Registration::nr_iterations_; + using Registration::max_iterations_; + using Registration::previous_transformation_; + using Registration::final_transformation_; + using Registration::transformation_; + using Registration::transformation_epsilon_; + using Registration::transformation_rotation_epsilon_; + using Registration::converged_; + using Registration::corr_dist_threshold_; + using Registration::inlier_threshold_; + using Registration::min_number_correspondences_; + using Registration::update_visualizer_; + using Registration::euclidean_fitness_epsilon_; + using Registration::correspondences_; + using Registration::transformation_estimation_; + using Registration::correspondence_estimation_; + using Registration::correspondence_rejectors_; + + typename pcl::registration::DefaultConvergenceCriteria::Ptr convergence_criteria_; + using Matrix4 = typename Registration::Matrix4; + + /** \brief Empty constructor. */ + IterativeClosestPoint () + : x_idx_offset_ (0) + , y_idx_offset_ (0) + , z_idx_offset_ (0) + , nx_idx_offset_ (0) + , ny_idx_offset_ (0) + , nz_idx_offset_ (0) + , use_reciprocal_correspondence_ (false) + , source_has_normals_ (false) + , target_has_normals_ (false) + { + reg_name_ = "IterativeClosestPoint"; + transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD ()); + correspondence_estimation_.reset (new pcl::registration::CorrespondenceEstimation); + convergence_criteria_.reset(new pcl::registration::DefaultConvergenceCriteria (nr_iterations_, transformation_, *correspondences_)); + }; + + /** \brief Empty destructor */ + ~IterativeClosestPoint () {} + + /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class. + * This allows to check the convergence state after the align() method as well as to configure + * DefaultConvergenceCriteria's parameters not available through the ICP API before the align() + * method is called. Please note that the align method sets max_iterations_, + * euclidean_fitness_epsilon_ and transformation_epsilon_ and therefore overrides the default / set + * values of the DefaultConvergenceCriteria instance. + * \return Pointer to the IterativeClosestPoint's DefaultConvergenceCriteria. + */ + inline typename pcl::registration::DefaultConvergenceCriteria::Ptr + getConvergeCriteria () + { + return convergence_criteria_; + } + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud source + */ + void + setInputSource (const PointCloudSourceConstPtr &cloud) override + { + Registration::setInputSource (cloud); + const auto fields = pcl::getFields (); + source_has_normals_ = false; + for (const auto &field : fields) + { + if (field.name == "x") x_idx_offset_ = field.offset; + else if (field.name == "y") y_idx_offset_ = field.offset; + else if (field.name == "z") z_idx_offset_ = field.offset; + else if (field.name == "normal_x") + { + source_has_normals_ = true; + nx_idx_offset_ = field.offset; + } + else if (field.name == "normal_y") + { + source_has_normals_ = true; + ny_idx_offset_ = field.offset; + } + else if (field.name == "normal_z") + { + source_has_normals_ = true; + nz_idx_offset_ = field.offset; + } + } + } + + /** \brief Provide a pointer to the input target + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud target + */ + void + setInputTarget (const PointCloudTargetConstPtr &cloud) override + { + Registration::setInputTarget (cloud); + const auto fields = pcl::getFields (); + target_has_normals_ = false; + for (const auto &field : fields) + { + if (field.name == "normal_x" || field.name == "normal_y" || field.name == "normal_z") + { + target_has_normals_ = true; + break; + } + } + } + + /** \brief Set whether to use reciprocal correspondence or not + * + * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence or not + */ + inline void + setUseReciprocalCorrespondences (bool use_reciprocal_correspondence) + { + use_reciprocal_correspondence_ = use_reciprocal_correspondence; + } + + /** \brief Obtain whether reciprocal correspondence are used or not */ + inline bool + getUseReciprocalCorrespondences () const + { + return (use_reciprocal_correspondence_); + } + + protected: + + /** \brief Apply a rigid transform to a given dataset. Here we check whether whether + * the dataset has surface normals in addition to XYZ, and rotate normals as well. + * \param[in] input the input point cloud + * \param[out] output the resultant output point cloud + * \param[in] transform a 4x4 rigid transformation + * \note Can be used with cloud_in equal to cloud_out + */ + virtual void + transformCloud (const PointCloudSource &input, + PointCloudSource &output, + const Matrix4 &transform); + + /** \brief Rigid transformation computation method with initial guess. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess the initial guess of the transformation to compute + */ + void + computeTransformation (PointCloudSource &output, const Matrix4 &guess) override; + + /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */ + virtual void + determineRequiredBlobData (); + + /** \brief XYZ fields offset. */ + std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_; + + /** \brief Normal fields offset. */ + std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_; + + /** \brief The correspondence type used for correspondence estimation. */ + bool use_reciprocal_correspondence_; + + /** \brief Internal check whether source dataset has normals or not. */ + bool source_has_normals_; + /** \brief Internal check whether target dataset has normals or not. */ + bool target_has_normals_; + + /** \brief Checks for whether estimators and rejectors need various data */ + bool need_source_blob_, need_target_blob_; + }; + + /** \brief @b IterativeClosestPointWithNormals is a special case of + * IterativeClosestPoint, that uses a transformation estimated based on + * Point to Plane distances by default. + * + * By default, this implementation uses the traditional point to plane objective + * and computes point to plane distances using the normals of the target point + * cloud. It also provides the option (through setUseSymmetricObjective) of + * using the symmetric objective function of [Rusinkiewicz 2019]. This objective + * uses the normals of both the source and target point cloud and has a similar + * computational cost to the traditional point to plane objective while also + * offering improved convergence speed and a wider basin of convergence. + * + * Note that this implementation not demean the point clouds which can lead + * to increased numerical error. If desired, a user can demean the point cloud, + * run iterative closest point, and composite the resulting ICP transformation + * with the translations from demeaning to obtain a transformation between + * the original point clouds. + * + * \author Radu B. Rusu, Matthew Cong + * \ingroup registration + */ + template + class IterativeClosestPointWithNormals : public IterativeClosestPoint + { + public: + using PointCloudSource = typename IterativeClosestPoint::PointCloudSource; + using PointCloudTarget = typename IterativeClosestPoint::PointCloudTarget; + using Matrix4 = typename IterativeClosestPoint::Matrix4; + + using IterativeClosestPoint::reg_name_; + using IterativeClosestPoint::transformation_estimation_; + using IterativeClosestPoint::correspondence_rejectors_; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor. */ + IterativeClosestPointWithNormals () + { + reg_name_ = "IterativeClosestPointWithNormals"; + setUseSymmetricObjective (false); + setEnforceSameDirectionNormals (true); + //correspondence_rejectors_.add + }; + + /** \brief Empty destructor */ + virtual ~IterativeClosestPointWithNormals () {} + + /** \brief Set whether to use a symmetric objective function or not + * + * \param[in] use_symmetric_objective whether to use a symmetric objective function or not + */ + inline void + setUseSymmetricObjective (bool use_symmetric_objective) + { + use_symmetric_objective_ = use_symmetric_objective; + if (use_symmetric_objective_) + { + auto symmetric_transformation_estimation = pcl::make_shared > (); + symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_); + transformation_estimation_ = symmetric_transformation_estimation; + } + else + { + transformation_estimation_.reset (new pcl::registration::TransformationEstimationPointToPlaneLLS ()); + } + } + + /** \brief Obtain whether a symmetric objective is used or not */ + inline bool + getUseSymmetricObjective () const + { + return use_symmetric_objective_; + } + + /** \brief Set whether or not to negate source or target normals on a per-point basis such that they point in the same direction. Only applicable to the symmetric objective function. + * + * \param[in] enforce_same_direction_normals whether to negate source or target normals on a per-point basis such that they point in the same direction. + */ + inline void + setEnforceSameDirectionNormals (bool enforce_same_direction_normals) + { + enforce_same_direction_normals_ = enforce_same_direction_normals; + auto symmetric_transformation_estimation = boost::dynamic_pointer_cast >(transformation_estimation_); + if (symmetric_transformation_estimation) + symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_); + } + + /** \brief Obtain whether source or target normals are negated on a per-point basis such that they point in the same direction or not */ + inline bool + getEnforceSameDirectionNormals () const + { + return enforce_same_direction_normals_; + } + + protected: + + /** \brief Apply a rigid transform to a given dataset + * \param[in] input the input point cloud + * \param[out] output the resultant output point cloud + * \param[in] transform a 4x4 rigid transformation + * \note Can be used with cloud_in equal to cloud_out + */ + virtual void + transformCloud (const PointCloudSource &input, + PointCloudSource &output, + const Matrix4 &transform); + + /** \brief Type of objective function (asymmetric vs. symmetric) used for transform estimation */ + bool use_symmetric_objective_; + /** \brief Whether or not to negate source and/or target normals such that they point in the same direction in the symmetric objective function */ + bool enforce_same_direction_normals_; + }; + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/icp_nl.h b/deps_install/include/pcl-1.10/pcl/registration/icp_nl.h new file mode 100755 index 0000000..4bffbd9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/icp_nl.h @@ -0,0 +1,90 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +// PCL includes +#include +#include + +namespace pcl +{ + /** \brief @b IterativeClosestPointNonLinear is an ICP variant that uses Levenberg-Marquardt optimization + * backend. The resultant transformation is optimized as a quaternion. + * + * The algorithm has several termination criteria: + * + *
    + *
  1. Number of iterations has reached the maximum user imposed number of iterations + * (via \ref setMaximumIterations)
    2. + *
  2. The epsilon (difference) between the previous transformation and the current estimated transformation is + * smaller than an user imposed value (via \ref setTransformationEpsilon)
    3. + *
  3. The sum of Euclidean squared errors is smaller than a user defined threshold + * (via \ref setEuclideanFitnessEpsilon)
    4. + *
+ * + * \author Radu B. Rusu, Michael Dixon + * \ingroup registration + */ + template + class IterativeClosestPointNonLinear : public IterativeClosestPoint + { + using IterativeClosestPoint::min_number_correspondences_; + using IterativeClosestPoint::reg_name_; + using IterativeClosestPoint::transformation_estimation_; + using IterativeClosestPoint::computeTransformation; + + public: + + using Ptr = shared_ptr< IterativeClosestPointNonLinear >; + using ConstPtr = shared_ptr< const IterativeClosestPointNonLinear >; + + using Matrix4 = typename Registration::Matrix4; + + /** \brief Empty constructor. */ + IterativeClosestPointNonLinear () + { + min_number_correspondences_ = 4; + reg_name_ = "IterativeClosestPointNonLinear"; + + transformation_estimation_.reset (new pcl::registration::TransformationEstimationLM); + } + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation.hpp new file mode 100755 index 0000000..3eae7d1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation.hpp @@ -0,0 +1,251 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimationBase::setInputTarget ( + const PointCloudTargetConstPtr &cloud) +{ + if (cloud->points.empty ()) + { + PCL_ERROR ("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); + return; + } + target_ = cloud; + + // Set the internal point representation of choice + if (point_representation_) + tree_->setPointRepresentation (point_representation_); + + target_cloud_updated_ = true; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::CorrespondenceEstimationBase::initCompute () +{ + if (!target_) + { + PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ()); + return (false); + } + + // Only update target kd-tree if a new target cloud was set + if (target_cloud_updated_ && !force_no_recompute_) + { + // If the target indices have been given via setIndicesTarget + if (target_indices_) + tree_->setInputCloud (target_, target_indices_); + else + tree_->setInputCloud (target_); + + target_cloud_updated_ = false; + } + + return (PCLBase::initCompute ()); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::CorrespondenceEstimationBase::initComputeReciprocal () +{ + // Only update source kd-tree if a new target cloud was set + if (source_cloud_updated_ && !force_no_recompute_reciprocal_) + { + if (point_representation_) + tree_reciprocal_->setPointRepresentation (point_representation_); + // If the target indices have been given via setIndicesTarget + if (indices_) + tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource()); + else + tree_reciprocal_->setInputCloud (getInputSource()); + + source_cloud_updated_ = false; + } + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimation::determineCorrespondences ( + pcl::Correspondences &correspondences, double max_distance) +{ + if (!initCompute ()) + return; + + double max_dist_sqr = max_distance * max_distance; + + correspondences.resize (indices_->size ()); + + std::vector index (1); + std::vector distance (1); + pcl::Correspondence corr; + unsigned int nr_valid_correspondences = 0; + + // Check if the template types are the same. If true, avoid a copy. + // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! + if (isSamePointType ()) + { + // Iterate over the input set of source indices + for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) + { + tree_->nearestKSearch (input_->points[*idx], 1, index, distance); + if (distance[0] > max_dist_sqr) + continue; + + corr.index_query = *idx; + corr.index_match = index[0]; + corr.distance = distance[0]; + correspondences[nr_valid_correspondences++] = corr; + } + } + else + { + PointTarget pt; + + // Iterate over the input set of source indices + for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) + { + // Copy the source data to a target PointTarget format so we can search in the tree + copyPoint (input_->points[*idx], pt); + + tree_->nearestKSearch (pt, 1, index, distance); + if (distance[0] > max_dist_sqr) + continue; + + corr.index_query = *idx; + corr.index_match = index[0]; + corr.distance = distance[0]; + correspondences[nr_valid_correspondences++] = corr; + } + } + correspondences.resize (nr_valid_correspondences); + deinitCompute (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimation::determineReciprocalCorrespondences ( + pcl::Correspondences &correspondences, double max_distance) +{ + if (!initCompute ()) + return; + + // setup tree for reciprocal search + // Set the internal point representation of choice + if (!initComputeReciprocal()) + return; + double max_dist_sqr = max_distance * max_distance; + + correspondences.resize (indices_->size()); + std::vector index (1); + std::vector distance (1); + std::vector index_reciprocal (1); + std::vector distance_reciprocal (1); + pcl::Correspondence corr; + unsigned int nr_valid_correspondences = 0; + int target_idx = 0; + + // Check if the template types are the same. If true, avoid a copy. + // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! + if (isSamePointType ()) + { + // Iterate over the input set of source indices + for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) + { + tree_->nearestKSearch (input_->points[*idx], 1, index, distance); + if (distance[0] > max_dist_sqr) + continue; + + target_idx = index[0]; + + tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); + if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0]) + continue; + + corr.index_query = *idx; + corr.index_match = index[0]; + corr.distance = distance[0]; + correspondences[nr_valid_correspondences++] = corr; + } + } + else + { + PointTarget pt_src; + PointSource pt_tgt; + + // Iterate over the input set of source indices + for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) + { + // Copy the source data to a target PointTarget format so we can search in the tree + copyPoint (input_->points[*idx], pt_src); + + tree_->nearestKSearch (pt_src, 1, index, distance); + if (distance[0] > max_dist_sqr) + continue; + + target_idx = index[0]; + + // Copy the target data to a target PointSource format so we can search in the tree_reciprocal + copyPoint (target_->points[target_idx], pt_tgt); + + tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal); + if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0]) + continue; + + corr.index_query = *idx; + corr.index_match = index[0]; + corr.distance = distance[0]; + correspondences[nr_valid_correspondences++] = corr; + } + } + correspondences.resize (nr_valid_correspondences); + deinitCompute (); +} + +//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation; + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_backprojection.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_backprojection.hpp new file mode 100755 index 0000000..fa80ca3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_backprojection.hpp @@ -0,0 +1,274 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::CorrespondenceEstimationBackProjection::initCompute () +{ + if (!source_normals_ || !target_normals_) + { + PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ()); + return (false); + } + + return (CorrespondenceEstimationBase::initCompute ()); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimationBackProjection::determineCorrespondences ( + pcl::Correspondences &correspondences, double max_distance) +{ + if (!initCompute ()) + return; + + correspondences.resize (indices_->size ()); + + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + float min_dist = std::numeric_limits::max (); + int min_index = 0; + + pcl::Correspondence corr; + unsigned int nr_valid_correspondences = 0; + + // Check if the template types are the same. If true, avoid a copy. + // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! + if (isSamePointType ()) + { + PointTarget pt; + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; + float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); + + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + else + { + PointTarget pt; + + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + PointSource pt_src; + // Copy the source data to a target PointTarget format so we can search in the tree + copyPoint (input_->points[*idx_i], pt_src); + + float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; + float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); + + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + correspondences.resize (nr_valid_correspondences); + deinitCompute (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimationBackProjection::determineReciprocalCorrespondences ( + pcl::Correspondences &correspondences, double max_distance) +{ + if (!initCompute ()) + return; + + // Set the internal point representation of choice + if(!initComputeReciprocal()) + return; + + correspondences.resize (indices_->size ()); + + std::vector nn_indices (k_); + std::vector nn_dists (k_); + std::vector index_reciprocal (1); + std::vector distance_reciprocal (1); + + float min_dist = std::numeric_limits::max (); + int min_index = 0; + + pcl::Correspondence corr; + unsigned int nr_valid_correspondences = 0; + int target_idx = 0; + + // Check if the template types are the same. If true, avoid a copy. + // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! + if (isSamePointType ()) + { + PointTarget pt; + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; + float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); + + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + // Check if the correspondence is reciprocal + target_idx = nn_indices[min_index]; + tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); + + if (*idx_i != index_reciprocal[0]) + continue; + + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + else + { + PointTarget pt; + + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + PointSource pt_src; + // Copy the source data to a target PointTarget format so we can search in the tree + copyPoint (input_->points[*idx_i], pt_src); + + float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; + float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); + + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + // Check if the correspondence is reciprocal + target_idx = nn_indices[min_index]; + tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); + + if (*idx_i != index_reciprocal[0]) + continue; + + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + correspondences.resize (nr_valid_correspondences); + deinitCompute (); +} + +#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp new file mode 100755 index 0000000..81e6e9e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp @@ -0,0 +1,310 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::CorrespondenceEstimationNormalShooting::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::initCompute ()); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimationNormalShooting::determineCorrespondences ( + pcl::Correspondences &correspondences, double max_distance) +{ + if (!initCompute ()) + return; + + correspondences.resize (indices_->size ()); + + std::vector nn_indices (k_); + std::vector nn_dists (k_); + + double min_dist = std::numeric_limits::max (); + int min_index = 0; + + pcl::Correspondence corr; + unsigned int nr_valid_correspondences = 0; + + // Check if the template types are the same. If true, avoid a copy. + // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! + if (isSamePointType ()) + { + PointTarget pt; + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + // computing the distance between a point and a line in 3d. + // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html + pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x; + pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y; + pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z; + + const NormalT &normal = source_normals_->points[*idx_i]; + Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); + Eigen::Vector3d V (pt.x, pt.y, pt.z); + Eigen::Vector3d C = N.cross (V); + + // Check if we have a better correspondence + double dist = C.dot (C); + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + else + { + PointTarget pt; + + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + PointSource pt_src; + // Copy the source data to a target PointTarget format so we can search in the tree + copyPoint (input_->points[*idx_i], pt_src); + + // computing the distance between a point and a line in 3d. + // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html + pt.x = target_->points[nn_indices[j]].x - pt_src.x; + pt.y = target_->points[nn_indices[j]].y - pt_src.y; + pt.z = target_->points[nn_indices[j]].z - pt_src.z; + + const NormalT &normal = source_normals_->points[*idx_i]; + Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); + Eigen::Vector3d V (pt.x, pt.y, pt.z); + Eigen::Vector3d C = N.cross (V); + + // Check if we have a better correspondence + double dist = C.dot (C); + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + correspondences.resize (nr_valid_correspondences); + deinitCompute (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimationNormalShooting::determineReciprocalCorrespondences ( + pcl::Correspondences &correspondences, double max_distance) +{ + if (!initCompute ()) + return; + + // setup tree for reciprocal search + // Set the internal point representation of choice + if (!initComputeReciprocal ()) + return; + + correspondences.resize (indices_->size ()); + + std::vector nn_indices (k_); + std::vector nn_dists (k_); + std::vector index_reciprocal (1); + std::vector distance_reciprocal (1); + + double min_dist = std::numeric_limits::max (); + int min_index = 0; + + pcl::Correspondence corr; + unsigned int nr_valid_correspondences = 0; + int target_idx = 0; + + // Check if the template types are the same. If true, avoid a copy. + // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! + if (isSamePointType ()) + { + PointTarget pt; + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + // computing the distance between a point and a line in 3d. + // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html + pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x; + pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y; + pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z; + + const NormalT &normal = source_normals_->points[*idx_i]; + Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); + Eigen::Vector3d V (pt.x, pt.y, pt.z); + Eigen::Vector3d C = N.cross (V); + + // Check if we have a better correspondence + double dist = C.dot (C); + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + // Check if the correspondence is reciprocal + target_idx = nn_indices[min_index]; + tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); + + if (*idx_i != index_reciprocal[0]) + continue; + + // Correspondence IS reciprocal, save it and continue + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + else + { + PointTarget pt; + + // Iterate over the input set of source indices + for (std::vector::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) + { + tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); + + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal + min_dist = std::numeric_limits::max (); + + // Find the best correspondence + for (std::size_t j = 0; j < nn_indices.size (); j++) + { + PointSource pt_src; + // Copy the source data to a target PointTarget format so we can search in the tree + copyPoint (input_->points[*idx_i], pt_src); + + // computing the distance between a point and a line in 3d. + // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html + pt.x = target_->points[nn_indices[j]].x - pt_src.x; + pt.y = target_->points[nn_indices[j]].y - pt_src.y; + pt.z = target_->points[nn_indices[j]].z - pt_src.z; + + const NormalT &normal = source_normals_->points[*idx_i]; + Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); + Eigen::Vector3d V (pt.x, pt.y, pt.z); + Eigen::Vector3d C = N.cross (V); + + // Check if we have a better correspondence + double dist = C.dot (C); + if (dist < min_dist) + { + min_dist = dist; + min_index = static_cast (j); + } + } + if (min_dist > max_distance) + continue; + + // Check if the correspondence is reciprocal + target_idx = nn_indices[min_index]; + tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); + + if (*idx_i != index_reciprocal[0]) + continue; + + // Correspondence IS reciprocal, save it and continue + corr.index_query = *idx_i; + corr.index_match = nn_indices[min_index]; + corr.distance = nn_dists[min_index];//min_dist; + correspondences[nr_valid_correspondences++] = corr; + } + } + correspondences.resize (nr_valid_correspondences); + deinitCompute (); +} + +#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_organized_projection.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_organized_projection.hpp new file mode 100755 index 0000000..557e5dd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_estimation_organized_projection.hpp @@ -0,0 +1,127 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_ +#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_ + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::CorrespondenceEstimationOrganizedProjection::initCompute () +{ + // Set the target_cloud_updated_ variable to true, so that the kd-tree is not built - it is not needed for this class + target_cloud_updated_ = false; + if (!CorrespondenceEstimationBase::initCompute ()) + return (false); + + /// Check if the target cloud is organized + if (!target_->isOrganized ()) + { + PCL_WARN ("[pcl::registration::%s::initCompute] Target cloud is not organized.\n", getClassName ().c_str ()); + return (false); + } + + /// Put the projection matrix together + projection_matrix_ (0, 0) = fx_; + projection_matrix_ (1, 1) = fy_; + projection_matrix_ (0, 2) = cx_; + projection_matrix_ (1, 2) = cy_; + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimationOrganizedProjection::determineCorrespondences ( + pcl::Correspondences &correspondences, + double max_distance) +{ + if (!initCompute ()) + return; + + correspondences.resize (indices_->size ()); + std::size_t c_index = 0; + + for (std::vector::const_iterator src_it = indices_->begin (); src_it != indices_->end (); ++src_it) + { + if (isFinite (input_->points[*src_it])) + { + Eigen::Vector4f p_src (src_to_tgt_transformation_ * input_->points[*src_it].getVector4fMap ()); + Eigen::Vector3f p_src3 (p_src[0], p_src[1], p_src[2]); + Eigen::Vector3f uv (projection_matrix_ * p_src3); + + /// Check if the point was behind the camera + if (uv[2] <= 0) + continue; + + int u = static_cast (uv[0] / uv[2]); + int v = static_cast (uv[1] / uv[2]); + + if (u >= 0 && u < static_cast (target_->width) && + v >= 0 && v < static_cast (target_->height)) + { + const PointTarget &pt_tgt = target_->at (u, v); + if (!isFinite (pt_tgt)) + continue; + /// Check if the depth difference is larger than the threshold + if (std::abs (uv[2] - pt_tgt.z) > depth_threshold_) + continue; + + double dist = (p_src3 - pt_tgt.getVector3fMap ()).norm (); + if (dist < max_distance) + correspondences[c_index++] = pcl::Correspondence (*src_it, v * target_->width + u, static_cast (dist)); + } + } + } + + correspondences.resize (c_index); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceEstimationOrganizedProjection::determineReciprocalCorrespondences ( + pcl::Correspondences &correspondences, + double max_distance) +{ + // Call the normal determineCorrespondences (...), as doing it both ways will not improve the results + determineCorrespondences (correspondences, max_distance); +} + +#endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_distance.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_distance.hpp new file mode 100755 index 0000000..e7a174f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_distance.hpp @@ -0,0 +1,44 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_ + + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_features.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_features.hpp new file mode 100755 index 0000000..96916ba --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_features.hpp @@ -0,0 +1,105 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_ + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::CorrespondenceRejectorFeatures::setSourceFeature ( + const typename pcl::PointCloud::ConstPtr &source_feature, const std::string &key) +{ + if (features_map_.count (key) == 0) + features_map_[key].reset (new FeatureContainer); + boost::static_pointer_cast > (features_map_[key])->setSourceFeature (source_feature); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline typename pcl::PointCloud::ConstPtr +pcl::registration::CorrespondenceRejectorFeatures::getSourceFeature (const std::string &key) +{ + if (features_map_.count (key) == 0) + return (nullptr); + return (boost::static_pointer_cast > (features_map_[key])->getSourceFeature ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::CorrespondenceRejectorFeatures::setTargetFeature ( + const typename pcl::PointCloud::ConstPtr &target_feature, const std::string &key) +{ + if (features_map_.count (key) == 0) + features_map_[key].reset (new FeatureContainer); + boost::static_pointer_cast > (features_map_[key])->setTargetFeature (target_feature); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline typename pcl::PointCloud::ConstPtr +pcl::registration::CorrespondenceRejectorFeatures::getTargetFeature (const std::string &key) +{ + if (features_map_.count (key) == 0) + return (nullptr); + return (boost::static_pointer_cast > (features_map_[key])->getTargetFeature ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::CorrespondenceRejectorFeatures::setDistanceThreshold ( + double thresh, const std::string &key) +{ + if (features_map_.count (key) == 0) + features_map_[key].reset (new FeatureContainer); + boost::static_pointer_cast > (features_map_[key])->setDistanceThreshold (thresh); +} + + + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::CorrespondenceRejectorFeatures::setFeatureRepresentation ( + const typename pcl::PointRepresentation::ConstPtr &fr, + const std::string &key) +{ + if (features_map_.count (key) == 0) + features_map_[key].reset (new FeatureContainer); + boost::static_pointer_cast > (features_map_[key])->setFeatureRepresentation (fr); +} + + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_median_distance.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_median_distance.hpp new file mode 100755 index 0000000..2884d25 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_median_distance.hpp @@ -0,0 +1,44 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_HPP_ + + +#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_MEDIAN_DISTANCE_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_one_to_one.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_one_to_one.hpp new file mode 100755 index 0000000..656b6db --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_one_to_one.hpp @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ONE_TO_ONE_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ONE_TO_ONE_HPP_ + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ONE_TO_ONE_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_organized_boundary.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_organized_boundary.hpp new file mode 100755 index 0000000..a6821e4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_organized_boundary.hpp @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) Alexandru-Eugen Ichim + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ORGANIZED_BOUNDARY_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ORGANIZED_BOUNDARY_HPP_ + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_ORGANIZED_BOUNDARY_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_poly.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_poly.hpp new file mode 100755 index 0000000..dac077e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_poly.hpp @@ -0,0 +1,241 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceRejectorPoly::getRemainingCorrespondences ( + const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) +{ + // This is reset after all the checks below + remaining_correspondences = original_correspondences; + + // Check source/target + if (!input_) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No source was input! Returning all input correspondences.\n", + getClassName ().c_str ()); + return; + } + + if (!target_) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No target was input! Returning all input correspondences.\n", + getClassName ().c_str ()); + return; + } + + // Check cardinality + if (cardinality_ < 2) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Polygon cardinality too low!. Returning all input correspondences.\n", + getClassName ().c_str() ); + return; + } + + // Number of input correspondences + const int nr_correspondences = static_cast (original_correspondences.size ()); + + // Not enough correspondences for polygonal rejections + if (cardinality_ >= nr_correspondences) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Number of correspondences smaller than polygon cardinality! Returning all input correspondences.\n", + getClassName ().c_str() ); + return; + } + + // Check similarity + if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length similarity - must be in [0,1]!. Returning all input correspondences.\n", + getClassName ().c_str() ); + return; + } + + // Similarity, squared + similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_; + + // Initialization of result + remaining_correspondences.clear (); + remaining_correspondences.reserve (nr_correspondences); + + // Number of times a correspondence is sampled and number of times it was accepted + std::vector num_samples (nr_correspondences, 0); + std::vector num_accepted (nr_correspondences, 0); + + // Main loop + for (int i = 0; i < iterations_; ++i) + { + // Sample cardinality_ correspondences without replacement + const std::vector idx = getUniqueRandomIndices (nr_correspondences, cardinality_); + + // Verify the polygon similarity + if (thresholdPolygon (original_correspondences, idx)) + { + // Increment sample counter and accept counter + for (int j = 0; j < cardinality_; ++j) + { + ++num_samples[ idx[j] ]; + ++num_accepted[ idx[j] ]; + } + } + else + { + // Not accepted, only increment sample counter + for (int j = 0; j < cardinality_; ++j) + ++num_samples[ idx[j] ]; + } + } + + // Now calculate the acceptance rate of each correspondence + std::vector accept_rate (nr_correspondences, 0.0f); + for (int i = 0; i < nr_correspondences; ++i) + { + const int numsi = num_samples[i]; + if (numsi == 0) + accept_rate[i] = 0.0f; + else + accept_rate[i] = static_cast (num_accepted[i]) / static_cast (numsi); + } + + // Compute a histogram in range [0,1] for acceptance rates + const int hist_size = nr_correspondences / 2; // TODO: Optimize this + const std::vector histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size); + + // Find the cut point between outliers and inliers using Otsu's thresholding method + const int cut_idx = findThresholdOtsu (histogram); + const float cut = static_cast (cut_idx) / static_cast (hist_size); + + // Threshold + for (int i = 0; i < nr_correspondences; ++i) + if (accept_rate[i] > cut) + remaining_correspondences.push_back (original_correspondences[i]); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector +pcl::registration::CorrespondenceRejectorPoly::computeHistogram (const std::vector& data, + float lower, float upper, int bins) +{ + // Result + std::vector result (bins, 0); + + // Last index into result and increment factor from data value --> index + const int last_idx = bins - 1; + const float idx_per_val = static_cast (bins) / (upper - lower); + + // Accumulate + for (const float &value : data) + ++result[ std::min (last_idx, int (value*idx_per_val)) ]; + + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::CorrespondenceRejectorPoly::findThresholdOtsu (const std::vector& histogram) +{ + // Precision + const double eps = std::numeric_limits::epsilon(); + + // Histogram dimension + const int nbins = static_cast (histogram.size ()); + + // Mean and inverse of the number of data points + double mean = 0.0; + double sum_inv = 0.0; + for (int i = 0; i < nbins; ++i) + { + mean += static_cast (i * histogram[i]); + sum_inv += static_cast (histogram[i]); + } + sum_inv = 1.0/sum_inv; + mean *= sum_inv; + + // Probability and mean of class 1 (data to the left of threshold) + double class_mean1 = 0.0; + double class_prob1 = 0.0; + double class_prob2 = 1.0; + + // Maximized between class variance and associated bin value + double between_class_variance_max = 0.0; + int result = 0; + + // Loop over all bin values + for (int i = 0; i < nbins; ++i) + { + class_mean1 *= class_prob1; + + // Probability of bin i + const double prob_i = static_cast (histogram[i]) * sum_inv; + + // Class probability 1: sum of probabilities from 0 to i + class_prob1 += prob_i; + + // Class probability 2: sum of probabilities from i+1 to nbins-1 + class_prob2 -= prob_i; + + // Avoid division by zero below + if (std::min (class_prob1,class_prob2) < eps || std::max (class_prob1,class_prob2) > 1.0-eps) + continue; + + // Class mean 1: sum of probabilities from 0 to i, weighted by bin value + class_mean1 = (class_mean1 + static_cast (i) * prob_i) / class_prob1; + + // Class mean 2: sum of probabilities from i+1 to nbins-1, weighted by bin value + const double class_mean2 = (mean - class_prob1*class_mean1) / class_prob2; + + // Between class variance + const double between_class_variance = class_prob1 * class_prob2 + * (class_mean1 - class_mean2) + * (class_mean1 - class_mean2); + + // If between class variance is maximized, update result + if (between_class_variance > between_class_variance_max) + { + between_class_variance_max = between_class_variance; + result = i; + } + } + + return (result); +} + +#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp new file mode 100755 index 0000000..d8a4073 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp @@ -0,0 +1,136 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences ( + const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) +{ + if (!input_) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ()); + return; + } + + if (!target_) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ()); + return; + } + + if (save_inliers_) + inlier_indices_.clear (); + + int nr_correspondences = static_cast (original_correspondences.size ()); + std::vector source_indices (nr_correspondences); + std::vector target_indices (nr_correspondences); + + // Copy the query-match indices + for (std::size_t i = 0; i < original_correspondences.size (); ++i) + { + source_indices[i] = original_correspondences[i].index_query; + target_indices[i] = original_correspondences[i].index_match; + } + + { + // From the set of correspondences found, attempt to remove outliers + // Create the registration model + using SampleConsensusModelRegistrationPtr = typename pcl::SampleConsensusModelRegistration::Ptr; + SampleConsensusModelRegistrationPtr model; + model.reset (new pcl::SampleConsensusModelRegistration (input_, source_indices)); + // Pass the target_indices + model->setInputTarget (target_, target_indices); + // Create a RANSAC model + pcl::RandomSampleConsensus sac (model, inlier_threshold_); + sac.setMaxIterations (max_iterations_); + + // Compute the set of inliers + if (!sac.computeModel ()) + { + remaining_correspondences = original_correspondences; + best_transformation_.setIdentity (); + return; + } + if (refine_ && !sac.refineModel ()) + { + PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n"); + return; + } + + std::vector inliers; + sac.getInliers (inliers); + + if (inliers.size () < 3) + { + remaining_correspondences = original_correspondences; + best_transformation_.setIdentity (); + return; + } + std::unordered_map index_to_correspondence; + for (int i = 0; i < nr_correspondences; ++i) + index_to_correspondence[original_correspondences[i].index_query] = i; + + remaining_correspondences.resize (inliers.size ()); + for (std::size_t i = 0; i < inliers.size (); ++i) + remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; + + if (save_inliers_) + { + inlier_indices_.reserve (inliers.size ()); + for (const int &inlier : inliers) + inlier_indices_.push_back (index_to_correspondence[inlier]); + } + + // get best transformation + Eigen::VectorXf model_coefficients; + sac.getModelCoefficients (model_coefficients); + best_transformation_.row (0) = model_coefficients.segment<4>(0); + best_transformation_.row (1) = model_coefficients.segment<4>(4); + best_transformation_.row (2) = model_coefficients.segment<4>(8); + best_transformation_.row (3) = model_coefficients.segment<4>(12); + } +} + +#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp new file mode 100755 index 0000000..12357df --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp @@ -0,0 +1,131 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ + +#include +#include + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::CorrespondenceRejectorSampleConsensus2D::getRemainingCorrespondences ( + const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) +{ + if (!input_) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ()); + return; + } + + if (!target_) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ()); + return; + } + + if (projection_matrix_ == Eigen::Matrix3f::Identity ()) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Intrinsic camera parameters not given!\n", getClassName ().c_str ()); + return; + } + + int nr_correspondences = static_cast (original_correspondences.size ()); + std::vector source_indices (nr_correspondences); + std::vector target_indices (nr_correspondences); + + // Copy the query-match indices + for (std::size_t i = 0; i < original_correspondences.size (); ++i) + { + source_indices[i] = original_correspondences[i].index_query; + target_indices[i] = original_correspondences[i].index_match; + } + + // From the set of correspondences found, attempt to remove outliers + typename pcl::SampleConsensusModelRegistration2D::Ptr model (new pcl::SampleConsensusModelRegistration2D (input_, source_indices)); + // Pass the target_indices + model->setInputTarget (target_, target_indices); + model->setProjectionMatrix (projection_matrix_); + + // Create a RANSAC model + pcl::RandomSampleConsensus sac (model, inlier_threshold_); + sac.setMaxIterations (max_iterations_); + + // Compute the set of inliers + if (!sac.computeModel ()) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Error computing model! Returning the original correspondences...\n", getClassName ().c_str ()); + remaining_correspondences = original_correspondences; + best_transformation_.setIdentity (); + return; + } + if (refine_ && !sac.refineModel (2.0)) + PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n", getClassName ().c_str ()); + + std::vector inliers; + sac.getInliers (inliers); + + if (inliers.size () < 3) + { + PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Less than 3 correspondences found!\n", getClassName ().c_str ()); + remaining_correspondences = original_correspondences; + best_transformation_.setIdentity (); + return; + } + + std::unordered_map index_to_correspondence; + for (int i = 0; i < nr_correspondences; ++i) + index_to_correspondence[original_correspondences[i].index_query] = i; + + remaining_correspondences.resize (inliers.size ()); + for (std::size_t i = 0; i < inliers.size (); ++i) + remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; + + // get best transformation + Eigen::VectorXf model_coefficients; + sac.getModelCoefficients (model_coefficients); + best_transformation_.row (0) = model_coefficients.segment<4>(0); + best_transformation_.row (1) = model_coefficients.segment<4>(4); + best_transformation_.row (2) = model_coefficients.segment<4>(8); + best_transformation_.row (3) = model_coefficients.segment<4>(12); +} + +#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_surface_normal.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_surface_normal.hpp new file mode 100755 index 0000000..6509909 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_surface_normal.hpp @@ -0,0 +1,42 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_HPP_ + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_trimmed.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_trimmed.hpp new file mode 100755 index 0000000..0690e1d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_trimmed.hpp @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_TRIMMED_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_TRIMMED_HPP_ + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_TRIMMED_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_var_trimmed.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_var_trimmed.hpp new file mode 100755 index 0000000..53cd52f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_rejection_var_trimmed.hpp @@ -0,0 +1,42 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_VAR_TRIMMED_HPP_ +#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_VAR_TRIMMED_HPP_ + +#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_VAR_TRIMMED_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_types.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_types.hpp new file mode 100755 index 0000000..bc89e93 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/correspondence_types.hpp @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include +#include + +inline void +pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev) +{ + if (correspondences.empty ()) + return; + + double sum = 0, sq_sum = 0; + + for (const auto &correspondence : correspondences) + { + sum += correspondence.distance; + sq_sum += correspondence.distance * correspondence.distance; + } + mean = sum / static_cast (correspondences.size ()); + double variance = (sq_sum - sum * sum / static_cast (correspondences.size ())) / static_cast (correspondences.size () - 1); + stddev = sqrt (variance); +} + +inline void +pcl::registration::getQueryIndices (const pcl::Correspondences& correspondences, std::vector& indices) +{ + indices.resize (correspondences.size ()); + for (std::size_t i = 0; i < correspondences.size (); ++i) + indices[i] = correspondences[i].index_query; +} + + +inline void +pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, std::vector& indices) +{ + indices.resize (correspondences.size ()); + for (std::size_t i = 0; i < correspondences.size (); ++i) + indices[i] = correspondences[i].index_match; +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/default_convergence_criteria.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/default_convergence_criteria.hpp new file mode 100755 index 0000000..1dac278 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/default_convergence_criteria.hpp @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_ +#define PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::DefaultConvergenceCriteria::hasConverged () +{ + if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED) + { + //If it already converged or failed before, reset. + iterations_similar_transforms_ = 0; + convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED; + } + + bool is_similar = false; + + PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", iterations_, max_iterations_); + // 1. Number of iterations has reached the maximum user imposed number of iterations + if (iterations_ >= max_iterations_) + { + if (!failure_after_max_iter_) + { + convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS; + return (true); + } + convergence_state_ = CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS; + } + + // 2. The epsilon (difference) between the previous transformation and the current estimated transformation + double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1); + double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) + + transformation_.coeff (1, 3) * transformation_.coeff (1, 3) + + transformation_.coeff (2, 3) * transformation_.coeff (2, 3); + PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave %f rotation (cosine) and %f translation.\n", cos_angle, translation_sqr); + + if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_) + { + if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) + { + convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM; + return (true); + } + is_similar = true; + } + + correspondences_cur_mse_ = calculateMSE (correspondences_); + PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: %f / %f.\n", correspondences_prev_mse_, correspondences_cur_mse_); + + // 3. The relative sum of Euclidean squared errors is smaller than a user defined threshold + // Absolute + if (std::abs (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_) + { + if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) + { + convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE; + return (true); + } + is_similar = true; + } + + // Relative + if (std::abs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_) + { + if (iterations_similar_transforms_ >= max_iterations_similar_transforms_) + { + convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE; + return (true); + } + is_similar = true; + } + + if (is_similar) + { + // Increment the number of transforms that the thresholds are allowed to be similar + ++iterations_similar_transforms_; + } + else + { + // When the transform becomes large, reset. + iterations_similar_transforms_ = 0; + } + + correspondences_prev_mse_ = correspondences_cur_mse_; + + return (false); +} + +#endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/elch.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/elch.hpp new file mode 100755 index 0000000..4b234f5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/elch.hpp @@ -0,0 +1,276 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_IMPL_ELCH_H_ +#define PCL_REGISTRATION_IMPL_ELCH_H_ + +#include +#include +#include + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::ELCH::loopOptimizerAlgorithm (LOAGraph &g, double *weights) +{ + std::list crossings, branches; + crossings.push_back (static_cast (loop_start_)); + crossings.push_back (static_cast (loop_end_)); + weights[loop_start_] = 0; + weights[loop_end_] = 1; + + int *p = new int[num_vertices (g)]; + int *p_min = new int[num_vertices (g)]; + double *d = new double[num_vertices (g)]; + double *d_min = new double[num_vertices (g)]; + bool do_swap = false; + std::list::iterator start_min, end_min; + + // process all junctions + while (!crossings.empty ()) + { + double dist = -1; + // find shortest crossing for all vertices on the loop + for (auto crossings_it = crossings.begin (); crossings_it != crossings.end (); ) + { + dijkstra_shortest_paths (g, *crossings_it, + predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))). + distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g)))); + + auto end_it = crossings_it; + end_it++; + // find shortest crossing for one vertex + for (; end_it != crossings.end (); end_it++) + { + if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist)) + { + dist = d[*end_it]; + start_min = crossings_it; + end_min = end_it; + do_swap = true; + } + } + if (do_swap) + { + std::swap (p, p_min); + std::swap (d, d_min); + do_swap = false; + } + // vertex starts a branch + if (dist < 0) + { + branches.push_back (static_cast (*crossings_it)); + crossings_it = crossings.erase (crossings_it); + } + else + crossings_it++; + } + + if (dist > -1) + { + remove_edge (*end_min, p_min[*end_min], g); + for (int i = p_min[*end_min]; i != *start_min; i = p_min[i]) + { + //even right with weights[*start_min] > weights[*end_min]! (math works) + weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min]; + remove_edge (i, p_min[i], g); + if (degree (i, g) > 0) + { + crossings.push_back (i); + } + } + + if (degree (*start_min, g) == 0) + crossings.erase (start_min); + + if (degree (*end_min, g) == 0) + crossings.erase (end_min); + } + } + + delete[] p; + delete[] p_min; + delete[] d; + delete[] d_min; + + boost::graph_traits::adjacency_iterator adjacent_it, adjacent_it_end; + + // error propagation + while (!branches.empty ()) + { + int s = branches.front (); + branches.pop_front (); + + for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it) + { + weights[*adjacent_it] = weights[s]; + if (degree (*adjacent_it, g) > 1) + branches.push_back (static_cast (*adjacent_it)); + } + clear_vertex (s, g); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::ELCH::initCompute () +{ + /*if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n"); + return (false); + }*/ //TODO + + if (loop_end_ == 0) + { + PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\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::adjacency_iterator si, si_end; + for (std::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++) + *meta_start += *(*loop_graph_)[*si].cloud; + + for (std::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_->setInputSource (meta_end); + + reg_->align (*tmp); + + loop_transform_ = reg_->getFinalTransformation (); + //TODO hack + //loop_transform_ *= trans.matrix (); + + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::ELCH::compute () +{ + if (!initCompute ()) + { + return; + } + + LOAGraph grb[4]; + + typename boost::graph_traits::edge_iterator edge_it, edge_it_end; + for (std::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++) + { + for (auto &j : grb) + add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, j); //TODO add variance + } + + double *weights[4]; + for (int i = 0; i < 4; i++) + { + weights[i] = new double[num_vertices (*loop_graph_)]; + loopOptimizerAlgorithm (grb[i], weights[i]); + } + + //TODO use pose + //Eigen::Vector4f cend; + //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend); + //Eigen::Translation3f tend (cend.head (3)); + //Eigen::Affine3f aend (tend); + //Eigen::Affine3f aendI = aend.inverse (); + + //TODO iterate ovr loop_graph_ + //typename boost::graph_traits::vertex_iterator vertex_it, vertex_it_end; + //for (std::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++) + for (std::size_t i = 0; i < num_vertices (*loop_graph_); i++) + { + Eigen::Vector3f t2; + t2[0] = loop_transform_ (0, 3) * static_cast (weights[0][i]); + t2[1] = loop_transform_ (1, 3) * static_cast (weights[1][i]); + t2[2] = loop_transform_ (2, 3) * static_cast (weights[2][i]); + + Eigen::Affine3f bl (loop_transform_); + Eigen::Quaternionf q (bl.rotation ()); + Eigen::Quaternionf q2; + q2 = Eigen::Quaternionf::Identity ().slerp (static_cast (weights[3][i]), q); + + //TODO use rotation from branch start + Eigen::Translation3f t3 (t2); + Eigen::Affine3f a (t3 * q2); + //a = aend * a * aendI; + + pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a); + (*loop_graph_)[i].transform = a; + } + + add_edge (loop_start_, loop_end_, *loop_graph_); + + deinitCompute (); +} + +#endif // PCL_REGISTRATION_IMPL_ELCH_H_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/gicp.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/gicp.hpp new file mode 100755 index 0000000..234b28e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/gicp.hpp @@ -0,0 +1,479 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_GICP_HPP_ +#define PCL_REGISTRATION_IMPL_GICP_HPP_ + +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////// +template +template void +pcl::GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, + const typename pcl::search::KdTree::Ptr kdtree, + MatricesVector& cloud_covariances) +{ + if (k_correspondences_ > int (cloud->size ())) + { + PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_); + return; + } + + Eigen::Vector3d mean; + std::vector nn_indecies; nn_indecies.reserve (k_correspondences_); + std::vector nn_dist_sq; nn_dist_sq.reserve (k_correspondences_); + + // We should never get there but who knows + if(cloud_covariances.size () < cloud->size ()) + cloud_covariances.resize (cloud->size ()); + + MatricesVector::iterator matrices_iterator = cloud_covariances.begin (); + for(auto points_iterator = cloud->begin (); + points_iterator != cloud->end (); + ++points_iterator, ++matrices_iterator) + { + const PointT &query_point = *points_iterator; + Eigen::Matrix3d &cov = *matrices_iterator; + // Zero out the cov and mean + cov.setZero (); + mean.setZero (); + + // Search for the K nearest neighbours + kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq); + + // Find the covariance matrix + for(int j = 0; j < k_correspondences_; j++) { + const PointT &pt = (*cloud)[nn_indecies[j]]; + + mean[0] += pt.x; + mean[1] += pt.y; + mean[2] += pt.z; + + cov(0,0) += pt.x*pt.x; + + cov(1,0) += pt.y*pt.x; + cov(1,1) += pt.y*pt.y; + + cov(2,0) += pt.z*pt.x; + cov(2,1) += pt.z*pt.y; + cov(2,2) += pt.z*pt.z; + } + + mean /= static_cast (k_correspondences_); + // Get the actual covariance + for (int k = 0; k < 3; k++) + for (int l = 0; l <= k; l++) + { + cov(k,l) /= static_cast (k_correspondences_); + cov(k,l) -= mean[k]*mean[l]; + cov(l,k) = cov(k,l); + } + + // Compute the SVD (covariance matrix is symmetric so U = V') + Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); + cov.setZero (); + Eigen::Matrix3d U = svd.matrixU (); + // Reconstitute the covariance matrix with modified singular values using the column // vectors in V. + for(int k = 0; k < 3; k++) { + Eigen::Vector3d col = U.col(k); + double v = 1.; // biggest 2 singular values replaced by 1 + if(k == 2) // smallest singular value replaced by gicp_epsilon + v = gicp_epsilon_; + cov+= v * col * col.transpose(); + } + } +} + +//////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GeneralizedIterativeClosestPoint::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const +{ + Eigen::Matrix3d dR_dPhi; + Eigen::Matrix3d dR_dTheta; + Eigen::Matrix3d dR_dPsi; + + double phi = x[3], theta = x[4], psi = x[5]; + + double cphi = std::cos(phi), sphi = sin(phi); + double ctheta = std::cos(theta), stheta = sin(theta); + double cpsi = std::cos(psi), spsi = sin(psi); + + dR_dPhi(0,0) = 0.; + dR_dPhi(1,0) = 0.; + dR_dPhi(2,0) = 0.; + + dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta; + dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta; + dR_dPhi(2,1) = cphi*ctheta; + + dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta; + dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta; + dR_dPhi(2,2) = -ctheta*sphi; + + dR_dTheta(0,0) = -cpsi*stheta; + dR_dTheta(1,0) = -spsi*stheta; + dR_dTheta(2,0) = -ctheta; + + dR_dTheta(0,1) = cpsi*ctheta*sphi; + dR_dTheta(1,1) = ctheta*sphi*spsi; + dR_dTheta(2,1) = -sphi*stheta; + + dR_dTheta(0,2) = cphi*cpsi*ctheta; + dR_dTheta(1,2) = cphi*ctheta*spsi; + dR_dTheta(2,2) = -cphi*stheta; + + dR_dPsi(0,0) = -ctheta*spsi; + dR_dPsi(1,0) = cpsi*ctheta; + dR_dPsi(2,0) = 0.; + + dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta; + dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta; + dR_dPsi(2,1) = 0.; + + dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta; + dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta; + dR_dPsi(2,2) = 0.; + + g[3] = matricesInnerProd(dR_dPhi, R); + g[4] = matricesInnerProd(dR_dTheta, R); + g[5] = matricesInnerProd(dR_dPsi, R); +} + +//////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, + const std::vector &indices_src, + const PointCloudTarget &cloud_tgt, + const std::vector &indices_tgt, + Eigen::Matrix4f &transformation_matrix) +{ + if (indices_src.size () < 4) // need at least 4 samples + { + PCL_THROW_EXCEPTION (NotEnoughPointsException, + "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!"); + return; + } + // Set the initial solution + Vector6d x = Vector6d::Zero (); + x[0] = transformation_matrix (0,3); + x[1] = transformation_matrix (1,3); + x[2] = transformation_matrix (2,3); + x[3] = std::atan2 (transformation_matrix (2,1), transformation_matrix (2,2)); + x[4] = asin (-transformation_matrix (2,0)); + x[5] = std::atan2 (transformation_matrix (1,0), transformation_matrix (0,0)); + + // Set temporary pointers + tmp_src_ = &cloud_src; + tmp_tgt_ = &cloud_tgt; + tmp_idx_src_ = &indices_src; + tmp_idx_tgt_ = &indices_tgt; + + // Optimize using forward-difference approximation LM + const double gradient_tol = 1e-2; + OptimizationFunctorWithIndices functor(this); + BFGS bfgs (functor); + bfgs.parameters.sigma = 0.01; + bfgs.parameters.rho = 0.01; + bfgs.parameters.tau1 = 9; + bfgs.parameters.tau2 = 0.05; + bfgs.parameters.tau3 = 0.5; + bfgs.parameters.order = 3; + + int inner_iterations_ = 0; + int result = bfgs.minimizeInit (x); + result = BFGSSpace::Running; + do + { + inner_iterations_++; + result = bfgs.minimizeOneStep (x); + if(result) + { + break; + } + result = bfgs.testGradient(gradient_tol); + } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_); + if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_) + { + PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]"); + PCL_DEBUG ("BFGS solver finished with exit code %i \n", result); + transformation_matrix.setIdentity(); + applyState(transformation_matrix, x); + } + else + PCL_THROW_EXCEPTION(SolverDidntConvergeException, + "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!"); +} + +//////////////////////////////////////////////////////////////////////////////////////// +template inline double +pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::operator() (const Vector6d& x) +{ + Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; + gicp_->applyState(transformation_matrix, x); + double f = 0; + int m = static_cast (gicp_->tmp_idx_src_->size ()); + for (int i = 0; i < m; ++i) + { + // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap (); + // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap (); + Eigen::Vector4f pp (transformation_matrix * p_src); + // Estimate the distance (cost function) + // The last coordinate is still guaranteed to be set to 1.0 + Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]); + Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res); + //increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes) + f+= double(res.transpose() * temp); + } + return f/m; +} + +//////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g) +{ + Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; + gicp_->applyState(transformation_matrix, x); + //Zero out g + g.setZero (); + //Eigen::Vector3d g_t = g.head<3> (); + Eigen::Matrix3d R = Eigen::Matrix3d::Zero (); + int m = static_cast (gicp_->tmp_idx_src_->size ()); + for (int i = 0; i < m; ++i) + { + // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap (); + // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap (); + + Eigen::Vector4f pp (transformation_matrix * p_src); + // The last coordinate is still guaranteed to be set to 1.0 + Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]); + // temp = M*res + Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res); + // Increment translation gradient + // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes) + g.head<3> ()+= temp; + // Increment rotation gradient + pp = gicp_->base_transformation_ * p_src; + Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]); + R+= p_src3 * temp.transpose(); + } + g.head<3> ()*= 2.0/m; + R*= 2.0/m; + gicp_->computeRDerivative(x, R, g); +} + +//////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g) +{ + Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; + gicp_->applyState(transformation_matrix, x); + f = 0; + g.setZero (); + Eigen::Matrix3d R = Eigen::Matrix3d::Zero (); + const int m = static_cast (gicp_->tmp_idx_src_->size ()); + for (int i = 0; i < m; ++i) + { + // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap (); + // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap (); + Eigen::Vector4f pp (transformation_matrix * p_src); + // The last coordinate is still guaranteed to be set to 1.0 + Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]); + // temp = M*res + Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res); + // Increment total error + f+= double(res.transpose() * temp); + // Increment translation gradient + // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes) + g.head<3> ()+= temp; + pp = gicp_->base_transformation_ * p_src; + Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]); + // Increment rotation gradient + R+= p_src3 * temp.transpose(); + } + f/= double(m); + g.head<3> ()*= double(2.0/m); + R*= 2.0/m; + gicp_->computeRDerivative(x, R, g); +} + +//////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::GeneralizedIterativeClosestPoint::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) +{ + pcl::IterativeClosestPoint::initComputeReciprocal (); + using namespace std; + // Difference between consecutive transforms + double delta = 0; + // Get the size of the target + const std::size_t N = indices_->size (); + // Set the mahalanobis matrices to identity + mahalanobis_.resize (N, Eigen::Matrix3d::Identity ()); + // Compute target cloud covariance matrices + if ((!target_covariances_) || (target_covariances_->empty ())) + { + target_covariances_.reset (new MatricesVector); + computeCovariances (target_, tree_, *target_covariances_); + } + // Compute input cloud covariance matrices + if ((!input_covariances_) || (input_covariances_->empty ())) + { + input_covariances_.reset (new MatricesVector); + computeCovariances (input_, tree_reciprocal_, *input_covariances_); + } + + base_transformation_ = Eigen::Matrix4f::Identity(); + nr_iterations_ = 0; + converged_ = false; + double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; + std::vector nn_indices (1); + std::vector nn_dists (1); + + pcl::transformPointCloud(output, output, guess); + + while(!converged_) + { + std::size_t cnt = 0; + std::vector source_indices (indices_->size ()); + std::vector target_indices (indices_->size ()); + + // guess corresponds to base_t and transformation_ to t + Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero (); + for(std::size_t i = 0; i < 4; i++) + for(std::size_t j = 0; j < 4; j++) + for(std::size_t k = 0; k < 4; k++) + transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j)); + + Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> (); + + for (std::size_t i = 0; i < N; i++) + { + PointSource query = output[i]; + query.getVector4fMap () = transformation_ * query.getVector4fMap (); + + if (!searchForNeighbors (query, 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_)[i]); + return; + } + + // Check if the distance to the nearest neighbor is smaller than the user imposed threshold + if (nn_dists[0] < dist_threshold) + { + Eigen::Matrix3d &C1 = (*input_covariances_)[i]; + Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]]; + Eigen::Matrix3d &M = mahalanobis_[i]; + // M = R*C1 + M = R * C1; + // temp = M*R' + C2 = R*C1*R' + C2 + Eigen::Matrix3d temp = M * R.transpose(); + temp+= C2; + // M = temp^-1 + M = temp.inverse (); + source_indices[cnt] = static_cast (i); + target_indices[cnt] = nn_indices[0]; + cnt++; + } + } + // Resize to the actual number of valid correspondences + source_indices.resize(cnt); target_indices.resize(cnt); + /* optimize transformation using the current assignment and Mahalanobis metrics*/ + previous_transformation_ = transformation_; + //optimization right here + try + { + rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_); + /* compute the delta from this iteration */ + delta = 0.; + for(int k = 0; k < 4; k++) { + for(int l = 0; l < 4; l++) { + double ratio = 1; + if(k < 3 && l < 3) // rotation part of the transform + ratio = 1./rotation_epsilon_; + else + ratio = 1./transformation_epsilon_; + double c_delta = ratio*std::abs(previous_transformation_(k,l) - transformation_(k,l)); + if(c_delta > delta) + delta = c_delta; + } + } + } + catch (PCLException &e) + { + PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ()); + break; + } + nr_iterations_++; + // Check for convergence + if (nr_iterations_ >= max_iterations_ || delta < 1) + { + 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_, (transformation_ - previous_transformation_).array ().abs ().sum ()); + previous_transformation_ = transformation_; + } + else + PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ()); + } + final_transformation_ = previous_transformation_ * guess; + + // Transform the point cloud + pcl::transformPointCloud (*input_, output, final_transformation_); +} + +template void +pcl::GeneralizedIterativeClosestPoint::applyState(Eigen::Matrix4f &t, const Vector6d& x) const +{ + // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention + Eigen::Matrix3f R; + R = Eigen::AngleAxisf (static_cast (x[5]), Eigen::Vector3f::UnitZ ()) + * Eigen::AngleAxisf (static_cast (x[4]), Eigen::Vector3f::UnitY ()) + * Eigen::AngleAxisf (static_cast (x[3]), Eigen::Vector3f::UnitX ()); + t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix (); + Eigen::Vector4f T (static_cast (x[0]), static_cast (x[1]), static_cast (x[2]), 0.0f); + t.col (3) += T; +} + +#endif //PCL_REGISTRATION_IMPL_GICP_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/ia_fpcs.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/ia_fpcs.hpp new file mode 100755 index 0000000..d758c14 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/ia_fpcs.hpp @@ -0,0 +1,914 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel. + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_ +#define PCL_REGISTRATION_IMPL_IA_FPCS_H_ + +#include +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline float +pcl::getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, float max_dist, int nr_threads) +{ + const float max_dist_sqr = max_dist * max_dist; + const std::size_t s = cloud.size (); + + pcl::search::KdTree tree; + tree.setInputCloud (cloud); + + float mean_dist = 0.f; + int num = 0; + std::vector ids (2); + std::vector dists_sqr (2); + +#ifdef _OPENMP +#pragma omp parallel for \ + reduction (+:mean_dist, num) \ + private (ids, dists_sqr) shared (tree, cloud) firstprivate (s, max_dist_sqr) \ + default (none)num_threads (nr_threads) +#endif + + for (int i = 0; i < 1000; i++) + { + tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr); + if (dists_sqr[1] < max_dist_sqr) + { + mean_dist += std::sqrt (dists_sqr[1]); + num++; + } + } + + return (mean_dist / num); +}; + + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline float +pcl::getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &cloud, const std::vector &indices, + float max_dist, int nr_threads) +{ + const float max_dist_sqr = max_dist * max_dist; + const std::size_t s = indices.size (); + + pcl::search::KdTree tree; + tree.setInputCloud (cloud); + + float mean_dist = 0.f; + int num = 0; + std::vector ids (2); + std::vector dists_sqr (2); + +#ifdef _OPENMP +#pragma omp parallel for \ + reduction (+:mean_dist, num) \ + private (ids, dists_sqr) shared (tree, cloud, indices) firstprivate (s, max_dist_sqr) \ + default (none)num_threads (nr_threads) +#endif + + for (int i = 0; i < 1000; i++) + { + tree.nearestKSearch (cloud->points[indices[rand () % s]], 2, ids, dists_sqr); + if (dists_sqr[1] < max_dist_sqr) + { + mean_dist += std::sqrt (dists_sqr[1]); + num++; + } + } + + return (mean_dist / num); +}; + + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::registration::FPCSInitialAlignment ::FPCSInitialAlignment () : + source_normals_ (), + target_normals_ (), + nr_threads_ (1), + approx_overlap_ (0.5f), + delta_ (1.f), + score_threshold_ (FLT_MAX), + nr_samples_ (0), + max_norm_diff_ (90.f), + max_runtime_ (0), + fitness_score_ (FLT_MAX), + diameter_ (), + max_base_diameter_sqr_ (), + use_normals_ (false), + normalize_delta_ (true), + max_pair_diff_ (), + max_edge_diff_ (), + coincidation_limit_ (), + max_mse_ (), + max_inlier_dist_sqr_ (), + small_error_ (0.00001f) +{ + reg_name_ = "pcl::registration::FPCSInitialAlignment"; + max_iterations_ = 0; + ransac_iterations_ = 1000; + transformation_estimation_.reset (new pcl::registration::TransformationEstimation3Point ); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::computeTransformation ( + PointCloudSource &output, + const Eigen::Matrix4f &guess) +{ + if (!initCompute ()) + return; + + final_transformation_ = guess; + bool abort = false; + std::vector all_candidates (max_iterations_); + pcl::StopWatch timer; + + #ifdef _OPENMP + #pragma omp parallel num_threads (nr_threads_) + #endif + { + #ifdef _OPENMP + std::srand (static_cast (std::time (NULL)) ^ omp_get_thread_num ()); + #pragma omp for schedule (dynamic) + #endif + for (int i = 0; i < max_iterations_; i++) + { + + #ifdef _OPENMP + #pragma omp flush (abort) + #endif + + MatchingCandidates candidates (1); + std::vector base_indices (4); + all_candidates[i] = candidates; + + if (!abort) + { + float ratio[2]; + // select four coplanar point base + if (selectBase (base_indices, ratio) == 0) + { + // calculate candidate pair correspondences using diagonal lengths of base + pcl::Correspondences pairs_a, pairs_b; + if (bruteForceCorrespondences (base_indices[0], base_indices[1], pairs_a) == 0 && + bruteForceCorrespondences (base_indices[2], base_indices[3], pairs_b) == 0) + { + // determine candidate matches by combining pair correspondences based on segment distances + std::vector > matches; + if (determineBaseMatches (base_indices, matches, pairs_a, pairs_b, ratio) == 0) + { + // check and evaluate candidate matches and store them + handleMatches (base_indices, matches, candidates); + if (!candidates.empty ()) + all_candidates[i] = candidates; + } + } + } + + // check terminate early (time or fitness_score threshold reached) + abort = (!candidates.empty () ? candidates[0].fitness_score < score_threshold_ : abort); + abort = (abort ? abort : timer.getTimeSeconds () > max_runtime_); + + + #ifdef _OPENMP + #pragma omp flush (abort) + #endif + } + } + } + + + // determine best match over all tries + finalCompute (all_candidates); + + // apply the final transformation + pcl::transformPointCloud (*input_, output, final_transformation_); + + deinitCompute (); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::FPCSInitialAlignment ::initCompute () +{ + std::srand (static_cast (std::time (nullptr))); + + // basic pcl initialization + if (!pcl::PCLBase ::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_->empty ()) + { + target_indices_.reset (new std::vector (static_cast (target_->size ()))); + int index = 0; + for (int &target_index : *target_indices_) + target_index = 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 (indices_->size ()); + const int sample_fraction_src = std::max (1, static_cast (ss / nr_samples_)); + + source_indices_ = pcl::IndicesPtr (new std::vector ); + 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 (target_, *target_indices_, 0.05f * diameter_, nr_threads_); + delta_ *= mean_dist; + } + + // heuristic determination of number of trials to have high probability 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 (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); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::selectBase ( + std::vector &base_indices, + float (&ratio)[2]) +{ + const float too_close_sqr = max_base_diameter_sqr_*0.01; + + Eigen::VectorXf coefficients (4); + pcl::SampleConsensusModelPlane plane (target_); + plane.setIndices (target_indices_); + Eigen::Vector4f centre_pt; + float nearest_to_plane = FLT_MAX; + + // repeat base search until valid quadruple was found or ransac_iterations_ number of tries were unsuccessful + for (int i = 0; i < ransac_iterations_; i++) + { + // random select an appropriate point triple + if (selectBaseTriangle (base_indices) < 0) + continue; + + std::vector base_triple (base_indices.begin (), base_indices.end () - 1); + plane.computeModelCoefficients (base_triple, coefficients); + pcl::compute3DCentroid (*target_, base_triple, centre_pt); + + // loop over all points in source cloud to find most suitable fourth point + const PointTarget *pt1 = &(target_->points[base_indices[0]]); + const PointTarget *pt2 = &(target_->points[base_indices[1]]); + const PointTarget *pt3 = &(target_->points[base_indices[2]]); + + for (const int &target_index : *target_indices_) + { + const PointTarget *pt4 = &(target_->points[target_index]); + + float d1 = pcl::squaredEuclideanDistance (*pt4, *pt1); + float d2 = pcl::squaredEuclideanDistance (*pt4, *pt2); + float d3 = pcl::squaredEuclideanDistance (*pt4, *pt3); + float d4 = (pt4->getVector3fMap () - centre_pt.head (3)).squaredNorm (); + + // check distance between points w.r.t minimum sampling distance; EDITED -> 4th point now also limited by max base line + if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr || d4 < too_close_sqr || + d1 > max_base_diameter_sqr_ || d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_) + continue; + + // check distance to plane to get point closest to plane + float dist_to_plane = pcl::pointToPlaneDistance (*pt4, coefficients); + if (dist_to_plane < nearest_to_plane) + { + base_indices[3] = target_index; + nearest_to_plane = dist_to_plane; + } + } + + // check if at least one point fulfilled the conditions + if (nearest_to_plane != FLT_MAX) + { + // order points to build largest quadrangle and calcuate intersection ratios of diagonals + setupBase (base_indices, ratio); + return (0); + } + } + + // return unsuccessful if no quadruple was selected + return (-1); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::selectBaseTriangle (std::vector &base_indices) +{ + int nr_points = static_cast (target_indices_->size ()); + float best_t = 0.f; + + // choose random first point + base_indices[0] = (*target_indices_)[rand () % nr_points]; + int *index1 = &base_indices[0]; + + // random search for 2 other points (as far away as overlap allows) + for (int i = 0; i < ransac_iterations_; i++) + { + int *index2 = &(*target_indices_)[rand () % nr_points]; + int *index3 = &(*target_indices_)[rand () % nr_points]; + + Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap (); + Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap (); + float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal + + // check for most suitable point triple + if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_) + { + best_t = t; + base_indices[1] = *index2; + base_indices[2] = *index3; + } + } + + // return if a triplet could be selected + return (best_t == 0.f ? -1 : 0); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::setupBase ( + std::vector &base_indices, + float (&ratio)[2]) +{ + float best_t = FLT_MAX; + const std::vector copy (base_indices.begin (), base_indices.end ()); + std::vector temp (base_indices.begin (), base_indices.end ()); + + // loop over all combinations of base points + for (std::vector ::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; ++i) + for (std::vector ::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; ++j) + { + if (i == j) + continue; + + for (std::vector ::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; ++k) + { + if (k == j || k == i) + continue; + + std::vector ::const_iterator l = copy.begin (); + while (l == i || l == j || l == k) + ++l; + + temp[0] = *i; + temp[1] = *j; + temp[2] = *k; + temp[3] = *l; + + // calculate diagonal intersection ratios and check for suitable segment to segment distances + float ratio_temp[2]; + float t = segmentToSegmentDist (temp, ratio_temp); + if (t < best_t) + { + best_t = t; + ratio[0] = ratio_temp[0]; + ratio[1] = ratio_temp[1]; + base_indices = temp; + } + } + } +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::registration::FPCSInitialAlignment ::segmentToSegmentDist ( + const std::vector &base_indices, + float (&ratio)[2]) +{ + // get point vectors + Eigen::Vector3f u = target_->points[base_indices[1]].getVector3fMap () - target_->points[base_indices[0]].getVector3fMap (); + Eigen::Vector3f v = target_->points[base_indices[3]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap (); + Eigen::Vector3f w = target_->points[base_indices[0]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap (); + + // calculate segment distances + float a = u.dot (u); + float b = u.dot (v); + float c = v.dot (v); + float d = u.dot (w); + float e = v.dot (w); + float D = a * c - b * b; + float sN = 0.f, sD = D; + float tN = 0.f, tD = D; + + // check segments + if (D < small_error_) + { + sN = 0.f; + sD = 1.f; + tN = e; + tD = c; + } + else + { + sN = (b * e - c * d); + tN = (a * e - b * d); + + if (sN < 0.f) + { + sN = 0.f; + tN = e; + tD = c; + } + else if (sN > sD) + { + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < 0.f) + { + tN = 0.f; + + if (-d < 0.f) + sN = 0.f; + + else if (-d > a) + sN = sD; + + else + { + sN = -d; + sD = a; + } + } + + else if (tN > tD) + { + tN = tD; + + if ((-d + b) < 0.f) + sN = 0.f; + + else if ((-d + b) > a) + sN = sD; + + else + { + sN = (-d + b); + sD = a; + } + } + + // set intersection ratios + ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD; + ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD; + + Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v); + return (x.norm ()); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::bruteForceCorrespondences ( + int idx1, + int idx2, + pcl::Correspondences &pairs) +{ + const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f; + + // calculate reference segment distance and normal angle + float ref_dist = pcl::euclideanDistance (target_->points[idx1], target_->points[idx2]); + float ref_norm_angle = (use_normals_ ? (target_normals_->points[idx1].getNormalVector3fMap () - + target_normals_->points[idx2].getNormalVector3fMap ()).norm () : 0.f); + + // loop over all pairs of points in source point cloud + auto it_out = source_indices_->begin (), it_out_e = source_indices_->end () - 1; + auto it_in_e = source_indices_->end (); + for ( ; it_out != it_out_e; it_out++) + { + auto it_in = it_out + 1; + const PointSource *pt1 = &(*input_)[*it_out]; + for ( ; it_in != it_in_e; it_in++) + { + const PointSource *pt2 = &(*input_)[*it_in]; + + // check point distance compared to reference dist (from base) + float dist = pcl::euclideanDistance (*pt1, *pt2); + if (std::abs(dist - ref_dist) < max_pair_diff_) + { + // add here normal evaluation if normals are given + if (use_normals_) + { + const NormalT *pt1_n = &(source_normals_->points[*it_out]); + const NormalT *pt2_n = &(source_normals_->points[*it_in]); + + float norm_angle_1 = (pt1_n->getNormalVector3fMap () - pt2_n->getNormalVector3fMap ()).norm (); + float norm_angle_2 = (pt1_n->getNormalVector3fMap () + pt2_n->getNormalVector3fMap ()).norm (); + + float norm_diff = std::min (std::abs (norm_angle_1 - ref_norm_angle), std::abs (norm_angle_2 - ref_norm_angle)); + if (norm_diff > max_norm_diff) + continue; + } + + pairs.push_back (pcl::Correspondence (*it_in, *it_out, dist)); + pairs.push_back (pcl::Correspondence (*it_out, *it_in, dist)); + } + } + } + + // return success if at least one correspondence was found + return (pairs.empty () ? -1 : 0); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::determineBaseMatches ( + const std::vector &base_indices, + std::vector > &matches, + const pcl::Correspondences &pairs_a, + const pcl::Correspondences &pairs_b, + const float (&ratio)[2]) +{ + // calculate edge lengths of base + float dist_base[4]; + dist_base[0] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[2]]); + dist_base[1] = pcl::euclideanDistance (target_->points[base_indices[0]], target_->points[base_indices[3]]); + dist_base[2] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[2]]); + dist_base[3] = pcl::euclideanDistance (target_->points[base_indices[1]], target_->points[base_indices[3]]); + + // loop over first point pair correspondences and store intermediate points 'e' in new point cloud + PointCloudSourcePtr cloud_e (new PointCloudSource); + cloud_e->resize (pairs_a.size () * 2); + PointCloudSourceIterator it_pt = cloud_e->begin (); + for (const auto &pair : pairs_a) + { + const PointSource *pt1 = &(input_->points[pair.index_match]); + const PointSource *pt2 = &(input_->points[pair.index_query]); + + // calculate intermediate points using both ratios from base (r1,r2) + for (int i = 0; i < 2; i++, it_pt++) + { + it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x); + it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y); + it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z); + } + } + + // initialize new kd tree of intermediate points from first point pair correspondences + KdTreeReciprocalPtr tree_e (new KdTreeReciprocal); + tree_e->setInputCloud (cloud_e); + + std::vector ids; + std::vector dists_sqr; + + // loop over second point pair correspondences + for (const auto &pair : pairs_b) + { + const PointTarget *pt1 = &(input_->points[pair.index_match]); + const PointTarget *pt2 = &(input_->points[pair.index_query]); + + // calculate intermediate points using both ratios from base (r1,r2) + for (const float &r : ratio) + { + PointTarget pt_e; + pt_e.x = pt1->x + r * (pt2->x - pt1->x); + pt_e.y = pt1->y + r * (pt2->y - pt1->y); + pt_e.z = pt1->z + r * (pt2->z - pt1->z); + + // search for corresponding intermediate points + tree_e->radiusSearch (pt_e, coincidation_limit_, ids, dists_sqr); + for (const int &id : ids) + { + std::vector match_indices (4); + + match_indices[0] = pairs_a[static_cast (std::floor ((float)(id/2.f)))].index_match; + match_indices[1] = pairs_a[static_cast (std::floor ((float)(id/2.f)))].index_query; + match_indices[2] = pair.index_match; + match_indices[3] = pair.index_query; + + // EDITED: added coarse check of match based on edge length (due to rigid-body ) + if (checkBaseMatch (match_indices, dist_base) < 0) + continue; + + matches.push_back (match_indices); + } + } + } + + // return unsuccessful if no match was found + return (!matches.empty () ? 0 : -1); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::checkBaseMatch ( + const std::vector &match_indices, + const float (&dist_ref)[4]) +{ + float d0 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[2]]); + float d1 = pcl::euclideanDistance (input_->points[match_indices[0]], input_->points[match_indices[3]]); + float d2 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[2]]); + float d3 = pcl::euclideanDistance (input_->points[match_indices[1]], input_->points[match_indices[3]]); + + // check edge distances of match w.r.t the base + return (std::abs (d0 - dist_ref[0]) < max_edge_diff_ && std::abs (d1 - dist_ref[1]) < max_edge_diff_ && + std::abs (d2 - dist_ref[2]) < max_edge_diff_ && std::abs (d3 - dist_ref[3]) < max_edge_diff_) ? 0 : -1; +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::handleMatches ( + const std::vector &base_indices, + std::vector > &matches, + MatchingCandidates &candidates) +{ + candidates.resize (1); + float fitness_score = FLT_MAX; + + // loop over all Candidate matches + for (auto &match : matches) + { + Eigen::Matrix4f transformation_temp; + pcl::Correspondences correspondences_temp; + + // determine corresondences between base and match according to their distance to centroid + linkMatchWithBase (base_indices, match, correspondences_temp); + + // check match based on residuals of the corresponding points after + if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0) + continue; + + // check resulting using a sub sample of the source point cloud and compare to previous matches + if (validateTransformation (transformation_temp, fitness_score) < 0) + continue; + + // store best match as well as associated fitness_score and transformation + candidates[0].fitness_score = fitness_score; + candidates [0].transformation = transformation_temp; + correspondences_temp.erase (correspondences_temp.end () - 1); + candidates[0].correspondences = correspondences_temp; + } +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::linkMatchWithBase ( + const std::vector &base_indices, + std::vector &match_indices, + pcl::Correspondences &correspondences) +{ + // calculate centroid of base and target + Eigen::Vector4f centre_base {0, 0, 0, 0}, centre_match {0, 0, 0, 0}; + pcl::compute3DCentroid (*target_, base_indices, centre_base); + pcl::compute3DCentroid (*input_, match_indices, centre_match); + + PointTarget centre_pt_base; + centre_pt_base.x = centre_base[0]; + centre_pt_base.y = centre_base[1]; + centre_pt_base.z = centre_base[2]; + + PointSource centre_pt_match; + centre_pt_match.x = centre_match[0]; + centre_pt_match.y = centre_match[1]; + centre_pt_match.z = centre_match[2]; + + // find corresponding points according to their distance to the centroid + std::vector copy = match_indices; + + auto it_match_orig = match_indices.begin (); + for (auto it_base = base_indices.cbegin (), it_base_e = base_indices.cend (); it_base != it_base_e; it_base++, it_match_orig++) + { + float dist_sqr_1 = pcl::squaredEuclideanDistance (target_->points[*it_base], centre_pt_base); + float best_diff_sqr = FLT_MAX; + int best_index = -1; + + for (const int &match_index : copy) + { + // calculate difference of distances to centre point + float dist_sqr_2 = pcl::squaredEuclideanDistance (input_->points[match_index], centre_pt_match); + float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2); + + if (diff_sqr < best_diff_sqr) + { + best_diff_sqr = diff_sqr; + best_index = match_index; + } + } + + // assign new correspondence and update indices of matched targets + correspondences.push_back (pcl::Correspondence (best_index, *it_base, best_diff_sqr)); + *it_match_orig = best_index; + } +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::validateMatch ( + const std::vector &base_indices, + const std::vector &match_indices, + const pcl::Correspondences &correspondences, + Eigen::Matrix4f &transformation) +{ + // only use triplet of points to simlify process (possible due to planar case) + pcl::Correspondences correspondences_temp = correspondences; + correspondences_temp.erase (correspondences_temp.end () - 1); + + // estimate transformation between correspondence set + transformation_estimation_->estimateRigidTransformation (*input_, *target_, correspondences_temp, transformation); + + // transform base points + PointCloudSource match_transformed; + pcl::transformPointCloud (*input_, match_indices, match_transformed, transformation); + + // calculate residuals of transformation and check against maximum threshold + std::size_t nr_points = correspondences_temp.size (); + float mse = 0.f; + for (std::size_t i = 0; i < nr_points; i++) + mse += pcl::squaredEuclideanDistance (match_transformed.points [i], target_->points [base_indices[i]]); + + mse /= nr_points; + return (mse < max_mse_ ? 0 : -1); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::FPCSInitialAlignment ::validateTransformation ( + Eigen::Matrix4f &transformation, + float &fitness_score) +{ + // transform source point cloud + PointCloudSource source_transformed; + pcl::transformPointCloud (*input_, *source_indices_, source_transformed, transformation); + + std::size_t nr_points = source_transformed.size (); + std::size_t terminate_value = fitness_score > 1 ? 0 : static_cast ((1.f - fitness_score) * nr_points); + + float inlier_score_temp = 0; + std::vector ids; + std::vector dists_sqr; + PointCloudSourceIterator it = source_transformed.begin (); + + for (std::size_t i = 0; i < nr_points; it++, i++) + { + // search for nearest point using kd tree search + tree_->nearestKSearch (*it, 1, ids, dists_sqr); + inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0); + + // early terminating + if (nr_points - i + inlier_score_temp < terminate_value) + break; + } + + // check current costs and return unsuccessful if larger than previous ones + inlier_score_temp /= static_cast (nr_points); + float fitness_score_temp = 1.f - inlier_score_temp; + + if (fitness_score_temp > fitness_score) + return (-1); + + fitness_score = fitness_score_temp; + return (0); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::FPCSInitialAlignment ::finalCompute ( + const std::vector &candidates) +{ + // get best fitness_score over all tries + int nr_candidates = static_cast (candidates.size ()); + int best_index = -1; + float best_score = FLT_MAX; + for (int i = 0; i < nr_candidates; i++) + { + const float &fitness_score = candidates [i][0].fitness_score; + if (fitness_score < best_score) + { + best_score = fitness_score; + best_index = i; + } + } + + // check if a valid candidate was available + if (!(best_index < 0)) + { + fitness_score_ = candidates [best_index][0].fitness_score; + final_transformation_ = candidates [best_index][0].transformation; + *correspondences_ = candidates [best_index][0].correspondences; + + // here we define convergence if resulting fitness_score is below 1-threshold + converged_ = fitness_score_ < score_threshold_; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// + +#endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/ia_kfpcs.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/ia_kfpcs.hpp new file mode 100755 index 0000000..7f08fd5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/ia_kfpcs.hpp @@ -0,0 +1,289 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_ +#define PCL_REGISTRATION_IMPL_IA_KFPCS_H_ + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::registration::KFPCSInitialAlignment ::KFPCSInitialAlignment () : + lower_trl_boundary_ (-1.f), + upper_trl_boundary_ (-1.f), + lambda_ (0.5f), + use_trl_score_ (false), + indices_validation_ (new std::vector ) +{ + reg_name_ = "pcl::registration::KFPCSInitialAlignment"; +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::registration::KFPCSInitialAlignment ::initCompute () +{ + // due to sparse keypoint cloud, do not normalize delta with estimated point density + if (normalize_delta_) + { + PCL_WARN ("[%s::initCompute] Delta should be set according to keypoint precision! Normalization according to point cloud density is ignored.", reg_name_.c_str ()); + normalize_delta_ = false; + } + + // initialize as in fpcs + pcl::registration::FPCSInitialAlignment ::initCompute (); + + // set the threshold values with respect to keypoint charactersitics + max_pair_diff_ = delta_ * 1.414f; // diff between 2 points of delta_ accuracy + coincidation_limit_ = delta_ * 2.828f; // diff between diff of 2 points + max_edge_diff_ = delta_ * 3.f; // diff between 2 points + some inaccuracy due to quadruple orientation + max_mse_ = powf (delta_ * 4.f, 2.f); // diff between 2 points + some registration inaccuracy + max_inlier_dist_sqr_ = powf (delta_ * 8.f, 2.f); // set rel. high, because MSAC is used (residual based score function) + + // check use of translation costs and calculate upper boundary if not set by user + if (upper_trl_boundary_ < 0) + upper_trl_boundary_ = diameter_ * (1.f - approx_overlap_) * 0.5f; + + if (!(lower_trl_boundary_ < 0) && upper_trl_boundary_ > lower_trl_boundary_) + use_trl_score_ = true; + else + lambda_ = 0.f; + + // generate a subset of indices of size ransac_iterations_ on which to evaluate candidates on + std::size_t nr_indices = indices_->size (); + if (nr_indices < std::size_t (ransac_iterations_)) + indices_validation_ = indices_; + else + for (int i = 0; i < ransac_iterations_; i++) + indices_validation_->push_back ((*indices_)[rand () % nr_indices]); + + return (true); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::KFPCSInitialAlignment ::handleMatches ( + const std::vector &base_indices, + std::vector > &matches, + MatchingCandidates &candidates) +{ + candidates.clear (); + float fitness_score = FLT_MAX; + + // loop over all Candidate matches + for (auto &match : matches) + { + Eigen::Matrix4f transformation_temp; + pcl::Correspondences correspondences_temp; + fitness_score = FLT_MAX; // reset to FLT_MAX to accept all candidates and not only best + + // determine corresondences between base and match according to their distance to centroid + linkMatchWithBase (base_indices, match, correspondences_temp); + + // check match based on residuals of the corresponding points after transformation + if (validateMatch (base_indices, match, correspondences_temp, transformation_temp) < 0) + continue; + + // check resulting transformation using a sub sample of the source point cloud + // all candidates are stored and later sorted according to their fitness score + validateTransformation (transformation_temp, fitness_score); + + // store all valid match as well as associated score and transformation + candidates.push_back (MatchingCandidate (fitness_score, correspondences_temp, transformation_temp)); + } +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::KFPCSInitialAlignment ::validateTransformation ( + Eigen::Matrix4f &transformation, + float &fitness_score) +{ + // transform sub sampled source cloud + PointCloudSource source_transformed; + pcl::transformPointCloud (*input_, *indices_validation_, source_transformed, transformation); + + const std::size_t nr_points = source_transformed.size (); + float score_a = 0.f, score_b = 0.f; + + // residual costs based on mse + std::vector ids; + std::vector dists_sqr; + for (PointCloudSourceIterator it = source_transformed.begin (), it_e = source_transformed.end (); it != it_e; ++it) + { + // search for nearest point using kd tree search + tree_->nearestKSearch (*it, 1, ids, dists_sqr); + score_a += (dists_sqr[0] < max_inlier_dist_sqr_ ? dists_sqr[0] : max_inlier_dist_sqr_); // MSAC + } + + score_a /= (max_inlier_dist_sqr_ * nr_points); // MSAC + //score_a = 1.f - (1.f - score_a) / (1.f - approx_overlap_); // make score relative to estimated overlap + + // translation score (solutions with small translation are down-voted) + float scale = 1.f; + if (use_trl_score_) + { + float trl = transformation.rightCols <1> ().head (3).norm (); + float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_); + + score_b = (trl_ratio < 0.f ? 1.f : (trl_ratio > 1.f ? 0.f : 0.5f * sin (M_PI * trl_ratio + M_PI_2) + 0.5f)); // sinusoidal costs + scale += lambda_; + } + + // calculate the fitness and return unsuccessful if smaller than previous ones + float fitness_score_temp = (score_a + lambda_ * score_b) / scale; + if (fitness_score_temp > fitness_score) + return (-1); + + fitness_score = fitness_score_temp; + return (0); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::KFPCSInitialAlignment ::finalCompute ( + const std::vector &candidates) +{ + // reorganize candidates into single vector + std::size_t total_size = 0; + for (const auto &candidate : candidates) + total_size += candidate.size (); + + candidates_.clear (); + candidates_.reserve (total_size); + + for (const auto &candidate : candidates) + for (const auto &match : candidate) + candidates_.push_back (match); + + // sort according to score value + std::sort (candidates_.begin (), candidates_.end (), by_score ()); + + // return here if no score was valid, i.e. all scores are FLT_MAX + if (candidates_[0].fitness_score == FLT_MAX) + { + converged_ = false; + return; + } + + // save best candidate as output result + // note, all other candidates are accessible via getNBestCandidates () and getTBestCandidates () + fitness_score_ = candidates_ [0].fitness_score; + final_transformation_ = candidates_ [0].transformation; + *correspondences_ = candidates_ [0].correspondences; + + // here we define convergence if resulting score is above threshold + converged_ = fitness_score_ < score_threshold_; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::KFPCSInitialAlignment ::getNBestCandidates ( + int n, + float min_angle3d, + float min_translation3d, + MatchingCandidates &candidates) +{ + candidates.clear (); + + // loop over all candidates starting from the best one + for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate) + { + // stop if current candidate has no valid score + if (it_candidate->fitness_score == FLT_MAX) + return; + + // check if current candidate is a unique one compared to previous using the min_diff threshold + bool unique = true; + MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end (); + while (unique && it != it_e2) + { + Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation); + const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle (); + const float translation3d = diff.block <3, 1> (0, 3).norm (); + unique = angle3d > min_angle3d && translation3d > min_translation3d; + ++it; + } + + // add candidate to best candidates + if (unique) + candidates.push_back (*it_candidate); + + // stop if n candidates are reached + if (candidates.size () == n) + return; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::KFPCSInitialAlignment ::getTBestCandidates ( + float t, + float min_angle3d, + float min_translation3d, + MatchingCandidates &candidates) +{ + candidates.clear (); + + // loop over all candidates starting from the best one + for (MatchingCandidates::iterator it_candidate = candidates_.begin (), it_e = candidates_.end (); it_candidate != it_e; ++it_candidate) + { + // stop if current candidate has score below threshold + if (it_candidate->fitness_score > t) + return; + + // check if current candidate is a unique one compared to previous using the min_diff threshold + bool unique = true; + MatchingCandidates::iterator it = candidates.begin (), it_e2 = candidates.end (); + while (unique && it != it_e2) + { + Eigen::Matrix4f diff = it_candidate->transformation.colPivHouseholderQr ().solve (it->transformation); + const float angle3d = Eigen::AngleAxisf (diff.block <3, 3> (0, 0)).angle (); + const float translation3d = diff.block <3, 1> (0, 3).norm (); + unique = angle3d > min_angle3d && translation3d > min_translation3d; + ++it; + } + + // add candidate to best candidates + if (unique) + candidates.push_back (*it_candidate); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// + +#endif // PCL_REGISTRATION_IMPL_IA_KFPCS_H_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/ia_ransac.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/ia_ransac.hpp new file mode 100755 index 0000000..2026ccb --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/ia_ransac.hpp @@ -0,0 +1,256 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef IA_RANSAC_HPP_ +#define IA_RANSAC_HPP_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusInitialAlignment::setSourceFeatures (const FeatureCloudConstPtr &features) +{ + if (features == nullptr || features->empty ()) + { + PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); + return; + } + input_features_ = features; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusInitialAlignment::setTargetFeatures (const FeatureCloudConstPtr &features) +{ + if (features == nullptr || features->empty ()) + { + PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); + return; + } + target_features_ = features; + feature_tree_->setInputCloud (target_features_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusInitialAlignment::selectSamples ( + const PointCloudSource &cloud, int nr_samples, float min_sample_distance, + std::vector &sample_indices) +{ + if (nr_samples > static_cast (cloud.points.size ())) + { + PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); + PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n", + nr_samples, cloud.points.size ()); + return; + } + + // Iteratively draw random samples until nr_samples is reached + int iterations_without_a_sample = 0; + int max_iterations_without_a_sample = static_cast (3 * cloud.points.size ()); + sample_indices.clear (); + while (static_cast (sample_indices.size ()) < nr_samples) + { + // Choose a sample at random + int sample_index = getRandomIndex (static_cast (cloud.points.size ())); + + // Check to see if the sample is 1) unique and 2) far away from the other samples + bool valid_sample = true; + for (const int &sample_idx : sample_indices) + { + float distance_between_samples = euclideanDistance (cloud.points[sample_index], cloud.points[sample_idx]); + + if (sample_index == sample_idx || distance_between_samples < min_sample_distance) + { + valid_sample = false; + break; + } + } + + // If the sample is valid, add it to the output + if (valid_sample) + { + sample_indices.push_back (sample_index); + iterations_without_a_sample = 0; + } + else + ++iterations_without_a_sample; + + // If no valid samples can be found, relax the inter-sample distance requirements + if (iterations_without_a_sample >= max_iterations_without_a_sample) + { + PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); + PCL_WARN ("No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n", + iterations_without_a_sample, 0.5*min_sample_distance); + + min_sample_distance_ *= 0.5f; + min_sample_distance = min_sample_distance_; + iterations_without_a_sample = 0; + } + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusInitialAlignment::findSimilarFeatures ( + const FeatureCloud &input_features, const std::vector &sample_indices, + std::vector &corresponding_indices) +{ + std::vector nn_indices (k_correspondences_); + std::vector nn_distances (k_correspondences_); + + corresponding_indices.resize (sample_indices.size ()); + for (std::size_t i = 0; i < sample_indices.size (); ++i) + { + // Find the k features nearest to input_features.points[sample_indices[i]] + feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances); + + // Select one at random and add it to corresponding_indices + int random_correspondence = getRandomIndex (k_correspondences_); + corresponding_indices[i] = nn_indices[random_correspondence]; + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::SampleConsensusInitialAlignment::computeErrorMetric ( + const PointCloudSource &cloud, float) +{ + std::vector nn_index (1); + std::vector nn_distance (1); + + const ErrorFunctor & compute_error = *error_functor_; + float error = 0; + + for (int i = 0; i < static_cast (cloud.points.size ()); ++i) + { + // Find the distance between cloud.points[i] and its nearest neighbor in the target point cloud + tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance); + + // Compute the error + error += compute_error (nn_distance[0]); + } + return (error); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusInitialAlignment::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) +{ + // Some sanity checks first + 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 (input_->size () != input_features_->size ()) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n", + input_->size (), input_features_->size ()); + return; + } + + if (target_->size () != target_features_->size ()) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n", + target_->size (), target_features_->size ()); + return; + } + + if (!error_functor_) + error_functor_.reset (new TruncatedError (static_cast (corr_dist_threshold_))); + + + std::vector sample_indices (nr_samples_); + std::vector corresponding_indices (nr_samples_); + PointCloudSource input_transformed; + float lowest_error (0); + + final_transformation_ = guess; + int i_iter = 0; + converged_ = false; + 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 (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_); + + // Transform the data and compute the error + transformPointCloud (*input_, input_transformed, transformation_); + float error = computeErrorMetric (input_transformed, static_cast (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_; + converged_=true; + } + } + + // Apply the final transformation + transformPointCloud (*input_, output, final_transformation_); +} + +#endif //#ifndef IA_RANSAC_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/icp.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/icp.hpp new file mode 100755 index 0000000..fb7f62f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/icp.hpp @@ -0,0 +1,300 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_IMPL_ICP_HPP_ +#define PCL_REGISTRATION_IMPL_ICP_HPP_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IterativeClosestPoint::transformCloud ( + const PointCloudSource &input, + PointCloudSource &output, + const Matrix4 &transform) +{ + Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t; + Eigen::Matrix4f tr = transform.template cast (); + + // XYZ is ALWAYS present due to the templatization, so we only have to check for normals + if (source_has_normals_) + { + Eigen::Vector3f nt, nt_t; + Eigen::Matrix3f rot = tr.block<3, 3> (0, 0); + + for (std::size_t i = 0; i < input.size (); ++i) + { + const std::uint8_t* data_in = reinterpret_cast (&input[i]); + std::uint8_t* data_out = reinterpret_cast (&output[i]); + memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float)); + memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float)); + memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float)); + + if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) + continue; + + pt_t = tr * pt; + + memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float)); + memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float)); + memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float)); + + memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float)); + memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float)); + memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float)); + + if (!std::isfinite (nt[0]) || !std::isfinite (nt[1]) || !std::isfinite (nt[2])) + continue; + + nt_t = rot * nt; + + memcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float)); + memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float)); + memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float)); + } + } + else + { + for (std::size_t i = 0; i < input.size (); ++i) + { + const std::uint8_t* data_in = reinterpret_cast (&input[i]); + std::uint8_t* data_out = reinterpret_cast (&output[i]); + memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float)); + memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float)); + memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float)); + + if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2])) + continue; + + pt_t = tr * pt; + + memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float)); + memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float)); + memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float)); + } + } + +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IterativeClosestPoint::computeTransformation ( + PointCloudSource &output, const Matrix4 &guess) +{ + // Point cloud containing the correspondences of each point in + PointCloudSourcePtr input_transformed (new PointCloudSource); + + nr_iterations_ = 0; + converged_ = false; + + // Initialise final transformation to the guessed one + final_transformation_ = guess; + + // If the guessed transformation is non identity + if (guess != Matrix4::Identity ()) + { + input_transformed->resize (input_->size ()); + // Apply guessed transformation prior to search for neighbours + transformCloud (*input_, *input_transformed, guess); + } + else + *input_transformed = *input_; + + transformation_ = Matrix4::Identity (); + + // Make blobs if necessary + determineRequiredBlobData (); + PCLPointCloud2::Ptr target_blob (new PCLPointCloud2); + if (need_target_blob_) + pcl::toPCLPointCloud2 (*target_, *target_blob); + + // Pass in the default target for the Correspondence Estimation/Rejection code + correspondence_estimation_->setInputTarget (target_); + if (correspondence_estimation_->requiresTargetNormals ()) + correspondence_estimation_->setTargetNormals (target_blob); + // Correspondence Rejectors need a binary blob + for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + if (rej->requiresTargetPoints ()) + rej->setTargetPoints (target_blob); + if (rej->requiresTargetNormals () && target_has_normals_) + rej->setTargetNormals (target_blob); + } + + convergence_criteria_->setMaximumIterations (max_iterations_); + convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_); + convergence_criteria_->setTranslationThreshold (transformation_epsilon_); + if (transformation_rotation_epsilon_ > 0) + convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_); + else + convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_); + + // Repeat until convergence + do + { + // Get blob data if needed + PCLPointCloud2::Ptr input_transformed_blob; + if (need_source_blob_) + { + input_transformed_blob.reset (new PCLPointCloud2); + toPCLPointCloud2 (*input_transformed, *input_transformed_blob); + } + // Save the previously estimated transformation + previous_transformation_ = transformation_; + + // Set the source each iteration, to ensure the dirty flag is updated + correspondence_estimation_->setInputSource (input_transformed); + if (correspondence_estimation_->requiresSourceNormals ()) + correspondence_estimation_->setSourceNormals (input_transformed_blob); + // Estimate correspondences + if (use_reciprocal_correspondence_) + correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_); + else + correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_); + + //if (correspondence_rejectors_.empty ()) + CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_)); + for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ()); + if (rej->requiresSourcePoints ()) + rej->setSourcePoints (input_transformed_blob); + if (rej->requiresSourceNormals () && source_has_normals_) + rej->setSourceNormals (input_transformed_blob); + rej->setInputCorrespondences (temp_correspondences); + rej->getCorrespondences (*correspondences_); + // Modify input for the next iteration + if (i < correspondence_rejectors_.size () - 1) + *temp_correspondences = *correspondences_; + } + + std::size_t cnt = correspondences_->size (); + // Check whether we have enough correspondences + if (static_cast (cnt) < min_number_correspondences_) + { + PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); + convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES); + converged_ = false; + break; + } + + // Estimate the transform + transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_); + + // Transform the data + transformCloud (*input_transformed, *input_transformed, 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 ); + + converged_ = static_cast ((*convergence_criteria_)); + } + while (convergence_criteria_->getConvergenceState() == pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NOT_CONVERGED); + + // Transform the input cloud using the final transformation + PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", + final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3), + final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3), + final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3), + final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3)); + + // Copy all the values + output = *input_; + // Transform the XYZ + normals + transformCloud (*input_, output, final_transformation_); +} + +template void +pcl::IterativeClosestPoint::determineRequiredBlobData () +{ + need_source_blob_ = false; + need_target_blob_ = false; + // Check estimator + need_source_blob_ |= correspondence_estimation_->requiresSourceNormals (); + need_target_blob_ |= correspondence_estimation_->requiresTargetNormals (); + // Add warnings if necessary + if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ()); + } + if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ()); + } + // Check rejectors + for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + need_source_blob_ |= rej->requiresSourcePoints (); + need_source_blob_ |= rej->requiresSourceNormals (); + need_target_blob_ |= rej->requiresTargetPoints (); + need_target_blob_ |= rej->requiresTargetNormals (); + if (rej->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + if (rej->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::IterativeClosestPointWithNormals::transformCloud ( + const PointCloudSource &input, + PointCloudSource &output, + const Matrix4 &transform) +{ + pcl::transformPointCloudWithNormals (input, output, transform); +} + + +#endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/icp_nl.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/icp_nl.hpp new file mode 100755 index 0000000..0cbf0ba --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/icp_nl.hpp @@ -0,0 +1,44 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_ICP_NL_HPP_ +#define PCL_REGISTRATION_ICP_NL_HPP_ + +#endif /* PCL_REGISTRATION_ICP_NL_HPP_ */ + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/incremental_registration.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/incremental_registration.hpp new file mode 100755 index 0000000..47b1843 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/incremental_registration.hpp @@ -0,0 +1,103 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015, Michael 'v4hn' Goerner + * Copyright (c) 2015-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_ +#define PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_ + +template +pcl::registration::IncrementalRegistration::IncrementalRegistration () : + delta_transform_ (Matrix4::Identity ()), + abs_transform_ (Matrix4::Identity ()) +{} + +template bool +pcl::registration::IncrementalRegistration::registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate) +{ + assert (registration_); + + if (!last_cloud_) + { + last_cloud_ = cloud; + abs_transform_ = delta_transform_ = delta_estimate; + return (true); + } + + registration_->setInputSource (cloud); + registration_->setInputTarget (last_cloud_); + + { + pcl::PointCloud p; + registration_->align (p, delta_estimate); + } + + bool converged = registration_->hasConverged (); + + if ( converged ){ + delta_transform_ = registration_->getFinalTransformation (); + abs_transform_ *= delta_transform_; + last_cloud_ = cloud; + } + + return (converged); +} + +template inline typename pcl::registration::IncrementalRegistration::Matrix4 +pcl::registration::IncrementalRegistration::getDeltaTransform () const +{ + return (delta_transform_); +} + +template inline typename pcl::registration::IncrementalRegistration::Matrix4 +pcl::registration::IncrementalRegistration::getAbsoluteTransform () const +{ + return (abs_transform_); +} + +template inline void +pcl::registration::IncrementalRegistration::reset () +{ + last_cloud_.reset (); + delta_transform_ = abs_transform_ = Matrix4::Identity (); +} + +template inline void +pcl::registration::IncrementalRegistration::setRegistration (RegistrationPtr registration) +{ + registration_ = registration; +} + +#endif /*PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_*/ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/joint_icp.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/joint_icp.hpp new file mode 100755 index 0000000..f8b16e7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/joint_icp.hpp @@ -0,0 +1,326 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ +#define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ + +#include +#include +#include + + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::JointIterativeClosestPoint::computeTransformation ( + PointCloudSource &output, const Matrix4 &guess) +{ + // Point clouds containing the correspondences of each point in + if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ()) + { + PCL_ERROR ("[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n", + getClassName ().c_str ()); + return; + } + bool manual_correspondence_estimations_set = true; + if (correspondence_estimations_.empty ()) + { + manual_correspondence_estimations_set = false; + correspondence_estimations_.resize (sources_.size ()); + for (std::size_t i = 0; i < sources_.size (); i++) + { + correspondence_estimations_[i] = correspondence_estimation_->clone (); + KdTreeReciprocalPtr src_tree (new KdTreeReciprocal); + KdTreePtr tgt_tree (new KdTree); + correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree); + correspondence_estimations_[i]->setSearchMethodSource (src_tree); + } + } + if (correspondence_estimations_.size () != sources_.size ()) + { + PCL_ERROR ("[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n", + getClassName ().c_str ()); + return; + } + std::vector inputs_transformed (sources_.size ()); + for (std::size_t i = 0; i < sources_.size (); i++) + { + inputs_transformed[i].reset (new PointCloudSource); + } + + nr_iterations_ = 0; + converged_ = false; + + // Initialise final transformation to the guessed one + final_transformation_ = guess; + + // Make a combined transformed input and output + std::vector input_offsets (sources_.size ()); + std::vector target_offsets (targets_.size ()); + PointCloudSourcePtr sources_combined (new PointCloudSource); + PointCloudSourcePtr inputs_transformed_combined (new PointCloudSource); + PointCloudTargetPtr targets_combined (new PointCloudTarget); + std::size_t input_offset = 0; + std::size_t target_offset = 0; + for (std::size_t i = 0; i < sources_.size (); i++) + { + // If the guessed transformation is non identity + if (guess != Matrix4::Identity ()) + { + // Apply guessed transformation prior to search for neighbours + this->transformCloud (*sources_[i], *inputs_transformed[i], guess); + } + else + { + *inputs_transformed[i] = *sources_[i]; + } + *sources_combined += *sources_[i]; + *inputs_transformed_combined += *inputs_transformed[i]; + *targets_combined += *targets_[i]; + input_offsets[i] = input_offset; + target_offsets[i] = target_offset; + input_offset += inputs_transformed[i]->size (); + target_offset += targets_[i]->size (); + } + + + + transformation_ = Matrix4::Identity (); + // Make blobs if necessary + determineRequiredBlobData (); + // Pass in the default target for the Correspondence Estimation/Rejection code + for (std::size_t i = 0; i < sources_.size (); i++) + { + correspondence_estimations_[i]->setInputTarget (targets_[i]); + if (correspondence_estimations_[i]->requiresTargetNormals ()) + { + PCLPointCloud2::Ptr target_blob (new PCLPointCloud2); + pcl::toPCLPointCloud2 (*targets_[i], *target_blob); + correspondence_estimations_[i]->setTargetNormals (target_blob); + } + } + + PCLPointCloud2::Ptr targets_combined_blob (new PCLPointCloud2); + if (!correspondence_rejectors_.empty () && need_target_blob_) + pcl::toPCLPointCloud2 (*targets_combined, *targets_combined_blob); + + for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + if (rej->requiresTargetPoints ()) + rej->setTargetPoints (targets_combined_blob); + if (rej->requiresTargetNormals () && target_has_normals_) + rej->setTargetNormals (targets_combined_blob); + } + + convergence_criteria_->setMaximumIterations (max_iterations_); + convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_); + convergence_criteria_->setTranslationThreshold (transformation_epsilon_); + convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_); + + // Repeat until convergence + std::vector partial_correspondences_ (sources_.size ()); + for (std::size_t i = 0; i < sources_.size (); i++) + { + partial_correspondences_[i].reset (new pcl::Correspondences); + } + + do + { + // Save the previously estimated transformation + previous_transformation_ = transformation_; + + // Set the source each iteration, to ensure the dirty flag is updated + correspondences_->clear (); + for (std::size_t i = 0; i < correspondence_estimations_.size (); i++) + { + correspondence_estimations_[i]->setInputSource (inputs_transformed[i]); + // Get blob data if needed + if (correspondence_estimations_[i]->requiresSourceNormals ()) + { + PCLPointCloud2::Ptr input_transformed_blob (new PCLPointCloud2); + toPCLPointCloud2 (*inputs_transformed[i], *input_transformed_blob); + correspondence_estimations_[i]->setSourceNormals (input_transformed_blob); + } + + // Estimate correspondences on each cloud pair separately + if (use_reciprocal_correspondence_) + { + correspondence_estimations_[i]->determineReciprocalCorrespondences (*partial_correspondences_[i], corr_dist_threshold_); + } + else + { + correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_); + } + PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n", + getClassName ().c_str (), + partial_correspondences_[i]->size (), i); + for (std::size_t j = 0; j < partial_correspondences_[i]->size (); j++) + { + pcl::Correspondence corr = partial_correspondences_[i]->at (j); + // Update the offsets to be for the combined clouds + corr.index_query += input_offsets[i]; + corr.index_match += target_offsets[i]; + correspondences_->push_back (corr); + } + } + PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ()); + + PCLPointCloud2::Ptr inputs_transformed_combined_blob; + if (need_source_blob_) + { + inputs_transformed_combined_blob.reset (new PCLPointCloud2); + toPCLPointCloud2 (*inputs_transformed_combined, *inputs_transformed_combined_blob); + } + CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_)); + for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i) + { + PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ()); + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ()); + if (rej->requiresSourcePoints ()) + rej->setSourcePoints (inputs_transformed_combined_blob); + if (rej->requiresSourceNormals () && source_has_normals_) + rej->setSourceNormals (inputs_transformed_combined_blob); + rej->setInputCorrespondences (temp_correspondences); + rej->getCorrespondences (*correspondences_); + // Modify input for the next iteration + if (i < correspondence_rejectors_.size () - 1) + *temp_correspondences = *correspondences_; + } + + int cnt = correspondences_->size (); + // Check whether we have enough correspondences + if (cnt < min_number_correspondences_) + { + PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); + convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES); + converged_ = false; + break; + } + + // Estimate the transform jointly, on a combined correspondence set + transformation_estimation_->estimateRigidTransformation (*inputs_transformed_combined, *targets_combined, *correspondences_, transformation_); + + // Transform the combined data + this->transformCloud (*inputs_transformed_combined, *inputs_transformed_combined, transformation_); + // And all its components + for (std::size_t i = 0; i < sources_.size (); i++) + { + this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], 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 ); + + converged_ = static_cast ((*convergence_criteria_)); + } + while (!converged_); + + PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", + final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3), + final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3), + final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3), + final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3)); + + // For fitness checks, etc, we'll use an aggregated cloud for now (should be evaluating independently for correctness, but this requires propagating a few virtual methods from Registration) + IterativeClosestPoint::setInputSource (sources_combined); + IterativeClosestPoint::setInputTarget (targets_combined); + + // If we automatically set the correspondence estimators, we should clear them now + if (!manual_correspondence_estimations_set) + { + correspondence_estimations_.clear (); + } + + + // By definition, this method will return an empty cloud (for compliance with the ICP API). + // We can figure out a better solution, if necessary. + output = PointCloudSource (); +} + + template void +pcl::JointIterativeClosestPoint::determineRequiredBlobData () +{ + need_source_blob_ = false; + need_target_blob_ = false; + // Check estimators + for (std::size_t i = 0; i < correspondence_estimations_.size (); i++) + { + CorrespondenceEstimationPtr& ce = correspondence_estimations_[i]; + + need_source_blob_ |= ce->requiresSourceNormals (); + need_target_blob_ |= ce->requiresTargetNormals (); + // Add warnings if necessary + if (ce->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ()); + } + if (ce->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ()); + } + } + // Check rejectors + for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++) + { + registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i]; + need_source_blob_ |= rej->requiresSourcePoints (); + need_source_blob_ |= rej->requiresSourceNormals (); + need_target_blob_ |= rej->requiresTargetPoints (); + need_target_blob_ |= rej->requiresTargetNormals (); + if (rej->requiresSourceNormals () && !source_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + if (rej->requiresTargetNormals () && !target_has_normals_) + { + PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ()); + } + } +} + + +#endif /* PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ */ + + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/lum.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/lum.hpp new file mode 100755 index 0000000..9a54279 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/lum.hpp @@ -0,0 +1,427 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lum.hpp 5663 2012-05-02 13:49:39Z florentinus $ + * + */ + +#ifndef PCL_REGISTRATION_IMPL_LUM_HPP_ +#define PCL_REGISTRATION_IMPL_LUM_HPP_ + +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::LUM::setLoopGraph (const SLAMGraphPtr &slam_graph) +{ + slam_graph_ = slam_graph; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline typename pcl::registration::LUM::SLAMGraphPtr +pcl::registration::LUM::getLoopGraph () const +{ + return (slam_graph_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::registration::LUM::SLAMGraph::vertices_size_type +pcl::registration::LUM::getNumVertices () const +{ + return (num_vertices (*slam_graph_)); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::LUM::setMaxIterations (int max_iterations) +{ + max_iterations_ = max_iterations; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline int +pcl::registration::LUM::getMaxIterations () const +{ + return (max_iterations_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::LUM::setConvergenceThreshold (float convergence_threshold) +{ + convergence_threshold_ = convergence_threshold; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline float +pcl::registration::LUM::getConvergenceThreshold () const +{ + return (convergence_threshold_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::registration::LUM::Vertex +pcl::registration::LUM::addPointCloud (const 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 inline void +pcl::registration::LUM::setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud) +{ + if (vertex >= getNumVertices ()) + { + PCL_ERROR("[pcl::registration::LUM::setPointCloud] You are attempting to set a point cloud to a non-existing graph vertex.\n"); + return; + } + (*slam_graph_)[vertex].cloud_ = cloud; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline typename pcl::registration::LUM::PointCloudPtr +pcl::registration::LUM::getPointCloud (const Vertex &vertex) const +{ + if (vertex >= getNumVertices ()) + { + PCL_ERROR("[pcl::registration::LUM::getPointCloud] You are attempting to get a point cloud from a non-existing graph vertex.\n"); + return (PointCloudPtr ()); + } + return ((*slam_graph_)[vertex].cloud_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::LUM::setPose (const Vertex &vertex, const Eigen::Vector6f &pose) +{ + if (vertex >= getNumVertices ()) + { + PCL_ERROR("[pcl::registration::LUM::setPose] You are attempting to set a pose estimate to a non-existing graph vertex.\n"); + return; + } + if (vertex == 0) + { + PCL_ERROR("[pcl::registration::LUM::setPose] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n"); + return; + } + (*slam_graph_)[vertex].pose_ = pose; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline Eigen::Vector6f +pcl::registration::LUM::getPose (const Vertex &vertex) const +{ + if (vertex >= getNumVertices ()) + { + PCL_ERROR("[pcl::registration::LUM::getPose] You are attempting to get a pose estimate from a non-existing graph vertex.\n"); + return (Eigen::Vector6f::Zero ()); + } + return ((*slam_graph_)[vertex].pose_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline Eigen::Affine3f +pcl::registration::LUM::getTransformation (const Vertex &vertex) const +{ + Eigen::Vector6f pose = getPose (vertex); + return (pcl::getTransformation (pose (0), pose (1), pose (2), pose (3), pose (4), pose (5))); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::LUM::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs) +{ + if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex) + { + PCL_ERROR("[pcl::registration::LUM::setCorrespondences] You are attempting to set a set of correspondences between non-existing or identical graph vertices.\n"); + return; + } + Edge e; + bool present; + std::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_); + if (!present) + std::tie (e, present) = add_edge (source_vertex, target_vertex, *slam_graph_); + (*slam_graph_)[e].corrs_ = corrs; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline pcl::CorrespondencesPtr +pcl::registration::LUM::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const +{ + if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ()) + { + PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences between non-existing graph vertices.\n"); + return (pcl::CorrespondencesPtr ()); + } + Edge e; + bool present; + std::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_); + if (!present) + { + PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences from a non-existing graph edge.\n"); + return (pcl::CorrespondencesPtr ()); + } + return ((*slam_graph_)[e].corrs_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::LUM::compute () +{ + int n = static_cast (getNumVertices ()); + if (n < 2) + { + PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n"); + return; + } + for (int i = 0; i < max_iterations_; ++i) + { + // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_) + typename SLAMGraph::edge_iterator e, e_end; + for (std::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e) + computeEdge (*e); + + // Declare matrices G and B + Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1)); + Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1)); + + // Start at 1 because 0 is the reference pose + for (int vi = 1; vi != n; ++vi) + { + for (int vj = 0; vj != n; ++vj) + { + // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge + Edge e; + bool present1; + std::tie (e, present1) = edge (vi, vj, *slam_graph_); + if (!present1) + { + bool present2; + std::tie (e, present2) = edge (vj, vi, *slam_graph_); + if (!present2) + continue; + } + + // Fill in elements of G and B + if (vj > 0) + G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_; + G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_; + B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_; + } + } + + // Computation of the linear equation system: GX = B + // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse) + Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B); + + // Update the poses + float sum = 0.0; + for (int vi = 1; vi != n; ++vi) + { + Eigen::Vector6f difference_pose = static_cast (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6)); + sum += difference_pose.norm (); + setPose (vi, getPose (vi) + difference_pose); + } + + // Convergence check + if (sum <= convergence_threshold_ * static_cast (n - 1)) + return; + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::registration::LUM::PointCloudPtr +pcl::registration::LUM::getTransformedCloud (const Vertex &vertex) const +{ + PointCloudPtr out (new PointCloud); + pcl::transformPointCloud (*getPointCloud (vertex), *out, getTransformation (vertex)); + return (out); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::registration::LUM::PointCloudPtr +pcl::registration::LUM::getConcatenatedCloud () const +{ + PointCloudPtr out (new PointCloud); + typename SLAMGraph::vertex_iterator v, v_end; + for (std::tie (v, v_end) = vertices (*slam_graph_); v != v_end; ++v) + { + PointCloud temp; + pcl::transformPointCloud (*getPointCloud (*v), temp, getTransformation (*v)); + *out += temp; + } + return (out); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::LUM::computeEdge (const Edge &e) +{ + // Get necessary local data from graph + PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_; + PointCloudPtr target_cloud = (*slam_graph_)[target (e, *slam_graph_)].cloud_; + Eigen::Vector6f source_pose = (*slam_graph_)[source (e, *slam_graph_)].pose_; + Eigen::Vector6f target_pose = (*slam_graph_)[target (e, *slam_graph_)].pose_; + pcl::CorrespondencesPtr corrs = (*slam_graph_)[e].corrs_; + + // Build the average and difference vectors for all correspondences + std::vector > corrs_aver (corrs->size ()); + std::vector > corrs_diff (corrs->size ()); + int oci = 0; // oci = output correspondence iterator + for (int ici = 0; ici != static_cast (corrs->size ()); ++ici) // ici = input correspondence iterator + { + // Compound the point pair onto the current pose + Eigen::Vector3f source_compounded = pcl::getTransformation (source_pose (0), source_pose (1), source_pose (2), source_pose (3), source_pose (4), source_pose (5)) * source_cloud->points[(*corrs)[ici].index_query].getVector3fMap (); + Eigen::Vector3f target_compounded = pcl::getTransformation (target_pose (0), target_pose (1), target_pose (2), target_pose (3), target_pose (4), target_pose (5)) * target_cloud->points[(*corrs)[ici].index_match].getVector3fMap (); + + // NaN points can not be passed to the remaining computational pipeline + if (!std::isfinite (source_compounded (0)) || !std::isfinite (source_compounded (1)) || !std::isfinite (source_compounded (2)) || !std::isfinite (target_compounded (0)) || !std::isfinite (target_compounded (1)) || !std::isfinite (target_compounded (2))) + continue; + + // Compute the point pair average and difference and store for later use + corrs_aver[oci] = 0.5 * (source_compounded + target_compounded); + corrs_diff[oci] = source_compounded - target_compounded; + oci++; + } + corrs_aver.resize (oci); + corrs_diff.resize (oci); + + // Need enough valid correspondences to get a good triangulation + if (oci < 3) + { + PCL_WARN("[pcl::registration::LUM::compute] The correspondences between vertex %d and %d do not contain enough valid correspondences to be considered for computation.\n", source (e, *slam_graph_), target (e, *slam_graph_)); + (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero (); + (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero (); + return; + } + + // Build the matrices M'M and M'Z + Eigen::Matrix6f MM = Eigen::Matrix6f::Zero (); + Eigen::Vector6f MZ = Eigen::Vector6f::Zero (); + for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator + { + // Fast computation of summation elements of M'M + MM (0, 4) -= corrs_aver[ci] (1); + MM (0, 5) += corrs_aver[ci] (2); + MM (1, 3) -= corrs_aver[ci] (2); + MM (1, 4) += corrs_aver[ci] (0); + MM (2, 3) += corrs_aver[ci] (1); + MM (2, 5) -= corrs_aver[ci] (0); + MM (3, 4) -= corrs_aver[ci] (0) * corrs_aver[ci] (2); + MM (3, 5) -= corrs_aver[ci] (0) * corrs_aver[ci] (1); + MM (4, 5) -= corrs_aver[ci] (1) * corrs_aver[ci] (2); + MM (3, 3) += corrs_aver[ci] (1) * corrs_aver[ci] (1) + corrs_aver[ci] (2) * corrs_aver[ci] (2); + MM (4, 4) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (1) * corrs_aver[ci] (1); + MM (5, 5) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (2) * corrs_aver[ci] (2); + + // Fast computation of M'Z + MZ (0) += corrs_diff[ci] (0); + MZ (1) += corrs_diff[ci] (1); + MZ (2) += corrs_diff[ci] (2); + MZ (3) += corrs_aver[ci] (1) * corrs_diff[ci] (2) - corrs_aver[ci] (2) * corrs_diff[ci] (1); + MZ (4) += corrs_aver[ci] (0) * corrs_diff[ci] (1) - corrs_aver[ci] (1) * corrs_diff[ci] (0); + MZ (5) += corrs_aver[ci] (2) * corrs_diff[ci] (0) - corrs_aver[ci] (0) * corrs_diff[ci] (2); + } + // Remaining elements of M'M + MM (0, 0) = MM (1, 1) = MM (2, 2) = static_cast (oci); + MM (4, 0) = MM (0, 4); + MM (5, 0) = MM (0, 5); + MM (3, 1) = MM (1, 3); + MM (4, 1) = MM (1, 4); + MM (3, 2) = MM (2, 3); + MM (5, 2) = MM (2, 5); + MM (4, 3) = MM (3, 4); + MM (5, 3) = MM (3, 5); + MM (5, 4) = MM (4, 5); + + // Compute pose difference estimation + Eigen::Vector6f D = static_cast (MM.inverse () * MZ); + + // Compute s^2 + float ss = 0.0f; + for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator + ss += static_cast (std::pow (corrs_diff[ci] (0) - (D (0) + corrs_aver[ci] (2) * D (5) - corrs_aver[ci] (1) * D (4)), 2.0f) + + std::pow (corrs_diff[ci] (1) - (D (1) + corrs_aver[ci] (0) * D (4) - corrs_aver[ci] (2) * D (3)), 2.0f) + + std::pow (corrs_diff[ci] (2) - (D (2) + corrs_aver[ci] (1) * D (3) - corrs_aver[ci] (0) * D (5)), 2.0f)); + + // When reaching the limitations of computation due to linearization + if (ss < 0.0000000000001 || !std::isfinite (ss)) + { + (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero (); + (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero (); + return; + } + + // Store the results in the slam graph + (*slam_graph_)[e].cinv_ = MM * (1.0f / ss); + (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template inline Eigen::Matrix6f +pcl::registration::LUM::incidenceCorrection (const Eigen::Vector6f &pose) +{ + Eigen::Matrix6f out = Eigen::Matrix6f::Identity (); + float cx = std::cos (pose (3)), sx = sinf (pose (3)), cy = std::cos (pose (4)), sy = sinf (pose (4)); + out (0, 4) = pose (1) * sx - pose (2) * cx; + out (0, 5) = pose (1) * cx * cy + pose (2) * sx * cy; + out (1, 3) = pose (2); + out (1, 4) = -pose (0) * sx; + out (1, 5) = -pose (0) * cx * cy + pose (2) * sy; + out (2, 3) = -pose (1); + out (2, 4) = pose (0) * cx; + out (2, 5) = -pose (0) * sx * cy - pose (1) * sy; + out (3, 5) = sy; + out (4, 4) = sx; + out (4, 5) = cx * cy; + out (5, 4) = cx; + out (5, 5) = -sx * cy; + return (out); +} + +#define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM; + +#endif // PCL_REGISTRATION_IMPL_LUM_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/meta_registration.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/meta_registration.hpp new file mode 100755 index 0000000..b1a9d16 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/meta_registration.hpp @@ -0,0 +1,101 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015, Michael 'v4hn' Goerner + * Copyright (c) 2015-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_ +#define PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_ + +template +pcl::registration::MetaRegistration::MetaRegistration () : + abs_transform_ (Matrix4::Identity ()) +{} + +template bool +pcl::registration::MetaRegistration::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate) +{ + assert (registration_); + + PointCloudPtr new_cloud_transformed (new pcl::PointCloud ()); + + if (!full_cloud_) + { + pcl::transformPointCloud(*new_cloud, *new_cloud_transformed, delta_estimate); + full_cloud_ = new_cloud_transformed; + abs_transform_ = delta_estimate; + return (true); + } + + registration_->setInputSource (new_cloud); + registration_->setInputTarget (full_cloud_); + + registration_->align (*new_cloud_transformed, abs_transform_ * delta_estimate); + + bool converged = registration_->hasConverged (); + + if (converged) + { + abs_transform_ = registration_->getFinalTransformation (); + *full_cloud_ += *new_cloud_transformed; + } + + return (converged); +} + +template inline typename pcl::registration::MetaRegistration::Matrix4 +pcl::registration::MetaRegistration::getAbsoluteTransform () const +{ + return (abs_transform_); +} + +template inline void +pcl::registration::MetaRegistration::reset () +{ + full_cloud_.reset (); + abs_transform_ = Matrix4::Identity (); +} + +template inline void +pcl::registration::MetaRegistration::setRegistration (RegistrationPtr reg) +{ + registration_ = reg; +} + +template inline typename pcl::registration::MetaRegistration::PointCloudConstPtr +pcl::registration::MetaRegistration::getMetaCloud () const +{ + return full_cloud_; +} +#endif /*PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_*/ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/ndt.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/ndt.hpp new file mode 100755 index 0000000..d5bfd77 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/ndt.hpp @@ -0,0 +1,751 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_NDT_IMPL_H_ +#define PCL_REGISTRATION_NDT_IMPL_H_ + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::NormalDistributionsTransform::NormalDistributionsTransform () + : target_cells_ () + , resolution_ (1.0f) + , step_size_ (0.1) + , outlier_ratio_ (0.55) + , gauss_d1_ () + , gauss_d2_ () + , trans_probability_ () +{ + reg_name_ = "NormalDistributionsTransform"; + + double gauss_c1, gauss_c2, gauss_d3; + + // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] + gauss_c1 = 10.0 * (1 - outlier_ratio_); + gauss_c2 = outlier_ratio_ / pow (resolution_, 3); + gauss_d3 = -std::log (gauss_c2); + gauss_d1_ = -std::log ( gauss_c1 + gauss_c2 ) - gauss_d3; + gauss_d2_ = -2 * std::log ((-std::log ( gauss_c1 * std::exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_); + + transformation_epsilon_ = 0.1; + max_iterations_ = 35; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalDistributionsTransform::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) +{ + nr_iterations_ = 0; + converged_ = false; + + double gauss_c1, gauss_c2, gauss_d3; + + // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009] + gauss_c1 = 10 * (1 - outlier_ratio_); + gauss_c2 = outlier_ratio_ / pow (resolution_, 3); + gauss_d3 = -std::log (gauss_c2); + gauss_d1_ = -std::log ( gauss_c1 + gauss_c2 ) - gauss_d3; + gauss_d2_ = -2 * std::log ((-std::log ( gauss_c1 * std::exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_); + + 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); + } + + // Initialize Point Gradient and Hessian + point_gradient_.setZero (); + point_gradient_.block<3, 3>(0, 0).setIdentity (); + point_hessian_.setZero (); + + Eigen::Transform eig_transformation; + eig_transformation.matrix () = final_transformation_; + + // Convert initial guess matrix to 6 element transformation vector + Eigen::Matrix p, delta_p, score_gradient; + Eigen::Vector3f init_translation = eig_transformation.translation (); + Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2); + p << init_translation (0), init_translation (1), init_translation (2), + init_rotation (0), init_rotation (1), init_rotation (2); + + Eigen::Matrix hessian; + + double score = 0; + + // Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination. + score = computeDerivatives (score_gradient, hessian, output, p); + + while (!converged_) + { + // Store previous transformation + previous_transformation_ = transformation_; + + // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009] + Eigen::JacobiSVD > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); + // Negative for maximization as opposed to minimization + delta_p = sv.solve (-score_gradient); + + //Calculate step length with guarnteed sufficient decrease [More, Thuente 1994] + double delta_p_norm = delta_p.norm (); + + if (delta_p_norm == 0 || std::isnan(delta_p_norm)) + { + trans_probability_ = score / static_cast (input_->points.size ()); + converged_ = delta_p_norm == delta_p_norm; + return; + } + + delta_p.normalize (); + delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output); + delta_p *= delta_p_norm; + + + transformation_ = (Eigen::Translation (static_cast (delta_p (0)), static_cast (delta_p (1)), static_cast (delta_p (2))) * + Eigen::AngleAxis (static_cast (delta_p (3)), Eigen::Vector3f::UnitX ()) * + Eigen::AngleAxis (static_cast (delta_p (4)), Eigen::Vector3f::UnitY ()) * + Eigen::AngleAxis (static_cast (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix (); + + + p += delta_p; + + // Update Visualizer (untested) + if (update_visualizer_) + update_visualizer_ (output, std::vector(), *target_, std::vector() ); + + double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1); + double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) + + transformation_.coeff (1, 3) * transformation_.coeff (1, 3) + + transformation_.coeff (2, 3) * transformation_.coeff (2, 3); + + nr_iterations_++; + + if (nr_iterations_ >= max_iterations_ || + ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) || + ((transformation_epsilon_ <= 0) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) || + ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0))) + { + converged_ = true; + } + } + + // Store transformation probability. The realtive differences within each scan registration are accurate + // but the normalization constants need to be modified for it to be globally accurate + trans_probability_ = score / static_cast (input_->points.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::NormalDistributionsTransform::computeDerivatives (Eigen::Matrix &score_gradient, + Eigen::Matrix &hessian, + PointCloudSource &trans_cloud, + Eigen::Matrix &p, + bool compute_hessian) +{ + // Original Point and Transformed Point + PointSource x_pt, x_trans_pt; + // Original Point and Transformed Point (for math) + Eigen::Vector3d x, x_trans; + // Occupied Voxel + TargetGridLeafConstPtr cell; + // Inverse Covariance of Occupied Voxel + Eigen::Matrix3d c_inv; + + score_gradient.setZero (); + hessian.setZero (); + double score = 0; + + // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009] + computeAngleDerivatives (p); + + // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] + for (std::size_t idx = 0; idx < input_->points.size (); idx++) + { + x_trans_pt = trans_cloud.points[idx]; + + // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + std::vector distances; + target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances); + + for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it) + { + cell = *neighborhood_it; + x_pt = input_->points[idx]; + x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z); + + x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); + + // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] + x_trans -= cell->getMean (); + // Uses precomputed covariance for speed. + c_inv = cell->getInverseCov (); + + // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] + computePointDerivatives (x); + // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] + score += updateDerivatives (score_gradient, hessian, x_trans, c_inv, compute_hessian); + + } + } + return (score); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalDistributionsTransform::computeAngleDerivatives (Eigen::Matrix &p, bool compute_hessian) +{ + // Simplified math for near 0 angles + double cx, cy, cz, sx, sy, sz; + if (std::abs (p (3)) < 10e-5) + { + //p(3) = 0; + cx = 1.0; + sx = 0.0; + } + else + { + cx = std::cos (p (3)); + sx = sin (p (3)); + } + if (std::abs (p (4)) < 10e-5) + { + //p(4) = 0; + cy = 1.0; + sy = 0.0; + } + else + { + cy = std::cos (p (4)); + sy = sin (p (4)); + } + + if (std::abs (p (5)) < 10e-5) + { + //p(5) = 0; + cz = 1.0; + sz = 0.0; + } + else + { + cz = std::cos (p (5)); + sz = sin (p (5)); + } + + // Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009] + j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy); + j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy); + j_ang_c_ << (-sy * cz), sy * sz, cy; + j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy; + j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy); + j_ang_f_ << (-cy * sz), (-cy * cz), 0; + j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0; + j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0; + + if (compute_hessian) + { + // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009] + h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy; + h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy); + + h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy); + h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy); + + h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0; + h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0; + + h_ang_d1_ << (-cy * cz), (cy * sz), (sy); + h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy); + h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy); + + h_ang_e1_ << (sy * sz), (sy * cz), 0; + h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0; + h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0; + + h_ang_f1_ << (-cy * cz), (cy * sz), 0; + h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0; + h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalDistributionsTransform::computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian) +{ + // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. + // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009] + point_gradient_ (1, 3) = x.dot (j_ang_a_); + point_gradient_ (2, 3) = x.dot (j_ang_b_); + point_gradient_ (0, 4) = x.dot (j_ang_c_); + point_gradient_ (1, 4) = x.dot (j_ang_d_); + point_gradient_ (2, 4) = x.dot (j_ang_e_); + point_gradient_ (0, 5) = x.dot (j_ang_f_); + point_gradient_ (1, 5) = x.dot (j_ang_g_); + point_gradient_ (2, 5) = x.dot (j_ang_h_); + + if (compute_hessian) + { + // Vectors from Equation 6.21 [Magnusson 2009] + Eigen::Vector3d a, b, c, d, e, f; + + a << 0, x.dot (h_ang_a2_), x.dot (h_ang_a3_); + b << 0, x.dot (h_ang_b2_), x.dot (h_ang_b3_); + c << 0, x.dot (h_ang_c2_), x.dot (h_ang_c3_); + d << x.dot (h_ang_d1_), x.dot (h_ang_d2_), x.dot (h_ang_d3_); + e << x.dot (h_ang_e1_), x.dot (h_ang_e2_), x.dot (h_ang_e3_); + f << x.dot (h_ang_f1_), x.dot (h_ang_f2_), x.dot (h_ang_f3_); + + // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. + // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009] + point_hessian_.block<3, 1>(9, 3) = a; + point_hessian_.block<3, 1>(12, 3) = b; + point_hessian_.block<3, 1>(15, 3) = c; + point_hessian_.block<3, 1>(9, 4) = b; + point_hessian_.block<3, 1>(12, 4) = d; + point_hessian_.block<3, 1>(15, 4) = e; + point_hessian_.block<3, 1>(9, 5) = c; + point_hessian_.block<3, 1>(12, 5) = e; + point_hessian_.block<3, 1>(15, 5) = f; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::NormalDistributionsTransform::updateDerivatives (Eigen::Matrix &score_gradient, + Eigen::Matrix &hessian, + Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv, + bool compute_hessian) +{ + Eigen::Vector3d cov_dxd_pi; + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = std::exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2); + // Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009] + double score_inc = -gauss_d1_ * e_x_cov_x; + + e_x_cov_x = gauss_d2_ * e_x_cov_x; + + // Error checking for invalid values. + if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) + return (0); + + // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + e_x_cov_x *= gauss_d1_; + + + for (int i = 0; i < 6; i++) + { + // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + cov_dxd_pi = c_inv * point_gradient_.col (i); + + // Update gradient, Equation 6.12 [Magnusson 2009] + score_gradient (i) += x_trans.dot (cov_dxd_pi) * e_x_cov_x; + + if (compute_hessian) + { + for (Eigen::Index j = 0; j < hessian.cols (); j++) + { + // Update hessian, Equation 6.13 [Magnusson 2009] + hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) + + x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) + + point_gradient_.col (j).dot (cov_dxd_pi) ); + } + } + } + + return (score_inc); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalDistributionsTransform::computeHessian (Eigen::Matrix &hessian, + PointCloudSource &trans_cloud, Eigen::Matrix &) +{ + // Original Point and Transformed Point + PointSource x_pt, x_trans_pt; + // Original Point and Transformed Point (for math) + Eigen::Vector3d x, x_trans; + // Occupied Voxel + TargetGridLeafConstPtr cell; + // Inverse Covariance of Occupied Voxel + Eigen::Matrix3d c_inv; + + hessian.setZero (); + + // Precompute Angular Derivatives unessisary because only used after regular derivative calculation + + // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] + for (std::size_t idx = 0; idx < input_->points.size (); idx++) + { + x_trans_pt = trans_cloud.points[idx]; + + // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. + std::vector neighborhood; + std::vector distances; + target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances); + + for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it) + { + cell = *neighborhood_it; + + { + x_pt = input_->points[idx]; + x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z); + + x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); + + // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] + x_trans -= cell->getMean (); + // Uses precomputed covariance for speed. + c_inv = cell->getInverseCov (); + + // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] + computePointDerivatives (x); + // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] + updateHessian (hessian, x_trans, c_inv); + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalDistributionsTransform::updateHessian (Eigen::Matrix &hessian, Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv) +{ + Eigen::Vector3d cov_dxd_pi; + // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] + double e_x_cov_x = gauss_d2_ * std::exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2); + + // Error checking for invalid values. + if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) + return; + + // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + e_x_cov_x *= gauss_d1_; + + for (int i = 0; i < 6; i++) + { + // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] + cov_dxd_pi = c_inv * point_gradient_.col (i); + + for (Eigen::Index j = 0; j < hessian.cols (); j++) + { + // Update hessian, Equation 6.13 [Magnusson 2009] + hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) + + x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) + + point_gradient_.col (j).dot (cov_dxd_pi) ); + } + } + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::NormalDistributionsTransform::updateIntervalMT (double &a_l, double &f_l, double &g_l, + double &a_u, double &f_u, double &g_u, + double a_t, double f_t, double g_t) +{ + // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] + if (f_t > f_l) + { + a_u = a_t; + f_u = f_t; + g_u = g_t; + return (false); + } + // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] + if (g_t * (a_l - a_t) > 0) + { + a_l = a_t; + f_l = f_t; + g_l = g_t; + return (false); + } + // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] + if (g_t * (a_l - a_t) < 0) + { + a_u = a_l; + f_u = f_l; + g_u = g_l; + + a_l = a_t; + f_l = f_t; + g_l = g_t; + return (false); + } + // Interval Converged + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::NormalDistributionsTransform::trialValueSelectionMT (double a_l, double f_l, double g_l, + double a_u, double f_u, double g_u, + double a_t, double f_t, double g_t) +{ + // Case 1 in Trial Value Selection [More, Thuente 1994] + if (f_t > f_l) + { + // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; + double w = std::sqrt (z * z - g_t * g_l); + // Equation 2.4.56 [Sun, Yuan 2006] + double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); + + // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l + // Equation 2.4.2 [Sun, Yuan 2006] + double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); + + if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l)) + return (a_c); + return (0.5 * (a_q + a_c)); + } + // Case 2 in Trial Value Selection [More, Thuente 1994] + if (g_t * g_l < 0) + { + // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; + double w = std::sqrt (z * z - g_t * g_l); + // Equation 2.4.56 [Sun, Yuan 2006] + double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); + + // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t + // Equation 2.4.5 [Sun, Yuan 2006] + double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; + + if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t)) + return (a_c); + return (a_s); + } + // Case 3 in Trial Value Selection [More, Thuente 1994] + if (std::fabs (g_t) <= std::fabs (g_l)) + { + // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; + double w = std::sqrt (z * z - g_t * g_l); + double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); + + // Calculate the minimizer of the quadratic that interpolates g_l and g_t + // Equation 2.4.5 [Sun, Yuan 2006] + double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; + + double a_t_next; + + if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t)) + a_t_next = a_c; + else + a_t_next = a_s; + + if (a_t > a_l) + return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next)); + return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next)); + } + // Case 4 in Trial Value Selection [More, Thuente 1994] + // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t + // Equation 2.4.52 [Sun, Yuan 2006] + double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; + double w = std::sqrt (z * z - g_t * g_u); + // Equation 2.4.56 [Sun, Yuan 2006] + return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::NormalDistributionsTransform::computeStepLengthMT (const Eigen::Matrix &x, Eigen::Matrix &step_dir, double step_init, double step_max, + double step_min, double &score, Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, + PointCloudSource &trans_cloud) +{ + // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] + double phi_0 = -score; + // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] + double d_phi_0 = -(score_gradient.dot (step_dir)); + + Eigen::Matrix x_t; + + if (d_phi_0 >= 0) + { + // Not a decent direction + if (d_phi_0 == 0) + return 0; + // Reverse step direction and calculate optimal step. + d_phi_0 *= -1; + step_dir *= -1; + + } + + // The Search Algorithm for T(mu) [More, Thuente 1994] + + int max_step_iterations = 10; + int step_iterations = 0; + + // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994] + double mu = 1.e-4; + // Curvature condition constant, Equation 1.2 [More, Thuete 1994] + double nu = 0.9; + + // Initial endpoints of Interval I, + double a_l = 0, a_u = 0; + + // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994] + double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu); + double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); + + double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu); + double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); + + // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max + bool interval_converged = (step_max - step_min) > 0, open_interval = true; + + double a_t = step_init; + a_t = std::min (a_t, step_max); + a_t = std::max (a_t, step_min); + + x_t = x + step_dir * a_t; + + final_transformation_ = (Eigen::Translation(static_cast (x_t (0)), static_cast (x_t (1)), static_cast (x_t (2))) * + Eigen::AngleAxis (static_cast (x_t (3)), Eigen::Vector3f::UnitX ()) * + Eigen::AngleAxis (static_cast (x_t (4)), Eigen::Vector3f::UnitY ()) * + Eigen::AngleAxis (static_cast (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); + + // New transformed point cloud + transformPointCloud (*input_, trans_cloud, final_transformation_); + + // Updates score, gradient and hessian. Hessian calculation is unessisary but testing showed that most step calculations use the + // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time. + score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true); + + // Calculate phi(alpha_t) + double phi_t = -score; + // Calculate phi'(alpha_t) + double d_phi_t = -(score_gradient.dot (step_dir)); + + // Calculate psi(alpha_t) + double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); + // Calculate psi'(alpha_t) + double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); + + // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994] + while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) + { + // Use auxiliary function if interval I is not closed + if (open_interval) + { + a_t = trialValueSelectionMT (a_l, f_l, g_l, + a_u, f_u, g_u, + a_t, psi_t, d_psi_t); + } + else + { + a_t = trialValueSelectionMT (a_l, f_l, g_l, + a_u, f_u, g_u, + a_t, phi_t, d_phi_t); + } + + a_t = std::min (a_t, step_max); + a_t = std::max (a_t, step_min); + + x_t = x + step_dir * a_t; + + final_transformation_ = (Eigen::Translation (static_cast (x_t (0)), static_cast (x_t (1)), static_cast (x_t (2))) * + Eigen::AngleAxis (static_cast (x_t (3)), Eigen::Vector3f::UnitX ()) * + Eigen::AngleAxis (static_cast (x_t (4)), Eigen::Vector3f::UnitY ()) * + Eigen::AngleAxis (static_cast (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); + + // New transformed point cloud + // Done on final cloud to prevent wasted computation + transformPointCloud (*input_, trans_cloud, final_transformation_); + + // Updates score, gradient. Values stored to prevent wasted computation. + score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false); + + // Calculate phi(alpha_t+) + phi_t = -score; + // Calculate phi'(alpha_t+) + d_phi_t = -(score_gradient.dot (step_dir)); + + // Calculate psi(alpha_t+) + psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); + // Calculate psi'(alpha_t+) + d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); + + // Check if I is now a closed interval + if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) + { + open_interval = false; + + // Converts f_l and g_l from psi to phi + f_l += phi_0 - mu * d_phi_0 * a_l; + g_l += mu * d_phi_0; + + // Converts f_u and g_u from psi to phi + f_u += phi_0 - mu * d_phi_0 * a_u; + g_u += mu * d_phi_0; + } + + if (open_interval) + { + // Update interval end points using Updating Algorithm [More, Thuente 1994] + interval_converged = updateIntervalMT (a_l, f_l, g_l, + a_u, f_u, g_u, + a_t, psi_t, d_psi_t); + } + else + { + // Update interval end points using Modified Updating Algorithm [More, Thuente 1994] + interval_converged = updateIntervalMT (a_l, f_l, g_l, + a_u, f_u, g_u, + a_t, phi_t, d_phi_t); + } + + step_iterations++; + } + + // If inner loop was run then hessian needs to be calculated. + // Hessian is unnessisary for step length determination but gradients are required + // so derivative and transform data is stored for the next iteration. + if (step_iterations) + computeHessian (hessian, trans_cloud, x_t); + + return (a_t); +} + +#endif // PCL_REGISTRATION_NDT_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/ndt_2d.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/ndt_2d.hpp new file mode 100755 index 0000000..c34bdf3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/ndt_2d.hpp @@ -0,0 +1,496 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_NDT_2D_IMPL_H_ +#define PCL_NDT_2D_IMPL_H_ + +#include +#include + +#include +#include + +namespace pcl +{ + namespace ndt2d + { + /** \brief Class to store vector value and first and second derivatives + * (grad vector and hessian matrix), so they can be returned easily from + * functions + */ + template + struct ValueAndDerivatives + { + ValueAndDerivatives () : hessian (), grad (), value () {} + + Eigen::Matrix hessian; + Eigen::Matrix grad; + T value; + + static ValueAndDerivatives + Zero () + { + ValueAndDerivatives r; + r.hessian = Eigen::Matrix::Zero (); + r.grad = Eigen::Matrix::Zero (); + r.value = 0; + return r; + } + + ValueAndDerivatives& + operator+= (ValueAndDerivatives const& r) + { + hessian += r.hessian; + grad += r.grad; + value += r.value; + return *this; + } + }; + + /** \brief A normal distribution estimation class. + * + * First the indices of of the points from a point cloud that should be + * modelled by the distribution are added with addIdx (...). + * + * Then estimateParams (...) uses the stored point indices to estimate the + * parameters of a normal distribution, and discards the stored indices. + * + * Finally the distriubution, and its derivatives, may be evaluated at any + * point using test (...). + */ + template + class NormalDist + { + using PointCloud = pcl::PointCloud; + + public: + NormalDist () + : min_n_ (3), n_ (0) + { + } + + /** \brief Store a point index to use later for estimating distribution parameters. + * \param[in] i Point index to store + */ + void + addIdx (std::size_t i) + { + pt_indices_.push_back (i); + } + + /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared. + * \param[in] cloud Point cloud corresponding to indices passed to addIdx. + * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest. + */ + void + estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001) + { + Eigen::Vector2d sx = Eigen::Vector2d::Zero (); + Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero (); + + for (auto i = pt_indices_.cbegin (); i != pt_indices_.cend (); i++) + { + Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y); + sx += p; + sxx += p * p.transpose (); + } + + n_ = pt_indices_.size (); + + if (n_ >= min_n_) + { + mean_ = sx / static_cast (n_); + // Using maximum likelihood estimation as in the original paper + Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast (n_) + mean_ * mean_.transpose (); + + Eigen::SelfAdjointEigenSolver solver (covar); + if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1]) + { + PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]); + Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal (); + Eigen::Matrix2d q = solver.eigenvectors (); + // set minimum smallest eigenvalue: + l (0,0) = l (1,1) * min_covar_eigvalue_mult; + covar = q * l * q.transpose (); + } + covar_inv_ = covar.inverse (); + } + + pt_indices_.clear (); + } + + /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. + * \param[in] transformed_pt Location to evaluate at. + * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * estimateParams must have been called after at least three points were provided, or this will return zero. + * + */ + ValueAndDerivatives<3,double> + test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const + { + if (n_ < min_n_) + return ValueAndDerivatives<3,double>::Zero (); + + ValueAndDerivatives<3,double> r; + const double x = transformed_pt.x; + const double y = transformed_pt.y; + const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y); + const Eigen::Vector2d q = p_xy - mean_; + const Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_); + const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q)); + r.value = -exp_qt_cvi_q; + + Eigen::Matrix jacobian; + jacobian << + 1, 0, -(x * sin_theta + y*cos_theta), + 0, 1, x * cos_theta - y*sin_theta; + + for (std::size_t i = 0; i < 3; i++) + r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q; + + // second derivative only for i == j == 2: + const Eigen::Vector2d d2q_didj ( + y * sin_theta - x*cos_theta, + -(x * sin_theta + y*cos_theta) + ); + + for (std::size_t i = 0; i < 3; i++) + for (std::size_t j = 0; j < 3; j++) + r.hessian (i,j) = -exp_qt_cvi_q * ( + double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) + + (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) + + (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i)) + ); + + return r; + } + + protected: + const std::size_t min_n_; + + std::size_t n_; + std::vector pt_indices_; + Eigen::Vector2d mean_; + Eigen::Matrix2d covar_inv_; + }; + + /** \brief Build a set of normal distributions modelling a 2D point cloud, + * and provide the value and derivatives of the model at any point via the + * test (...) function. + */ + template + class NDTSingleGrid: public boost::noncopyable + { + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using NormalDist = pcl::ndt2d::NormalDist; + + public: + NDTSingleGrid (PointCloudConstPtr cloud, + const Eigen::Vector2f& about, + const Eigen::Vector2f& extent, + const Eigen::Vector2f& step) + : min_ (about - extent), max_ (min_ + 2*extent), step_ (step), + cells_ ((max_[0]-min_[0]) / step_[0], + (max_[1]-min_[1]) / step_[1]), + normal_distributions_ (cells_[0], cells_[1]) + { + // sort through all points, assigning them to distributions: + std::size_t used_points = 0; + for (std::size_t i = 0; i < cloud->size (); i++) + if (NormalDist* n = normalDistForPoint (cloud->at (i))) + { + n->addIdx (i); + used_points++; + } + + PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ()); + + // then bake the distributions such that they approximate the + // points (and throw away memory of the points) + for (int x = 0; x < cells_[0]; x++) + for (int y = 0; y < cells_[1]; y++) + normal_distributions_.coeffRef (x,y).estimateParams (*cloud); + } + + /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. + * \param[in] transformed_pt Location to evaluate at. + * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + */ + ValueAndDerivatives<3,double> + test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const + { + const NormalDist* n = normalDistForPoint (transformed_pt); + // index is in grid, return score from the normal distribution from + // the correct part of the grid: + if (n) + return n->test (transformed_pt, cos_theta, sin_theta); + return ValueAndDerivatives<3,double>::Zero (); + } + + protected: + /** \brief Return the normal distribution covering the location of point p + * \param[in] p a point + */ + NormalDist* + normalDistForPoint (PointT const& p) const + { + // this would be neater in 3d... + Eigen::Vector2f idxf; + for (std::size_t i = 0; i < 2; i++) + idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i]; + Eigen::Vector2i idxi = idxf.cast (); + for (std::size_t i = 0; i < 2; i++) + if (idxi[i] >= cells_[i] || idxi[i] < 0) + return nullptr; + // const cast to avoid duplicating this function in const and + // non-const variants... + return const_cast (&normal_distributions_.coeffRef (idxi[0], idxi[1])); + } + + Eigen::Vector2f min_; + Eigen::Vector2f max_; + Eigen::Vector2f step_; + Eigen::Vector2i cells_; + + Eigen::Matrix normal_distributions_; + }; + + /** \brief Build a Normal Distributions Transform of a 2D point cloud. This + * consists of the sum of four overlapping models of the original points + * with normal distributions. + * The value and derivatives of the model at any point can be evaluated + * with the test (...) function. + */ + template + class NDT2D: public boost::noncopyable + { + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using SingleGrid = NDTSingleGrid; + + public: + /** \brief + * \param[in] cloud the input point cloud + * \param[in] about Centre of the grid for normal distributions model + * \param[in] extent Extent of grid for normal distributions model + * \param[in] step Size of region that each normal distribution will model + */ + NDT2D (PointCloudConstPtr cloud, + const Eigen::Vector2f& about, + const Eigen::Vector2f& extent, + const Eigen::Vector2f& step) + { + Eigen::Vector2f dx (step[0]/2, 0); + Eigen::Vector2f dy (0, step[1]/2); + single_grids_[0].reset(new SingleGrid (cloud, about, extent, step)); + single_grids_[1].reset(new SingleGrid (cloud, about +dx, extent, step)); + single_grids_[2].reset(new SingleGrid (cloud, about +dy, extent, step)); + single_grids_[3].reset(new SingleGrid (cloud, about +dx+dy, extent, step)); + } + + /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. + * \param[in] transformed_pt Location to evaluate at. + * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + */ + ValueAndDerivatives<3,double> + test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const + { + ValueAndDerivatives<3,double> r = ValueAndDerivatives<3,double>::Zero (); + for (const auto &single_grid : single_grids_) + r += single_grid->test (transformed_pt, cos_theta, sin_theta); + return r; + } + + protected: + std::shared_ptr single_grids_[4]; + }; + + } // namespace ndt2d +} // namespace pcl + + +namespace Eigen +{ + /* This NumTraits specialisation is necessary because NormalDist is used as + * the element type of an Eigen Matrix. + */ + template struct NumTraits > + { + using Real = double; + using Literal = double; + static Real dummy_precision () { return 1.0; } + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 0, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 1, + MulCost = 1 + }; + }; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::NormalDistributionsTransform2D::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) +{ + PointCloudSource intm_cloud = output; + + nr_iterations_ = 0; + converged_ = false; + + if (guess != Eigen::Matrix4f::Identity ()) + { + transformation_ = guess; + transformPointCloud (output, intm_cloud, transformation_); + } + + // build Normal Distribution Transform of target cloud: + ndt2d::NDT2D target_ndt (target_, grid_centre_, grid_extent_, grid_step_); + + // can't seem to use .block<> () member function on transformation_ + // directly... gcc bug? + Eigen::Matrix4f& transformation = transformation_; + + + // work with x translation, y translation and z rotation: extending to 3D + // would be some tricky maths, but not impossible. + const Eigen::Matrix3f initial_rot (transformation.block<3,3> (0,0)); + const Eigen::Vector3f rot_x (initial_rot*Eigen::Vector3f::UnitX ()); + const double z_rotation = std::atan2 (rot_x[1], rot_x[0]); + + Eigen::Vector3d xytheta_transformation ( + transformation (0,3), + transformation (1,3), + z_rotation + ); + + while (!converged_) + { + const double cos_theta = std::cos (xytheta_transformation[2]); + const double sin_theta = std::sin (xytheta_transformation[2]); + previous_transformation_ = transformation; + + ndt2d::ValueAndDerivatives<3, double> score = ndt2d::ValueAndDerivatives<3, double>::Zero (); + for (std::size_t i = 0; i < intm_cloud.size (); i++) + score += target_ndt.test (intm_cloud[i], cos_theta, sin_theta); + + PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score %f (x=%f,y=%f,r=%f)\n", + float (score.value), xytheta_transformation[0], xytheta_transformation[1], xytheta_transformation[2] + ); + + if (score.value != 0) + { + // test for positive definiteness, and adjust to ensure it if necessary: + Eigen::EigenSolver solver; + solver.compute (score.hessian, false); + double min_eigenvalue = 0; + for (int i = 0; i <3; i++) + if (solver.eigenvalues ()[i].real () < min_eigenvalue) + min_eigenvalue = solver.eigenvalues ()[i].real (); + + // ensure "safe" positive definiteness: this is a detail missing + // from the original paper + if (min_eigenvalue < 0) + { + double lambda = 1.1 * min_eigenvalue - 1; + score.hessian += Eigen::Vector3d (-lambda, -lambda, -lambda).asDiagonal (); + solver.compute (score.hessian, false); + PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] adjust hessian: %f: new eigenvalues:%f %f %f\n", + float (lambda), + solver.eigenvalues ()[0].real (), + solver.eigenvalues ()[1].real (), + solver.eigenvalues ()[2].real () + ); + } + assert (solver.eigenvalues ()[0].real () >= 0 && + solver.eigenvalues ()[1].real () >= 0 && + solver.eigenvalues ()[2].real () >= 0); + + Eigen::Vector3d delta_transformation (-score.hessian.inverse () * score.grad); + Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation); + + xytheta_transformation = new_transformation; + + // update transformation matrix from x, y, theta: + transformation.block<3,3> (0,0).matrix () = Eigen::Matrix3f (Eigen::AngleAxisf (static_cast (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ())); + transformation.block<3,1> (0,3).matrix () = Eigen::Vector3f (static_cast (xytheta_transformation[0]), static_cast (xytheta_transformation[1]), 0.0f); + + //std::cout << "new transformation:\n" << transformation << std::endl; + } + else + { + PCL_ERROR ("[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n"); + break; + } + + transformPointCloud (output, intm_cloud, transformation); + + nr_iterations_++; + + if (update_visualizer_) + update_visualizer_ (output, *indices_, *target_, *indices_); + + //std::cout << "eps=" << std::abs ((transformation - previous_transformation_).sum ()) << std::endl; + + Eigen::Matrix4f transformation_delta = transformation.inverse() * previous_transformation_; + double cos_angle = 0.5 * (transformation_delta.coeff (0, 0) + transformation_delta.coeff (1, 1) + transformation_delta.coeff (2, 2) - 1); + double translation_sqr = transformation_delta.coeff (0, 3) * transformation_delta.coeff (0, 3) + + transformation_delta.coeff (1, 3) * transformation_delta.coeff (1, 3) + + transformation_delta.coeff (2, 3) * transformation_delta.coeff (2, 3); + + if (nr_iterations_ >= max_iterations_ || + ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) || + ((transformation_epsilon_ <= 0) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) || + ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0))) + { + converged_ = true; + } + } + final_transformation_ = transformation; + output = intm_cloud; +} + +#endif // PCL_NDT_2D_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/ppf_registration.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/ppf_registration.hpp new file mode 100755 index 0000000..bcc19f3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/ppf_registration.hpp @@ -0,0 +1,289 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + + +#ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_ +#define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_ + +#include +#include +#include + +#include +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PPFRegistration::setInputTarget (const PointCloudTargetConstPtr &cloud) +{ + Registration::setInputTarget (cloud); + + scene_search_tree_ = typename pcl::KdTreeFLANN::Ptr (new pcl::KdTreeFLANN); + scene_search_tree_->setInputCloud (target_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PPFRegistration::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) +{ + if (!search_method_) + { + PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n"); + return; + } + + if (guess != Eigen::Matrix4f::Identity ()) + { + PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform (guess) not implemented!\n"); + } + + PoseWithVotesList voted_poses; + std::vector > accumulator_array; + accumulator_array.resize (input_->points.size ()); + + std::size_t aux_size = static_cast (std::floor (2 * M_PI / search_method_->getAngleDiscretizationStep ())); + for (std::size_t i = 0; i < input_->points.size (); ++i) + { + std::vector aux (aux_size); + accumulator_array[i] = aux; + } + PCL_INFO ("Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ()); + + // Consider every -th point as the reference point => fix s_r + float f1, f2, f3, f4; + for (std::size_t scene_reference_index = 0; scene_reference_index < target_->points.size (); scene_reference_index += scene_reference_point_sampling_rate_) + { + Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (), + scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap (); + + float rotation_angle_sg = std::acos (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x_sg = (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis_sg = (parallel_to_x_sg)?(Eigen::Vector3f::UnitY ()):(scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_sg (rotation_angle_sg, rotation_axis_sg); + Eigen::Affine3f transform_sg (Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg); + + // For every other point in the scene => now have pair (s_r, s_i) fixed + std::vector indices; + std::vector distances; + scene_search_tree_->radiusSearch (target_->points[scene_reference_index], + search_method_->getModelDiameter () /2, + indices, + distances); + for(const std::size_t &scene_point_index : indices) +// for(std::size_t i = 0; i < target_->points.size (); ++i) + { + //size_t scene_point_index = i; + if (scene_reference_index != scene_point_index) + { + if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures (target_->points[scene_reference_index].getVector4fMap (), + target_->points[scene_reference_index].getNormalVector4fMap (), + target_->points[scene_point_index].getVector4fMap (), + target_->points[scene_point_index].getNormalVector4fMap (), + f1, f2, f3, f4)) + { + std::vector > nearest_indices; + search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices); + + // Compute alpha_s angle + Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap (); + + Eigen::Vector3f scene_point_transformed = transform_sg * scene_point; + float alpha_s = std::atan2 ( -scene_point_transformed(2), scene_point_transformed(1)); + if (std::sin (alpha_s) * scene_point_transformed(2) < 0.0f) + alpha_s *= (-1); + alpha_s *= (-1); + + // Go through point pairs in the model with the same discretized feature + for (const auto &nearest_index : nearest_indices) + { + std::size_t model_reference_index = nearest_index.first; + std::size_t model_point_index = nearest_index.second; + // Calculate angle alpha = alpha_m - alpha_s + float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s; + unsigned int alpha_discretized = static_cast (std::floor (alpha) + std::floor (M_PI / search_method_->getAngleDiscretizationStep ())); + accumulator_array[model_reference_index][alpha_discretized] ++; + } + } + else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %u and %u went wrong.\n", scene_reference_index, scene_point_index); + } + } + + std::size_t max_votes_i = 0, max_votes_j = 0; + unsigned int max_votes = 0; + + for (std::size_t i = 0; i < accumulator_array.size (); ++i) + for (std::size_t j = 0; j < accumulator_array.back ().size (); ++j) + { + if (accumulator_array[i][j] > max_votes) + { + max_votes = accumulator_array[i][j]; + max_votes_i = i; + max_votes_j = j; + } + // Reset accumulator_array for the next set of iterations with a new scene reference point + accumulator_array[i][j] = 0; + } + + Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (), + model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap (); + float rotation_angle_mg = std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())); + bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f); + Eigen::Vector3f rotation_axis_mg = (parallel_to_x_mg)?(Eigen::Vector3f::UnitY ()):(model_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized()); + Eigen::AngleAxisf rotation_mg (rotation_angle_mg, rotation_axis_mg); + Eigen::Affine3f transform_mg (Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg); + Eigen::Affine3f max_transform = + transform_sg.inverse () * + Eigen::AngleAxisf ((static_cast (max_votes_j) - std::floor (static_cast (M_PI) / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) * + transform_mg; + + voted_poses.push_back (PoseWithVotes (max_transform, max_votes)); + } + PCL_DEBUG ("Done with the Hough Transform ...\n"); + + // Cluster poses for filtering out outliers and obtaining more precise results + PoseWithVotesList results; + clusterPoses (voted_poses, results); + + pcl::transformPointCloud (*input_, output, results.front ().pose); + + transformation_ = final_transformation_ = results.front ().pose.matrix (); + converged_ = true; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PPFRegistration::clusterPoses (typename pcl::PPFRegistration::PoseWithVotesList &poses, + typename pcl::PPFRegistration::PoseWithVotesList &result) +{ + PCL_INFO ("Clustering poses ...\n"); + // Start off by sorting the poses by the number of votes + sort(poses.begin (), poses.end (), poseWithVotesCompareFunction); + + std::vector clusters; + std::vector > cluster_votes; + for (std::size_t poses_i = 0; poses_i < poses.size(); ++ poses_i) + { + bool found_cluster = false; + for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i) + { + if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose)) + { + found_cluster = true; + clusters[clusters_i].push_back (poses[poses_i]); + cluster_votes[clusters_i].second += poses[poses_i].votes; + break; + } + } + + if (!found_cluster) + { + // Create a new cluster with the current pose + PoseWithVotesList new_cluster; + new_cluster.push_back (poses[poses_i]); + clusters.push_back (new_cluster); + cluster_votes.push_back (std::pair (clusters.size () - 1, poses[poses_i].votes)); + } + } + + // Sort clusters by total number of votes + std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction); + // Compute pose average and put them in result vector + /// @todo some kind of threshold for determining whether a cluster has enough votes or not... + /// now just taking the first three clusters + result.clear (); + std::size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3; + for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i) + { + PCL_INFO ("Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ()); + Eigen::Vector3f translation_average (0.0, 0.0, 0.0); + Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0); + for (typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it) + { + translation_average += v_it->pose.translation (); + /// averaging rotations by just averaging the quaternions in 4D space - reference "On Averaging Rotations" by CLAUS GRAMKOW + rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs (); + } + + translation_average /= static_cast (clusters[cluster_votes[cluster_i].first].size ()); + rotation_average /= static_cast (clusters[cluster_votes[cluster_i].first].size ()); + + Eigen::Affine3f transform_average; + transform_average.translation ().matrix () = translation_average; + transform_average.linear ().matrix () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix (); + + result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second)); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PPFRegistration::posesWithinErrorBounds (Eigen::Affine3f &pose1, + Eigen::Affine3f &pose2) +{ + float position_diff = (pose1.translation () - pose2.translation ()).norm (); + Eigen::AngleAxisf rotation_diff_mat ((pose1.rotation ().inverse ().lazyProduct (pose2.rotation ()).eval())); + + float rotation_diff_angle = std::abs (rotation_diff_mat.angle ()); + + return (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PPFRegistration::poseWithVotesCompareFunction (const typename pcl::PPFRegistration::PoseWithVotes &a, + const typename pcl::PPFRegistration::PoseWithVotes &b ) +{ + return (a.votes > b.votes); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PPFRegistration::clusterVotesCompareFunction (const std::pair &a, + const std::pair &b) +{ + return (a.second > b.second); +} + +//#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class PCL_EXPORTS pcl::PPFRegistration; + +#endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/pyramid_feature_matching.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/pyramid_feature_matching.hpp new file mode 100755 index 0000000..5dfd6c4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/pyramid_feature_matching.hpp @@ -0,0 +1,302 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ +#define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::PyramidFeatureHistogram::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; + for (std::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 += static_cast (pyramid_a->hist_levels[0].hist[bin_i]); + else + match_count_level += static_cast (pyramid_b->hist_levels[0].hist[bin_i]); + } + + + float match_count = match_count_level; + for (std::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; + } + + float match_count_prev_level = match_count_level; + match_count_level = 0.0f; + for (std::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 += static_cast (pyramid_a->hist_levels[level_i].hist[bin_i]); + else + match_count_level += static_cast (pyramid_b->hist_levels[level_i].hist[bin_i]); + } + + float level_normalization_factor = powf (2.0f, static_cast (level_i)); + match_count += (match_count_level - match_count_prev_level) / level_normalization_factor; + } + + + // include self-similarity factors + float self_similarity_a = static_cast (pyramid_a->nr_features), + self_similarity_b = static_cast (pyramid_b->nr_features); + PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b); + match_count /= std::sqrt (self_similarity_a * self_similarity_b); + + return match_count; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::PyramidFeatureHistogram::PyramidFeatureHistogram () : + nr_dimensions (0), nr_levels (0), nr_features (0), + feature_representation_ (new DefaultPointRepresentation), + is_computed_ (false), + hist_levels () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PyramidFeatureHistogram::PyramidFeatureHistogramLevel::initializeHistogramLevel () +{ + std::size_t total_vector_size = 1; + for (std::vector::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it) + total_vector_size *= *dim_it; + + hist.resize (total_vector_size, 0); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::PyramidFeatureHistogram::initializeHistogram () +{ + // a few consistency checks before starting the computations + if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n"); + return false; + } + + if (dimension_range_input_.empty ()) + { + PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n"); + return false; + } + + if (dimension_range_target_.empty ()) + { + 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 >::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 = std::sqrt (D); + nr_levels = static_cast (std::ceil (std::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 (std::size_t level_i = 0; level_i < nr_levels; ++level_i) + { + std::vector bins_per_dimension (nr_dimensions); + std::vector bin_step (nr_dimensions); + for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) + { + bins_per_dimension[dim_i] = + static_cast (std::ceil ((dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (powf (2.0f, static_cast (level_i)) * std::sqrt (static_cast (nr_dimensions))))); + bin_step[dim_i] = powf (2.0f, static_cast (level_i)) * std::sqrt (static_cast (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 (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) + PCL_DEBUG ("%u ", bins_per_dimension[dim_i]); + PCL_DEBUG ("\n"); + } + + return true; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int& +pcl::PyramidFeatureHistogram::at (std::vector &access, + std::size_t &level) +{ + if (access.size () != nr_dimensions) + { + PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n"); + return hist_levels.front ().hist.front (); + } + if (level >= hist_levels.size ()) + { + PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n"); + return hist_levels.front ().hist.front (); + } + + std::size_t vector_position = 0; + std::size_t dim_accumulator = 1; + + for (int i = static_cast (access.size ()) - 1; i >= 0; --i) + { + vector_position += access[i] * dim_accumulator; + dim_accumulator *= hist_levels[level].bins_per_dimension[i]; + } + + return hist_levels[level].hist[vector_position]; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int& +pcl::PyramidFeatureHistogram::at (std::vector &feature, + std::size_t &level) +{ + if (feature.size () != nr_dimensions) + { + PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] The given feature vector does not match the feature dimensions of the pyramid histogram: %u vs %u\n", feature.size (), nr_dimensions); + return hist_levels.front ().hist.front (); + } + if (level >= hist_levels.size ()) + { + PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n"); + return hist_levels.front ().hist.front (); + } + + std::vector access; + for (std::size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) + access.push_back (static_cast (std::floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i]))); + + return at (access, level); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PyramidFeatureHistogram::convertFeatureToVector (const PointFeature &feature, + std::vector &feature_vector) +{ + // convert feature to vector representation + feature_vector.resize (feature_representation_->getNumberOfDimensions ()); + feature_representation_->vectorize (feature, feature_vector); + + // adapt the values from the input range to the target range + for (std::size_t i = 0; i < feature_vector.size (); ++i) + feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) * + (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PyramidFeatureHistogram::compute () +{ + if (!initializeHistogram ()) + return; + + for (std::size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i) + { + std::vector feature_vector; + convertFeatureToVector (input_->points[feature_i], feature_vector); + addFeature (feature_vector); + } + + is_computed_ = true; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PyramidFeatureHistogram::addFeature (std::vector &feature) +{ + for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) + at (feature, level_i) ++; +} + +#define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram; + +#endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/registration.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/registration.hpp new file mode 100755 index 0000000..88dfab7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/registration.hpp @@ -0,0 +1,204 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::Registration::setInputTarget (const PointCloudTargetConstPtr &cloud) +{ + if (cloud->points.empty ()) + { + PCL_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); + return; + } + target_ = cloud; + target_cloud_updated_ = true; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Registration::initCompute () +{ + if (!target_) + { + PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ()); + return (false); + } + + // Only update target kd-tree if a new target cloud was set + if (target_cloud_updated_ && !force_no_recompute_) + { + tree_->setInputCloud (target_); + target_cloud_updated_ = false; + } + + + // Update the correspondence estimation + if (correspondence_estimation_) + { + correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_); + correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_); + } + + // Note: we /cannot/ update the search method on all correspondence rejectors, because we know + // nothing about them. If they should be cached, they must be cached individually. + + return (PCLBase::initCompute ()); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::Registration::initComputeReciprocal () +{ + if (!input_) + { + PCL_ERROR ("[pcl::registration::%s::compute] No input source dataset was given!\n", getClassName ().c_str ()); + return (false); + } + + if (source_cloud_updated_ && !force_no_recompute_reciprocal_) + { + tree_reciprocal_->setInputCloud (input_); + source_cloud_updated_ = false; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline double +pcl::Registration::getFitnessScore ( + const std::vector &distances_a, + const std::vector &distances_b) +{ + unsigned int nr_elem = static_cast (std::min (distances_a.size (), distances_b.size ())); + Eigen::VectorXf map_a = Eigen::VectorXf::Map (&distances_a[0], nr_elem); + Eigen::VectorXf map_b = Eigen::VectorXf::Map (&distances_b[0], nr_elem); + return (static_cast ((map_a - map_b).sum ()) / static_cast (nr_elem)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline double +pcl::Registration::getFitnessScore (double max_range) +{ + + double fitness_score = 0.0; + + // Transform the input dataset using the final transformation + PointCloudSource input_transformed; + transformPointCloud (*input_, input_transformed, final_transformation_); + + std::vector nn_indices (1); + std::vector nn_dists (1); + + // For each point in the source dataset + int nr = 0; + for (std::size_t i = 0; i < input_transformed.points.size (); ++i) + { + // Find its nearest neighbor in the target + tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); + + // Deal with occlusions (incomplete targets) + if (nn_dists[0] <= max_range) + { + // Add to the fitness score + fitness_score += nn_dists[0]; + nr++; + } + } + + if (nr > 0) + return (fitness_score / nr); + return (std::numeric_limits::max ()); + +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::Registration::align (PointCloudSource &output) +{ + align (output, Matrix4::Identity ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::Registration::align (PointCloudSource &output, const Matrix4& guess) +{ + if (!initCompute ()) + 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 = static_cast (indices_->size ()); + output.height = 1; + } + else + { + output.width = static_cast (input_->width); + output.height = input_->height; + } + output.is_dense = input_->is_dense; + + // Copy the point data to output + for (std::size_t i = 0; i < indices_->size (); ++i) + output.points[i] = input_->points[(*indices_)[i]]; + + // Set the internal point representation of choice unless otherwise noted + if (point_representation_ && !force_no_recompute_) + tree_->setPointRepresentation (point_representation_); + + // Perform the actual transformation computation + converged_ = false; + final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity (); + + // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid + // transformation + for (std::size_t i = 0; i < indices_->size (); ++i) + output.points[i].data[3] = 1.0; + + computeTransformation (output, guess); + + deinitCompute (); +} + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/sample_consensus_prerejective.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/sample_consensus_prerejective.hpp new file mode 100755 index 0000000..2fa4dd6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/sample_consensus_prerejective.hpp @@ -0,0 +1,336 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ +#define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusPrerejective::setSourceFeatures (const FeatureCloudConstPtr &features) +{ + if (features == nullptr || features->empty ()) + { + PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); + return; + } + input_features_ = features; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusPrerejective::setTargetFeatures (const FeatureCloudConstPtr &features) +{ + if (features == nullptr || features->empty ()) + { + PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); + return; + } + target_features_ = features; + feature_tree_->setInputCloud (target_features_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusPrerejective::selectSamples ( + const PointCloudSource &cloud, int nr_samples, std::vector &sample_indices) +{ + if (nr_samples > static_cast (cloud.points.size ())) + { + PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); + PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n", + nr_samples, cloud.points.size ()); + return; + } + + sample_indices.resize (nr_samples); + int temp_sample; + + // Draw random samples until n samples is reached + for (int i = 0; i < nr_samples; i++) + { + // Select a random number + sample_indices[i] = getRandomIndex (static_cast (cloud.points.size ()) - i); + + // Run trough list of numbers, starting at the lowest, to avoid duplicates + for (int j = 0; j < i; j++) + { + // Move value up if it is higher than previous selections to ensure true randomness + if (sample_indices[i] >= sample_indices[j]) + { + sample_indices[i]++; + } + else + { + // The new number is lower, place it at the correct point and break for a sorted list + temp_sample = sample_indices[i]; + for (int k = i; k > j; k--) + sample_indices[k] = sample_indices[k - 1]; + + sample_indices[j] = temp_sample; + break; + } + } + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusPrerejective::findSimilarFeatures ( + const std::vector &sample_indices, + std::vector >& similar_features, + std::vector &corresponding_indices) +{ + // Allocate results + corresponding_indices.resize (sample_indices.size ()); + std::vector nn_distances (k_correspondences_); + + // Loop over the sampled features + for (std::size_t i = 0; i < sample_indices.size (); ++i) + { + // Current feature index + const int idx = sample_indices[i]; + + // Find the k nearest feature neighbors to the sampled input feature if they are not in the cache already + if (similar_features[idx].empty ()) + feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances); + + // Select one at random and add it to corresponding_indices + if (k_correspondences_ == 1) + corresponding_indices[i] = similar_features[idx][0]; + else + corresponding_indices[i] = similar_features[idx][getRandomIndex (k_correspondences_)]; + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusPrerejective::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) +{ + // Some sanity checks first + 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 (input_->size () != input_features_->size ()) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n", + input_->size (), input_features_->size ()); + return; + } + + if (target_->size () != target_features_->size ()) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n", + target_->size (), target_features_->size ()); + return; + } + + if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("Illegal inlier fraction %f, must be in [0,1]!\n", + inlier_fraction_); + return; + } + + const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold (); + if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("Illegal prerejection similarity threshold %f, must be in [0,1[!\n", + similarity_threshold); + return; + } + + if (k_correspondences_ <= 0) + { + PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); + PCL_ERROR ("Illegal correspondence randomness %d, must be > 0!\n", + k_correspondences_); + return; + } + + // Initialize prerejector (similarity threshold already set to default value in constructor) + correspondence_rejector_poly_->setInputSource (input_); + correspondence_rejector_poly_->setInputTarget (target_); + correspondence_rejector_poly_->setCardinality (nr_samples_); + int num_rejections = 0; // For debugging + + // Initialize results + final_transformation_ = guess; + inliers_.clear (); + float lowest_error = std::numeric_limits::max (); + converged_ = false; + + // Temporaries + std::vector inliers; + float inlier_fraction; + float error; + + // If guess is not the Identity matrix we check it + if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) + { + getFitness (inliers, error); + inlier_fraction = static_cast (inliers.size ()) / static_cast (input_->size ()); + + if (inlier_fraction >= inlier_fraction_ && error < lowest_error) + { + inliers_ = inliers; + lowest_error = error; + converged_ = true; + } + } + + // Feature correspondence cache + std::vector > similar_features (input_->size ()); + + // Start + for (int i = 0; i < max_iterations_; ++i) + { + // Temporary containers + std::vector sample_indices; + std::vector corresponding_indices; + + // Draw nr_samples_ random samples + selectSamples (*input_, nr_samples_, sample_indices); + + // Find corresponding features in the target cloud + findSimilarFeatures (sample_indices, similar_features, corresponding_indices); + + // Apply prerejection + if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)) + { + ++num_rejections; + continue; + } + + // Estimate the transform from the correspondences, write to transformation_ + transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_); + + // Take a backup of previous result + const Matrix4 final_transformation_prev = final_transformation_; + + // Set final result to current transformation + final_transformation_ = transformation_; + + // Transform the input and compute the error (uses input_ and final_transformation_) + getFitness (inliers, error); + + // Restore previous result + final_transformation_ = final_transformation_prev; + + // If the new fit is better, update results + inlier_fraction = static_cast (inliers.size ()) / static_cast (input_->size ()); + + // Update result if pose hypothesis is better + if (inlier_fraction >= inlier_fraction_ && error < lowest_error) + { + inliers_ = inliers; + lowest_error = error; + converged_ = true; + final_transformation_ = transformation_; + } + } + + // Apply the final transformation + if (converged_) + transformPointCloud (*input_, output, final_transformation_); + + // Debug output + PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n", + getClassName ().c_str (), num_rejections, max_iterations_); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusPrerejective::getFitness (std::vector& inliers, float& fitness_score) +{ + // Initialize variables + inliers.clear (); + inliers.reserve (input_->size ()); + fitness_score = 0.0f; + + // Use squared distance for comparison with NN search results + const float max_range = corr_dist_threshold_ * corr_dist_threshold_; + + // Transform the input dataset using the final transformation + PointCloudSource input_transformed; + input_transformed.resize (input_->size ()); + transformPointCloud (*input_, input_transformed, final_transformation_); + + // For each point in the source dataset + for (std::size_t i = 0; i < input_transformed.points.size (); ++i) + { + // Find its nearest neighbor in the target + std::vector nn_indices (1); + std::vector nn_dists (1); + tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); + + // Check if point is an inlier + if (nn_dists[0] < max_range) + { + // Update inliers + inliers.push_back (static_cast (i)); + + // Update fitness score + fitness_score += nn_dists[0]; + } + } + + // Calculate MSE + if (!inliers.empty ()) + fitness_score /= static_cast (inliers.size ()); + else + fitness_score = std::numeric_limits::max (); +} + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_2D.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_2D.hpp new file mode 100755 index 0000000..36b9d67 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_2D.hpp @@ -0,0 +1,165 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_ + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = cloud_src.points.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != cloud_tgt.points.size ()) + { + PCL_ERROR ("[pcl::Transformation2D::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != indices_tgt.size ()) + { + PCL_ERROR ("[pcl::TransformationEstimation2D::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( + ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const +{ + source_it.reset (); target_it.reset (); + + Eigen::Matrix centroid_src, centroid_tgt; + // Estimate the centroids of source, target + compute3DCentroid (source_it, centroid_src); + compute3DCentroid (target_it, centroid_tgt); + source_it.reset (); target_it.reset (); + + // ignore z component + centroid_src[2] = 0.0f; + centroid_tgt[2] = 0.0f; + // Subtract the centroids from source, target + Eigen::Matrix cloud_src_demean, cloud_tgt_demean; + demeanPointCloud (source_it, centroid_src, cloud_src_demean); + demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); + + getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimation2D::getTransformationFromCorrelation ( + const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, + Matrix4 &transformation_matrix) const +{ + transformation_matrix.setIdentity (); + + // Assemble the correlation matrix H = source * target' + Eigen::Matrix H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3); + + float angle = std::atan2 ((H (0, 1) - H (1, 0)), (H(0, 0) + H (1, 1))); + + Eigen::Matrix R (Eigen::Matrix::Identity ()); + R (0, 0) = R (1, 1) = std::cos (angle); + R (0, 1) = -std::sin (angle); + R (1, 0) = std::sin (angle); + + // Return the correct transformation + transformation_matrix.topLeftCorner (3, 3).matrix () = R; + const Eigen::Matrix Rc (R * centroid_src.head (3).matrix ()); + transformation_matrix.block (0, 3, 3, 1).matrix () = centroid_tgt.head (3) - Rc; +} + +#endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_3point.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_3point.hpp new file mode 100755 index 0000000..614b6f2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_3point.hpp @@ -0,0 +1,181 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#ifndef PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_ +#define PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (cloud_src.points.size () != 3 || cloud_tgt.points.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of points in source (%lu) and target (%lu) must be 3!\n", + cloud_src.points.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != 3 || cloud_tgt.points.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of indices in source (%lu) and points in target (%lu) must be 3!\n", + indices_src.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != 3 || indices_tgt.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of indices in source (%lu) and target (%lu) must be 3!\n", + indices_src.size (), indices_tgt.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + if (correspondences.size () != 3) + { + PCL_ERROR ("[pcl::TransformationEstimation3Point::estimateRigidTransformation] Number of correspondences (%lu) must be 3!\n", + correspondences.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimation3Point::estimateRigidTransformation ( + ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const +{ + transformation_matrix.setIdentity (); + source_it.reset (); + target_it.reset (); + + Eigen::Matrix source_mean, target_mean; + pcl::compute3DCentroid (source_it, source_mean); + pcl::compute3DCentroid (target_it, target_mean); + + source_it.reset (); + target_it.reset (); + + Eigen::Matrix source_demean, target_demean; + pcl::demeanPointCloud (source_it, source_mean, source_demean, 3); + pcl::demeanPointCloud (target_it, target_mean, target_demean, 3); + + source_it.reset (); + target_it.reset (); + + Eigen::Matrix s1 = source_demean.col (1).head (3) - source_demean.col (0).head (3); + s1.normalize (); + + Eigen::Matrix s2 = source_demean.col (2).head (3) - source_demean.col (0).head (3); + s2 -= s2.dot (s1) * s1; + s2.normalize (); + + Eigen::Matrix source_rot; + source_rot.col (0) = s1; + source_rot.col (1) = s2; + source_rot.col (2) = s1.cross (s2); + + Eigen::Matrix t1 = target_demean.col (1).head (3) - target_demean.col (0).head (3); + t1.normalize (); + + Eigen::Matrix t2 = target_demean.col (2).head (3) - target_demean.col (0).head (3); + t2 -= t2.dot (t1) * t1; + t2.normalize (); + + Eigen::Matrix target_rot; + target_rot.col (0) = t1; + target_rot.col (1) = t2; + target_rot.col (2) = t1.cross (t2); + + //Eigen::Matrix R = source_rot * target_rot.transpose (); + Eigen::Matrix R = target_rot * source_rot.transpose (); + transformation_matrix.topLeftCorner (3, 3) = R; + //transformation_matrix.block (0, 3, 3, 1) = source_mean.head (3) - R * target_mean.head (3); + transformation_matrix.block (0, 3, 3, 1) = target_mean.head (3) - R * source_mean.head (3); +} + +//#define PCL_INSTANTIATE_TransformationEstimation3Point(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimation3Point; + +#endif // PCL_REGISTRATION_IMPL_TRANSFORMATION_ESTIMATION_3POINT_H_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp new file mode 100755 index 0000000..27f1347 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp @@ -0,0 +1,207 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = cloud_src.points.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationDualQuaternion::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != cloud_tgt.points.size ()) + { + PCL_ERROR ("[pcl::TransformationDQ::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != indices_tgt.size ()) + { + PCL_ERROR ("[pcl::TransformationEstimationDualQuaternion::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( + ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const +{ + const int npts = static_cast (source_it.size ()); + + transformation_matrix.setIdentity (); + + // dual quaternion optimization + Eigen::Matrix C1 = Eigen::Matrix::Zero (); + Eigen::Matrix C2 = Eigen::Matrix::Zero (); + double* c1 = C1.data (); + double* c2 = C2.data (); + + for (int i = 0; i < npts; ++i) + { + const PointSource& a = *source_it; + const PointTarget& b = *target_it; + const double axbx = a.x * b.x; + const double ayby = a.y * b.y; + const double azbz = a.z * b.z; + const double axby = a.x * b.y; + const double aybx = a.y * b.x; + const double axbz = a.x * b.z; + const double azbx = a.z * b.x; + const double aybz = a.y * b.z; + const double azby = a.z * b.y; + c1[0] += axbx - azbz - ayby; + c1[5] += ayby - azbz - axbx; + c1[10] += azbz - axbx - ayby; + c1[15] += axbx + ayby + azbz; + c1[1] += axby + aybx; + c1[2] += axbz + azbx; + c1[3] += aybz - azby; + c1[6] += azby + aybz; + c1[7] += azbx - axbz; + c1[11] += axby - aybx; + + c2[1] += a.z + b.z; + c2[2] -= a.y + b.y; + c2[3] += a.x - b.x; + c2[6] += a.x + b.x; + c2[7] += a.y - b.y; + c2[11] += a.z - b.z; + source_it++; + target_it++; + } + + c1[4] = c1[1]; + c1[8] = c1[2]; + c1[9] = c1[6]; + c1[12] = c1[3]; + c1[13] = c1[7]; + c1[14] = c1[11]; + c2[4] = -c2[1]; + c2[8] = -c2[2]; + c2[12] = -c2[3]; + c2[9] = -c2[6]; + c2[13] = -c2[7]; + c2[14] = -c2[11]; + + C1 *= -2.0; + C2 *= 2.0; + + const Eigen::Matrix A = (0.25 / double (npts)) * C2.transpose () * C2 - C1; + + const Eigen::EigenSolver > es (A); + + ptrdiff_t i; + es.eigenvalues ().real ().maxCoeff (&i); + const Eigen::Matrix qmat = es.eigenvectors ().col (i).real (); + const Eigen::Matrix smat = - (0.5 / double (npts)) * C2 * qmat; + + const Eigen::Quaternion q (qmat (3), qmat (0), qmat (1), qmat (2)); + const Eigen::Quaternion s (smat (3), smat (0), smat (1), smat (2)); + + const Eigen::Quaternion t = s * q.conjugate (); + + const Eigen::Matrix R (q.toRotationMatrix ()); + + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + transformation_matrix (i, j) = R (i, j); + + transformation_matrix (0, 3) = - t.x (); + transformation_matrix (1, 3) = - t.y (); + transformation_matrix (2, 3) = - t.z (); +} + +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_lm.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_lm.hpp new file mode 100755 index 0000000..280dccf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_lm.hpp @@ -0,0 +1,273 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ + +#include +#include +#include +#include + + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::registration::TransformationEstimationLM::TransformationEstimationLM () + : tmp_src_ () + , tmp_tgt_ () + , tmp_idx_src_ () + , tmp_idx_tgt_ () + , warp_point_ (new WarpPointRigid6D) +{ +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationLM::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + + // is the source dataset + if (cloud_src.points.size () != cloud_tgt.points.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "); + PCL_ERROR ("Number or points in source (%lu) differs than target (%lu)!\n", + cloud_src.points.size (), cloud_tgt.points.size ()); + return; + } + if (cloud_src.points.size () < 4) // need at least 4 samples + { + PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] "); + PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!\n", + cloud_src.points.size ()); + return; + } + + int n_unknowns = warp_point_->getDimension (); + VectorX x (n_unknowns); + x.setZero (); + + // Set temporary pointers + tmp_src_ = &cloud_src; + tmp_tgt_ = &cloud_tgt; + + OptimizationFunctor functor (static_cast (cloud_src.points.size ()), this); + Eigen::NumericalDiff num_diff (functor); + //Eigen::LevenbergMarquardt, double> lm (num_diff); + Eigen::LevenbergMarquardt, MatScalar> lm (num_diff); + int info = lm.minimize (x); + + // Compute the norm of the residuals + PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]"); + PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); + PCL_DEBUG ("Final solution: [%f", x[0]); + for (int i = 1; i < n_unknowns; ++i) + PCL_DEBUG (" %f", x[i]); + PCL_DEBUG ("]\n"); + + // Return the correct transformation + warp_point_->setParam (x); + transformation_matrix = warp_point_->getTransform (); + + tmp_src_ = nullptr; + tmp_tgt_ = nullptr; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationLM::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != cloud_tgt.points.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + // is the source dataset + transformation_matrix.setIdentity (); + + const int nr_correspondences = static_cast (cloud_tgt.points.size ()); + std::vector indices_tgt; + indices_tgt.resize(nr_correspondences); + for (int i = 0; i < nr_correspondences; ++i) + indices_tgt[i] = i; + + estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationLM::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != indices_tgt.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + if (indices_src.size () < 4) // need at least 4 samples + { + PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] "); + PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!", + indices_src.size ()); + return; + } + + int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space + VectorX x (n_unknowns); + x.setConstant (n_unknowns, 0); + + // Set temporary pointers + tmp_src_ = &cloud_src; + tmp_tgt_ = &cloud_tgt; + tmp_idx_src_ = &indices_src; + tmp_idx_tgt_ = &indices_tgt; + + OptimizationFunctorWithIndices functor (static_cast (indices_src.size ()), this); + Eigen::NumericalDiff num_diff (functor); + //Eigen::LevenbergMarquardt > lm (num_diff); + Eigen::LevenbergMarquardt, MatScalar> lm (num_diff); + int info = lm.minimize (x); + + // Compute the norm of the residuals + PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); + PCL_DEBUG ("Final solution: [%f", x[0]); + for (int i = 1; i < n_unknowns; ++i) + PCL_DEBUG (" %f", x[i]); + PCL_DEBUG ("]\n"); + + // Return the correct transformation + warp_point_->setParam (x); + transformation_matrix = warp_point_->getTransform (); + + tmp_src_ = nullptr; + tmp_tgt_ = nullptr; + tmp_idx_src_ = tmp_idx_tgt_ = nullptr; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationLM::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + const int nr_correspondences = static_cast (correspondences.size ()); + std::vector indices_src (nr_correspondences); + std::vector indices_tgt (nr_correspondences); + for (int i = 0; i < nr_correspondences; ++i) + { + indices_src[i] = correspondences[i].index_query; + indices_tgt[i] = correspondences[i].index_match; + } + + estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::TransformationEstimationLM::OptimizationFunctor::operator () ( + const VectorX &x, VectorX &fvec) const +{ + const PointCloud & src_points = *estimator_->tmp_src_; + const PointCloud & tgt_points = *estimator_->tmp_tgt_; + + // Initialize the warp function with the given parameters + estimator_->warp_point_->setParam (x); + + // Transform each source point and compute its distance to the corresponding target point + for (int i = 0; i < values (); ++i) + { + const PointSource & p_src = src_points.points[i]; + const PointTarget & p_tgt = tgt_points.points[i]; + + // Transform the source point based on the current warp parameters + Vector4 p_src_warped; + estimator_->warp_point_->warpPoint (p_src, p_src_warped); + + // Estimate the distance (cost function) + fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt); + } + return (0); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::TransformationEstimationLM::OptimizationFunctorWithIndices::operator() ( + const VectorX &x, VectorX &fvec) const +{ + const PointCloud & src_points = *estimator_->tmp_src_; + const PointCloud & tgt_points = *estimator_->tmp_tgt_; + const std::vector & src_indices = *estimator_->tmp_idx_src_; + const std::vector & tgt_indices = *estimator_->tmp_idx_tgt_; + + // Initialize the warp function with the given parameters + estimator_->warp_point_->setParam (x); + + // Transform each source point and compute its distance to the corresponding target point + for (int i = 0; i < values (); ++i) + { + const PointSource & p_src = src_points.points[src_indices[i]]; + const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]]; + + // Transform the source point based on the current warp parameters + Vector4 p_src_warped; + estimator_->warp_point_->warpPoint (p_src, p_src_warped); + + // Estimate the distance (cost function) + fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt); + } + return (0); +} + +//#define PCL_INSTANTIATE_TransformationEstimationLM(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationLM; + +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp new file mode 100755 index 0000000..03316e3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_lls.hpp @@ -0,0 +1,250 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = cloud_src.points.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = indices_src.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = indices_src.size (); + if (indices_tgt.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLS:: +constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, + const double & tx, const double & ty, const double & tz, + Matrix4 &transformation_matrix) const +{ + // Construct the transformation matrix from rotation and translation + transformation_matrix = Eigen::Matrix::Zero (); + transformation_matrix (0, 0) = static_cast ( std::cos (gamma) * std::cos (beta)); + transformation_matrix (0, 1) = static_cast (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha)); + transformation_matrix (0, 2) = static_cast ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha)); + transformation_matrix (1, 0) = static_cast ( sin (gamma) * std::cos (beta)); + transformation_matrix (1, 1) = static_cast ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha)); + transformation_matrix (1, 2) = static_cast (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha)); + transformation_matrix (2, 0) = static_cast (-sin (beta)); + transformation_matrix (2, 1) = static_cast ( std::cos (beta) * sin (alpha)); + transformation_matrix (2, 2) = static_cast ( std::cos (beta) * std::cos (alpha)); + + transformation_matrix (0, 3) = static_cast (tx); + transformation_matrix (1, 3) = static_cast (ty); + transformation_matrix (2, 3) = static_cast (tz); + transformation_matrix (3, 3) = static_cast (1); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLS:: +estimateRigidTransformation (ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const +{ + using Vector6d = Eigen::Matrix; + using Matrix6d = Eigen::Matrix; + + Matrix6d ATA; + Vector6d ATb; + ATA.setZero (); + ATb.setZero (); + + // Approximate as a linear least squares problem + while (source_it.isValid () && target_it.isValid ()) + { + if (!std::isfinite (source_it->x) || + !std::isfinite (source_it->y) || + !std::isfinite (source_it->z) || + !std::isfinite (target_it->x) || + !std::isfinite (target_it->y) || + !std::isfinite (target_it->z) || + !std::isfinite (target_it->normal_x) || + !std::isfinite (target_it->normal_y) || + !std::isfinite (target_it->normal_z)) + { + ++target_it; + ++source_it; + continue; + } + + const float & sx = source_it->x; + const float & sy = source_it->y; + const float & sz = source_it->z; + const float & dx = target_it->x; + const float & dy = target_it->y; + const float & dz = target_it->z; + const float & nx = target_it->normal[0]; + const float & ny = target_it->normal[1]; + const float & nz = target_it->normal[2]; + + double a = nz*sy - ny*sz; + double b = nx*sz - nz*sx; + double c = ny*sx - nx*sy; + + // 0 1 2 3 4 5 + // 6 7 8 9 10 11 + // 12 13 14 15 16 17 + // 18 19 20 21 22 23 + // 24 25 26 27 28 29 + // 30 31 32 33 34 35 + + ATA.coeffRef (0) += a * a; + ATA.coeffRef (1) += a * b; + ATA.coeffRef (2) += a * c; + ATA.coeffRef (3) += a * nx; + ATA.coeffRef (4) += a * ny; + ATA.coeffRef (5) += a * nz; + ATA.coeffRef (7) += b * b; + ATA.coeffRef (8) += b * c; + ATA.coeffRef (9) += b * nx; + ATA.coeffRef (10) += b * ny; + ATA.coeffRef (11) += b * nz; + ATA.coeffRef (14) += c * c; + ATA.coeffRef (15) += c * nx; + ATA.coeffRef (16) += c * ny; + ATA.coeffRef (17) += c * nz; + ATA.coeffRef (21) += nx * nx; + ATA.coeffRef (22) += nx * ny; + ATA.coeffRef (23) += nx * nz; + ATA.coeffRef (28) += ny * ny; + ATA.coeffRef (29) += ny * nz; + ATA.coeffRef (35) += nz * nz; + + double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; + ATb.coeffRef (0) += a * d; + ATb.coeffRef (1) += b * d; + ATb.coeffRef (2) += c * d; + ATb.coeffRef (3) += nx * d; + ATb.coeffRef (4) += ny * d; + ATb.coeffRef (5) += nz * d; + + ++target_it; + ++source_it; + } + ATA.coeffRef (6) = ATA.coeff (1); + ATA.coeffRef (12) = ATA.coeff (2); + ATA.coeffRef (13) = ATA.coeff (8); + ATA.coeffRef (18) = ATA.coeff (3); + ATA.coeffRef (19) = ATA.coeff (9); + ATA.coeffRef (20) = ATA.coeff (15); + ATA.coeffRef (24) = ATA.coeff (4); + ATA.coeffRef (25) = ATA.coeff (10); + ATA.coeffRef (26) = ATA.coeff (16); + ATA.coeffRef (27) = ATA.coeff (22); + ATA.coeffRef (30) = ATA.coeff (5); + ATA.coeffRef (31) = ATA.coeff (11); + ATA.coeffRef (32) = ATA.coeff (17); + ATA.coeffRef (33) = ATA.coeff (23); + ATA.coeffRef (34) = ATA.coeff (29); + + // Solve A*x = b + Vector6d x = static_cast (ATA.inverse () * ATb); + + // Construct the transformation matrix from x + constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); +} +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp new file mode 100755 index 0000000..dfbd161 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp @@ -0,0 +1,280 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = cloud_src.points.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); + return; + } + + if (weights_.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n"); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + typename std::vector::const_iterator weights_it = weights_.begin (); + estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = indices_src.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + if (weights_.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n"); + return; + } + + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + typename std::vector::const_iterator weights_it = weights_.begin (); + estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = indices_src.size (); + if (indices_tgt.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + if (weights_.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n"); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + typename std::vector::const_iterator weights_it = weights_.begin (); + estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + std::vector weights (correspondences.size ()); + for (std::size_t i = 0; i < correspondences.size (); ++i) + weights[i] = correspondences[i].weight; + typename std::vector::const_iterator weights_it = weights.begin (); + estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, + const double & tx, const double & ty, const double & tz, + Matrix4 &transformation_matrix) const +{ + // Construct the transformation matrix from rotation and translation + transformation_matrix = Eigen::Matrix::Zero (); + transformation_matrix (0, 0) = static_cast ( std::cos (gamma) * std::cos (beta)); + transformation_matrix (0, 1) = static_cast (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha)); + transformation_matrix (0, 2) = static_cast ( sin (gamma) * sin (alpha) + std::cos (gamma) * sin (beta) * std::cos (alpha)); + transformation_matrix (1, 0) = static_cast ( sin (gamma) * std::cos (beta)); + transformation_matrix (1, 1) = static_cast ( std::cos (gamma) * std::cos (alpha) + sin (gamma) * sin (beta) * sin (alpha)); + transformation_matrix (1, 2) = static_cast (-std::cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * std::cos (alpha)); + transformation_matrix (2, 0) = static_cast (-sin (beta)); + transformation_matrix (2, 1) = static_cast ( std::cos (beta) * sin (alpha)); + transformation_matrix (2, 2) = static_cast ( std::cos (beta) * std::cos (alpha)); + + transformation_matrix (0, 3) = static_cast (tx); + transformation_matrix (1, 3) = static_cast (ty); + transformation_matrix (2, 3) = static_cast (tz); + transformation_matrix (3, 3) = static_cast (1); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + typename std::vector::const_iterator& weights_it, + Matrix4 &transformation_matrix) const +{ + using Vector6d = Eigen::Matrix; + using Matrix6d = Eigen::Matrix; + + Matrix6d ATA; + Vector6d ATb; + ATA.setZero (); + ATb.setZero (); + + while (source_it.isValid () && target_it.isValid ()) + { + if (!std::isfinite (source_it->x) || + !std::isfinite (source_it->y) || + !std::isfinite (source_it->z) || + !std::isfinite (target_it->x) || + !std::isfinite (target_it->y) || + !std::isfinite (target_it->z) || + !std::isfinite (target_it->normal_x) || + !std::isfinite (target_it->normal_y) || + !std::isfinite (target_it->normal_z)) + { + ++ source_it; + ++ target_it; + ++ weights_it; + continue; + } + + const float & sx = source_it->x; + const float & sy = source_it->y; + const float & sz = source_it->z; + const float & dx = target_it->x; + const float & dy = target_it->y; + const float & dz = target_it->z; + const float & nx = target_it->normal[0] * (*weights_it); + const float & ny = target_it->normal[1] * (*weights_it); + const float & nz = target_it->normal[2] * (*weights_it); + + double a = nz*sy - ny*sz; + double b = nx*sz - nz*sx; + double c = ny*sx - nx*sy; + + // 0 1 2 3 4 5 + // 6 7 8 9 10 11 + // 12 13 14 15 16 17 + // 18 19 20 21 22 23 + // 24 25 26 27 28 29 + // 30 31 32 33 34 35 + + ATA.coeffRef (0) += a * a; + ATA.coeffRef (1) += a * b; + ATA.coeffRef (2) += a * c; + ATA.coeffRef (3) += a * nx; + ATA.coeffRef (4) += a * ny; + ATA.coeffRef (5) += a * nz; + ATA.coeffRef (7) += b * b; + ATA.coeffRef (8) += b * c; + ATA.coeffRef (9) += b * nx; + ATA.coeffRef (10) += b * ny; + ATA.coeffRef (11) += b * nz; + ATA.coeffRef (14) += c * c; + ATA.coeffRef (15) += c * nx; + ATA.coeffRef (16) += c * ny; + ATA.coeffRef (17) += c * nz; + ATA.coeffRef (21) += nx * nx; + ATA.coeffRef (22) += nx * ny; + ATA.coeffRef (23) += nx * nz; + ATA.coeffRef (28) += ny * ny; + ATA.coeffRef (29) += ny * nz; + ATA.coeffRef (35) += nz * nz; + + double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; + ATb.coeffRef (0) += a * d; + ATb.coeffRef (1) += b * d; + ATb.coeffRef (2) += c * d; + ATb.coeffRef (3) += nx * d; + ATb.coeffRef (4) += ny * d; + ATb.coeffRef (5) += nz * d; + + ++ source_it; + ++ target_it; + ++ weights_it; + } + + ATA.coeffRef (6) = ATA.coeff (1); + ATA.coeffRef (12) = ATA.coeff (2); + ATA.coeffRef (13) = ATA.coeff (8); + ATA.coeffRef (18) = ATA.coeff (3); + ATA.coeffRef (19) = ATA.coeff (9); + ATA.coeffRef (20) = ATA.coeff (15); + ATA.coeffRef (24) = ATA.coeff (4); + ATA.coeffRef (25) = ATA.coeff (10); + ATA.coeffRef (26) = ATA.coeff (16); + ATA.coeffRef (27) = ATA.coeff (22); + ATA.coeffRef (30) = ATA.coeff (5); + ATA.coeffRef (31) = ATA.coeff (11); + ATA.coeffRef (32) = ATA.coeff (17); + ATA.coeffRef (33) = ATA.coeff (23); + ATA.coeffRef (34) = ATA.coeff (29); + + // Solve A*x = b + Vector6d x = static_cast (ATA.inverse () * ATb); + + // Construct the transformation matrix from x + constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); +} +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp new file mode 100755 index 0000000..2162012 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp @@ -0,0 +1,302 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) Alexandru-Eugen Ichim + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ + +#include +#include +#include +#include + + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::registration::TransformationEstimationPointToPlaneWeighted::TransformationEstimationPointToPlaneWeighted () + : tmp_src_ () + , tmp_tgt_ () + , tmp_idx_src_ () + , tmp_idx_tgt_ () + , warp_point_ (new WarpPointRigid6D) + , use_correspondence_weights_ (true) +{ +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + + // is the source dataset + if (cloud_src.points.size () != cloud_tgt.points.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); + PCL_ERROR ("Number or points in source (%lu) differs than target (%lu)!\n", + cloud_src.points.size (), cloud_tgt.points.size ()); + return; + } + if (cloud_src.points.size () < 4) // need at least 4 samples + { + PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); + PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!\n", + cloud_src.points.size ()); + return; + } + + if (correspondence_weights_.size () != cloud_src.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); + PCL_ERROR ("Number of weights (%lu) differs than number of points (%lu)!\n", + correspondence_weights_.size (), cloud_src.points.size ()); + return; + } + + int n_unknowns = warp_point_->getDimension (); + VectorX x (n_unknowns); + x.setZero (); + + // Set temporary pointers + tmp_src_ = &cloud_src; + tmp_tgt_ = &cloud_tgt; + + OptimizationFunctor functor (static_cast (cloud_src.points.size ()), this); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, MatScalar> lm (num_diff); + int info = lm.minimize (x); + + // Compute the norm of the residuals + PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation]"); + PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); + PCL_DEBUG ("Final solution: [%f", x[0]); + for (int i = 1; i < n_unknowns; ++i) + PCL_DEBUG (" %f", x[i]); + PCL_DEBUG ("]\n"); + + // Return the correct transformation + warp_point_->setParam (x); + transformation_matrix = warp_point_->getTransform (); + + tmp_src_ = NULL; + tmp_tgt_ = NULL; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != cloud_tgt.points.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + if (correspondence_weights_.size () != indices_src.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); + PCL_ERROR ("Number of weights (%lu) differs than number of points (%lu)!\n", + correspondence_weights_.size (), indices_src.size ()); + return; + } + + + // is the source dataset + transformation_matrix.setIdentity (); + + const int nr_correspondences = static_cast (cloud_tgt.points.size ()); + std::vector indices_tgt; + indices_tgt.resize(nr_correspondences); + for (int i = 0; i < nr_correspondences; ++i) + indices_tgt[i] = i; + + estimateRigidTransformation(cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != indices_tgt.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + if (indices_src.size () < 4) // need at least 4 samples + { + PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] "); + PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!", + indices_src.size ()); + return; + } + + if (correspondence_weights_.size () != indices_src.size ()) + { + PCL_ERROR ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] "); + PCL_ERROR ("Number of weights (%lu) differs than number of points (%lu)!\n", + correspondence_weights_.size (), indices_src.size ()); + return; + } + + + int n_unknowns = warp_point_->getDimension (); // get dimension of unknown space + VectorX x (n_unknowns); + x.setConstant (n_unknowns, 0); + + // Set temporary pointers + tmp_src_ = &cloud_src; + tmp_tgt_ = &cloud_tgt; + tmp_idx_src_ = &indices_src; + tmp_idx_tgt_ = &indices_tgt; + + OptimizationFunctorWithIndices functor (static_cast (indices_src.size ()), this); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, MatScalar> lm (num_diff); + int info = lm.minimize (x); + + // Compute the norm of the residuals + PCL_DEBUG ("[pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation] LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ()); + PCL_DEBUG ("Final solution: [%f", x[0]); + for (int i = 1; i < n_unknowns; ++i) + PCL_DEBUG (" %f", x[i]); + PCL_DEBUG ("]\n"); + + // Return the correct transformation + warp_point_->setParam (x); + transformation_matrix = warp_point_->getTransform (); + + tmp_src_ = NULL; + tmp_tgt_ = NULL; + tmp_idx_src_ = tmp_idx_tgt_ = NULL; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationPointToPlaneWeighted::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + const int nr_correspondences = static_cast (correspondences.size ()); + std::vector indices_src (nr_correspondences); + std::vector indices_tgt (nr_correspondences); + for (int i = 0; i < nr_correspondences; ++i) + { + indices_src[i] = correspondences[i].index_query; + indices_tgt[i] = correspondences[i].index_match; + } + + if (use_correspondence_weights_) + { + correspondence_weights_.resize (nr_correspondences); + for (std::size_t i = 0; i < nr_correspondences; ++i) + correspondence_weights_[i] = correspondences[i].weight; + } + + estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::TransformationEstimationPointToPlaneWeighted::OptimizationFunctor::operator () ( + const VectorX &x, VectorX &fvec) const +{ + const PointCloud & src_points = *estimator_->tmp_src_; + const PointCloud & tgt_points = *estimator_->tmp_tgt_; + + // Initialize the warp function with the given parameters + estimator_->warp_point_->setParam (x); + + // Transform each source point and compute its distance to the corresponding target point + for (int i = 0; i < values (); ++i) + { + const PointSource & p_src = src_points.points[i]; + const PointTarget & p_tgt = tgt_points.points[i]; + + // Transform the source point based on the current warp parameters + Vector4 p_src_warped; + estimator_->warp_point_->warpPoint (p_src, p_src_warped); + + // Estimate the distance (cost function) + fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt); + } + return (0); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::registration::TransformationEstimationPointToPlaneWeighted::OptimizationFunctorWithIndices::operator() ( + const VectorX &x, VectorX &fvec) const +{ + const PointCloud & src_points = *estimator_->tmp_src_; + const PointCloud & tgt_points = *estimator_->tmp_tgt_; + const std::vector & src_indices = *estimator_->tmp_idx_src_; + const std::vector & tgt_indices = *estimator_->tmp_idx_tgt_; + + // Initialize the warp function with the given parameters + estimator_->warp_point_->setParam (x); + + // Transform each source point and compute its distance to the corresponding target point + for (int i = 0; i < values (); ++i) + { + const PointSource & p_src = src_points.points[src_indices[i]]; + const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]]; + + // Transform the source point based on the current warp parameters + Vector4 p_src_warped; + estimator_->warp_point_->warpPoint (p_src, p_src_warped); + + // Estimate the distance (cost function) + fvec[i] = estimator_->correspondence_weights_[i] * estimator_->computeDistance (p_src_warped, p_tgt); + } + return (0); +} + +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_svd.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_svd.hpp new file mode 100755 index 0000000..0717db4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_svd.hpp @@ -0,0 +1,206 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + std::size_t nr_points = cloud_src.points.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != cloud_tgt.points.size ()) + { + PCL_ERROR ("[pcl::TransformationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + if (indices_src.size () != indices_tgt.size ()) + { + PCL_ERROR ("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( + ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const +{ + // Convert to Eigen format + const int npts = static_cast (source_it.size ()); + + + + if (use_umeyama_) + { + Eigen::Matrix cloud_src (3, npts); + Eigen::Matrix cloud_tgt (3, npts); + + for (int i = 0; i < npts; ++i) + { + cloud_src (0, i) = source_it->x; + cloud_src (1, i) = source_it->y; + cloud_src (2, i) = source_it->z; + ++source_it; + + cloud_tgt (0, i) = target_it->x; + cloud_tgt (1, i) = target_it->y; + cloud_tgt (2, i) = target_it->z; + ++target_it; + } + + // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) + transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); + } + else + { + source_it.reset (); target_it.reset (); + // is the source dataset + transformation_matrix.setIdentity (); + + Eigen::Matrix centroid_src, centroid_tgt; + // Estimate the centroids of source, target + compute3DCentroid (source_it, centroid_src); + compute3DCentroid (target_it, centroid_tgt); + source_it.reset (); target_it.reset (); + + // Subtract the centroids from source, target + Eigen::Matrix cloud_src_demean, cloud_tgt_demean; + demeanPointCloud (source_it, centroid_src, cloud_src_demean); + demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean); + + getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationSVD::getTransformationFromCorrelation ( + const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, + Matrix4 &transformation_matrix) const +{ + transformation_matrix.setIdentity (); + + // Assemble the correlation matrix H = source * target' + Eigen::Matrix H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3); + + // Compute the Singular Value Decomposition + Eigen::JacobiSVD > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix u = svd.matrixU (); + Eigen::Matrix v = svd.matrixV (); + + // Compute R = V * U' + if (u.determinant () * v.determinant () < 0) + { + for (int x = 0; x < 3; ++x) + v (x, 2) *= -1; + } + + Eigen::Matrix R = v * u.transpose (); + + // Return the correct transformation + transformation_matrix.topLeftCorner (3, 3) = R; + const Eigen::Matrix Rc (R * centroid_src.head (3)); + transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc; +} + +//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD; + +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_svd_scale.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_svd_scale.hpp new file mode 100755 index 0000000..553342e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_svd_scale.hpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationSVDScale::getTransformationFromCorrelation ( + const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, + Matrix4 &transformation_matrix) const +{ + transformation_matrix.setIdentity (); + + // Assemble the correlation matrix H = source * target' + Eigen::Matrix H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3); + + // Compute the Singular Value Decomposition + Eigen::JacobiSVD > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix u = svd.matrixU (); + Eigen::Matrix v = svd.matrixV (); + + // Compute R = V * U' + if (u.determinant () * v.determinant () < 0) + { + for (int x = 0; x < 3; ++x) + v (x, 2) *= -1; + } + + Eigen::Matrix R = v * u.transpose (); + + // rotated cloud + Eigen::Matrix R4; + R4.block (0, 0, 3, 3) = R; + R4 (0, 3) = 0; + R4 (1, 3) = 0; + R4 (2, 3) = 0; + R4 (3, 3) = 1; + + Eigen::Matrix src_ = R4 * cloud_src_demean; + + float scale1, scale2; + double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f; + for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx) + { + sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx); + sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx); + sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx); + + sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx); + sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx); + sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx); + + sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx); + sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx); + sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx); + } + + scale1 = sqrt (sum_tt / sum_ss); + scale2 = sum_tt_ / sum_ss; + float scale = scale2; + transformation_matrix.topLeftCorner (3, 3) = scale * R; + const Eigen::Matrix Rc (scale * R * centroid_src.head (3)); + transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc; +} + +//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD; + +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ */ diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp new file mode 100755 index 0000000..1cf74db --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp @@ -0,0 +1,204 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2019-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + const auto nr_points = cloud_src.points.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs from target (%lu)!\n", nr_points, cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const +{ + const auto nr_points = indices_src.size (); + if (cloud_tgt.points.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const +{ + const auto nr_points = indices_src.size (); + if (indices_tgt.size () != nr_points) + { + PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ()); + return; + } + + ConstCloudIterator source_it (cloud_src, indices_src); + ConstCloudIterator target_it (cloud_tgt, indices_tgt); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +estimateRigidTransformation (const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const +{ + ConstCloudIterator source_it (cloud_src, correspondences, true); + ConstCloudIterator target_it (cloud_tgt, correspondences, false); + estimateRigidTransformation (source_it, target_it, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +constructTransformationMatrix (const Vector6 ¶meters, + Matrix4 &transformation_matrix) const +{ + // Construct the transformation matrix from rotation and translation + const Eigen::AngleAxis rotation_z (parameters (2), Eigen::Matrix::UnitZ ()); + const Eigen::AngleAxis rotation_y (parameters (1), Eigen::Matrix::UnitY ()); + const Eigen::AngleAxis rotation_x (parameters (0), Eigen::Matrix::UnitX ()); + const Eigen::Translation translation (parameters (3), parameters (4), parameters (5)); + const Eigen::Transform transform = rotation_z * rotation_y * rotation_x * + translation * + rotation_z * rotation_y * rotation_x; + transformation_matrix = transform.matrix (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +estimateRigidTransformation (ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const +{ + using Matrix6 = Eigen::Matrix; + using Vector3 = Eigen::Matrix; + + Matrix6 ATA; + Vector6 ATb; + ATA.setZero (); + ATb.setZero (); + auto M = ATA.template selfadjointView (); + + // Approximate as a linear least squares problem + source_it.reset (); + target_it.reset (); + for (; source_it.isValid () && target_it.isValid (); ++source_it, ++target_it) + { + const Vector3 p (source_it->x, source_it->y, source_it->z); + const Vector3 q (target_it->x, target_it->y, target_it->z); + const Vector3 n1 (source_it->getNormalVector3fMap().template cast()); + const Vector3 n2 (target_it->getNormalVector3fMap().template cast()); + Vector3 n; + if (enforce_same_direction_normals_) + { + if (n1.dot (n2) >= 0.) + n = n1 + n2; + else + n = n1 - n2; + } + else + { + n = n1 + n2; + } + + if (!p.array().isFinite().all() || + !q.array().isFinite().all() || + !n.array().isFinite().all()) + { + continue; + } + + Vector6 v; + v << (p + q).cross (n), n; + M.rankUpdate (v); + + ATb += v * (q - p).dot (n); + } + + // Solve A*x = b + const Vector6 x = M.ldlt ().solve (ATb); + + // Construct the transformation matrix from x + constructTransformationMatrix (x, transformation_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +setEnforceSameDirectionNormals (bool enforce_same_direction_normals) +{ + enforce_same_direction_normals_ = enforce_same_direction_normals; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template inline bool +pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +getEnforceSameDirectionNormals () +{ + return enforce_same_direction_normals_; +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_validation_euclidean.hpp b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_validation_euclidean.hpp new file mode 100755 index 0000000..f5ec7fd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/impl/transformation_validation_euclidean.hpp @@ -0,0 +1,97 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ +#define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ + +///////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::registration::TransformationValidationEuclidean::validateTransformation ( + const PointCloudSourceConstPtr &cloud_src, + const PointCloudTargetConstPtr &cloud_tgt, + const Matrix4 &transformation_matrix) const +{ + double fitness_score = 0.0; + + // Transform the input dataset using the final transformation + pcl::PointCloud input_transformed; + //transformPointCloud (*cloud_src, input_transformed, transformation_matrix); + input_transformed.resize (cloud_src->size ()); + for (std::size_t i = 0; i < cloud_src->size (); ++i) + { + const PointSource &src = cloud_src->points[i]; + PointTarget &tgt = input_transformed.points[i]; + tgt.x = static_cast (transformation_matrix (0, 0) * src.x + transformation_matrix (0, 1) * src.y + transformation_matrix (0, 2) * src.z + transformation_matrix (0, 3)); + tgt.y = static_cast (transformation_matrix (1, 0) * src.x + transformation_matrix (1, 1) * src.y + transformation_matrix (1, 2) * src.z + transformation_matrix (1, 3)); + tgt.z = static_cast (transformation_matrix (2, 0) * src.x + transformation_matrix (2, 1) * src.y + transformation_matrix (2, 2) * src.z + transformation_matrix (2, 3)); + } + + typename MyPointRepresentation::ConstPtr point_rep (new MyPointRepresentation); + if (!force_no_recompute_) + { + tree_->setPointRepresentation (point_rep); + tree_->setInputCloud (cloud_tgt); + } + + std::vector nn_indices (1); + std::vector nn_dists (1); + + // For each point in the source dataset + int nr = 0; + for (std::size_t i = 0; i < input_transformed.points.size (); ++i) + { + // Find its nearest neighbor in the target + tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); + + // Deal with occlusions (incomplete targets) + if (nn_dists[0] > max_range_) + continue; + + // Calculate the fitness score + fitness_score += nn_dists[0]; + ++nr; + } + + if (nr > 0) + return (fitness_score / nr); + return (std::numeric_limits::max ()); +} + +#endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/registration/incremental_registration.h b/deps_install/include/pcl-1.10/pcl/registration/incremental_registration.h new file mode 100755 index 0000000..515746d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/incremental_registration.h @@ -0,0 +1,127 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015, Michael 'v4hn' Goerner + * Copyright (c) 2015-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +namespace pcl { + namespace registration { + + /** \brief Incremental @ref IterativeClosestPoint class + * + * This class provides a way to register a stream of clouds where each cloud will be aligned to the previous cloud. + * + * \code + * IterativeClosestPoint::Ptr icp (new IterativeClosestPoint); + * icp->setMaxCorrespondenceDistance (0.05); + * icp->setMaximumIterations (50); + * + * IncrementalRegistration iicp; + * iicp.setRegistration (icp); + * + * while (true){ + * PointCloud::Ptr cloud (new PointCloud); + * read_cloud (*cloud); + * iicp.registerCloud (cloud); + * + * PointCloud::Ptr tmp (new PointCloud); + * transformPointCloud (*cloud, *tmp, iicp.getAbsoluteTransform ()); + * write_cloud (*tmp); + * } + * \endcode + * + * \author Michael 'v4hn' Goerner + * \ingroup registration + */ + template + class IncrementalRegistration { + public: + using PointCloudPtr = typename pcl::PointCloud::Ptr; + using PointCloudConstPtr = typename pcl::PointCloud::ConstPtr; + + using RegistrationPtr = typename pcl::Registration::Ptr; + using Matrix4 = typename pcl::Registration::Matrix4; + + IncrementalRegistration (); + + /** \brief Empty destructor */ + virtual ~IncrementalRegistration () {} + + /** \brief Register new point cloud incrementally + * \note You have to set a valid registration object with @ref setRegistration before using this + * \note The class doesn't copy cloud. If you afterwards change cloud, that will affect this class. + * \param[in] cloud point cloud to register + * \param[in] delta_estimate estimated transform between last registered cloud and this one + * \return true if registration converged + */ + bool + registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ()); + + /** \brief Get estimated transform between the last two registered clouds */ + inline Matrix4 + getDeltaTransform () const; + + /** \brief Get estimated overall transform */ + inline Matrix4 + getAbsoluteTransform () const; + + /** \brief Reset incremental Registration without resetting registration_ */ + inline void + reset (); + + /** \brief Set registration instance used to align clouds */ + inline void + setRegistration (RegistrationPtr); + protected: + + /** \brief last registered point cloud */ + PointCloudConstPtr last_cloud_; + + /** \brief registration instance to align clouds */ + RegistrationPtr registration_; + + /** \brief estimated transforms */ + Matrix4 delta_transform_; + Matrix4 abs_transform_; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/joint_icp.h b/deps_install/include/pcl-1.10/pcl/registration/joint_icp.h new file mode 100755 index 0000000..d4b1ca8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/joint_icp.h @@ -0,0 +1,228 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +// PCL includes +#include +namespace pcl +{ + /** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which + * share the same transform. This is particularly useful when solving for + * camera extrinsics using multiple observations. When given a single pair of + * clouds, this reduces to vanilla ICP. + * + * \author Stephen Miller + * \ingroup registration + */ + template + class JointIterativeClosestPoint : public IterativeClosestPoint + { + public: + using PointCloudSource = typename IterativeClosestPoint::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename IterativeClosestPoint::PointCloudTarget; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using KdTree = pcl::search::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using KdTreeReciprocal = pcl::search::KdTree; + using KdTreeReciprocalPtr = typename KdTree::Ptr; + + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using CorrespondenceEstimation = pcl::registration::CorrespondenceEstimationBase; + using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr; + using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr; + + + using IterativeClosestPoint::reg_name_; + using IterativeClosestPoint::getClassName; + using IterativeClosestPoint::setInputSource; + using IterativeClosestPoint::input_; + using IterativeClosestPoint::indices_; + using IterativeClosestPoint::target_; + using IterativeClosestPoint::nr_iterations_; + using IterativeClosestPoint::max_iterations_; + using IterativeClosestPoint::previous_transformation_; + using IterativeClosestPoint::final_transformation_; + using IterativeClosestPoint::transformation_; + using IterativeClosestPoint::transformation_epsilon_; + using IterativeClosestPoint::converged_; + using IterativeClosestPoint::corr_dist_threshold_; + using IterativeClosestPoint::inlier_threshold_; + using IterativeClosestPoint::min_number_correspondences_; + using IterativeClosestPoint::update_visualizer_; + using IterativeClosestPoint::euclidean_fitness_epsilon_; + using IterativeClosestPoint::correspondences_; + using IterativeClosestPoint::transformation_estimation_; + using IterativeClosestPoint::correspondence_estimation_; + using IterativeClosestPoint::correspondence_rejectors_; + + using IterativeClosestPoint::use_reciprocal_correspondence_; + + using IterativeClosestPoint::convergence_criteria_; + using IterativeClosestPoint::source_has_normals_; + using IterativeClosestPoint::target_has_normals_; + using IterativeClosestPoint::need_source_blob_; + using IterativeClosestPoint::need_target_blob_; + + + using Matrix4 = typename IterativeClosestPoint::Matrix4; + + /** \brief Empty constructor. */ + JointIterativeClosestPoint () + { + IterativeClosestPoint (); + reg_name_ = "JointIterativeClosestPoint"; + }; + + /** \brief Empty destructor */ + ~JointIterativeClosestPoint () {} + + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + */ + void + setInputSource (const PointCloudSourceConstPtr& /*cloud*/) override + { + PCL_WARN ("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputSource.", + getClassName ().c_str ()); + return; + } + + /** \brief Add a source cloud to the joint solver + * + * \param[in] cloud source cloud + */ + inline void + addInputSource (const PointCloudSourceConstPtr &cloud) + { + // Set the parent InputSource, just to get all cached values (e.g. the existence of normals). + if (sources_.empty ()) + IterativeClosestPoint::setInputSource (cloud); + sources_.push_back (cloud); + } + + /** \brief Provide a pointer to the input target + * (e.g., the point cloud that we want to align to the target) + */ + void + setInputTarget (const PointCloudTargetConstPtr& /*cloud*/) override + { + PCL_WARN ("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects multiple clouds. Please use addInputTarget.", + getClassName ().c_str ()); + return; + } + + /** \brief Add a target cloud to the joint solver + * + * \param[in] cloud target cloud + */ + inline void + addInputTarget (const PointCloudTargetConstPtr &cloud) + { + // Set the parent InputTarget, just to get all cached values (e.g. the existence of normals). + if (targets_.empty ()) + IterativeClosestPoint::setInputTarget (cloud); + targets_.push_back (cloud); + } + + /** \brief Add a manual correspondence estimator + * If you choose to do this, you must add one for each + * input source / target pair. They do not need to have trees + * or input clouds set ahead of time. + * + * \param[in] ce Correspondence estimation + */ + inline void + addCorrespondenceEstimation (CorrespondenceEstimationPtr ce) + { + correspondence_estimations_.push_back (ce); + } + + /** \brief Reset my list of input sources + */ + inline void + clearInputSources () + { sources_.clear (); } + + /** \brief Reset my list of input targets + */ + inline void + clearInputTargets () + { targets_.clear (); } + + /** \brief Reset my list of correspondence estimation methods. + */ + inline void + clearCorrespondenceEstimations () + { correspondence_estimations_.clear (); } + + + protected: + + /** \brief Rigid transformation computation method with initial guess. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess the initial guess of the transformation to compute + */ + void + computeTransformation (PointCloudSource &output, const Matrix4 &guess) override; + + /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */ + void + determineRequiredBlobData () override; + + std::vector sources_; + std::vector targets_; + std::vector correspondence_estimations_; + }; + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/lum.h b/deps_install/include/pcl-1.10/pcl/registration/lum.h new file mode 100755 index 0000000..85a49c0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/lum.h @@ -0,0 +1,344 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lum.h 5663 2012-05-02 13:49:39Z florentinus $ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace Eigen +{ + using Vector6f = Eigen::Matrix; + using Matrix6f = Eigen::Matrix; +} + +namespace pcl +{ + namespace registration + { + /** \brief Globally Consistent Scan Matching based on an algorithm by Lu and Milios. + * \details A GraphSLAM algorithm where registration data is managed in a graph: + *
    + *
  • Vertices represent poses and hold the point cloud data and relative transformations.
    • + *
  • Edges represent pose constraints and hold the correspondence data between two point clouds.
    • + *
+ * Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously. + * For more information: + *
  • + * F. Lu, E. Milios, + * Globally Consistent Range Scan Alignment for Environment Mapping, + * Autonomous Robots 4, April 1997 + *
  • + * Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nüchter, and Joachim Hertzberg, + * The Efficient Extension of Globally Consistent Scan Matching to 6 DoF, + * In Proceedings of the 4th International Symposium on 3D Data Processing, Visualization and Transmission (3DPVT '08), June 2008 + *
+ * Usage example: + * \code + * pcl::registration::LUM lum; + * // Add point clouds as vertices to the SLAM graph + * lum.addPointCloud (cloud_0); + * lum.addPointCloud (cloud_1); + * lum.addPointCloud (cloud_2); + * // Use your favorite pairwise correspondence estimation algorithm(s) + * corrs_0_to_1 = someAlgo (cloud_0, cloud_1); + * corrs_1_to_2 = someAlgo (cloud_1, cloud_2); + * corrs_2_to_0 = someAlgo (lum.getPointCloud (2), lum.getPointCloud (0)); + * // Add the correspondence results as edges to the SLAM graph + * lum.setCorrespondences (0, 1, corrs_0_to_1); + * lum.setCorrespondences (1, 2, corrs_1_to_2); + * lum.setCorrespondences (2, 0, corrs_2_to_0); + * // Change the computation parameters + * lum.setMaxIterations (5); + * lum.setConvergenceThreshold (0.0); + * // Perform the actual LUM computation + * lum.compute (); + * // Return the concatenated point cloud result + * cloud_out = lum.getConcatenatedCloud (); + * // Return the separate point cloud transformations + * for(int i = 0; i < lum.getNumVertices (); i++) + * { + * transforms_out[i] = lum.getTransformation (i); + * } + * \endcode + * \author Frits Florentinus, Jochen Sprickerhof + * \ingroup registration + */ + template + class LUM + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + struct VertexProperties + { + PointCloudPtr cloud_; + Eigen::Vector6f pose_; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + struct EdgeProperties + { + pcl::CorrespondencesPtr corrs_; + Eigen::Matrix6f cinv_; + Eigen::Vector6f cinvd_; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + using SLAMGraph = boost::adjacency_list; + using SLAMGraphPtr = shared_ptr; + using Vertex = typename SLAMGraph::vertex_descriptor; + using Edge = typename SLAMGraph::edge_descriptor; + + /** \brief Empty constructor. + */ + LUM () + : slam_graph_ (new SLAMGraph) + , max_iterations_ (5) + , convergence_threshold_ (0.0) + { + } + + /** \brief Set the internal SLAM graph structure. + * \details All data used and produced by LUM is stored in this boost::adjacency_list. + * It is recommended to use the LUM class itself to build the graph. + * This method could otherwise be useful for managing several SLAM graphs in one instance of LUM. + * \param[in] slam_graph The new SLAM graph. + */ + inline void + setLoopGraph (const SLAMGraphPtr &slam_graph); + + /** \brief Get the internal SLAM graph structure. + * \details All data used and produced by LUM is stored in this boost::adjacency_list. + * It is recommended to use the LUM class itself to build the graph. + * This method could otherwise be useful for managing several SLAM graphs in one instance of LUM. + * \return The current SLAM graph. + */ + inline SLAMGraphPtr + getLoopGraph () const; + + /** \brief Get the number of vertices in the SLAM graph. + * \return The current number of vertices in the SLAM graph. + */ + typename SLAMGraph::vertices_size_type + getNumVertices () const; + + /** \brief Set the maximum number of iterations for the compute() method. + * \details The compute() method finishes when max_iterations are met or when the convergence criteria is met. + * \param[in] max_iterations The new maximum number of iterations (default = 5). + */ + void + setMaxIterations (int max_iterations); + + /** \brief Get the maximum number of iterations for the compute() method. + * \details The compute() method finishes when max_iterations are met or when the convergence criteria is met. + * \return The current maximum number of iterations (default = 5). + */ + inline int + getMaxIterations () const; + + /** \brief Set the convergence threshold for the compute() method. + * \details When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector. + * When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met. + * \param[in] convergence_threshold The new convergence threshold (default = 0.0). + */ + void + setConvergenceThreshold (float convergence_threshold); + + /** \brief Get the convergence threshold for the compute() method. + * \details When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector. + * When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met. + * \return The current convergence threshold (default = 0.0). + */ + inline float + getConvergenceThreshold () const; + + /** \brief Add a new point cloud to the SLAM graph. + * \details This method will add a new vertex to the SLAM graph and attach a point cloud to that vertex. + * Optionally you can specify a pose estimate for this point cloud. + * A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added. + * Because this first vertex is the reference, you can not set a pose estimate for this vertex. + * Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM. + * \note Vertex descriptors are typecastable to int. + * \param[in] cloud The new point cloud. + * \param[in] pose (optional) The pose estimate relative to the reference pose (first point cloud that was added). + * \return The vertex descriptor of the newly created vertex. + */ + Vertex + addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose = Eigen::Vector6f::Zero ()); + + /** \brief Change a point cloud on one of the SLAM graph's vertices. + * \details This method will change the point cloud attached to an existing vertex and will not alter the SLAM graph structure. + * Note that the correspondences attached to this vertex will not change and may need to be updated manually. + * \note Vertex descriptors are typecastable to int. + * \param[in] vertex The vertex descriptor of which to change the point cloud. + * \param[in] cloud The new point cloud for that vertex. + */ + inline void + setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud); + + /** \brief Return a point cloud from one of the SLAM graph's vertices. + * \note Vertex descriptors are typecastable to int. + * \param[in] vertex The vertex descriptor of which to return the point cloud. + * \return The current point cloud for that vertex. + */ + inline PointCloudPtr + getPointCloud (const Vertex &vertex) const; + + /** \brief Change a pose estimate on one of the SLAM graph's vertices. + * \details A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added. + * Because this first vertex is the reference, you can not set a pose estimate for this vertex. + * Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM. + * \note Vertex descriptors are typecastable to int. + * \param[in] vertex The vertex descriptor of which to set the pose estimate. + * \param[in] pose The new pose estimate for that vertex. + */ + inline void + setPose (const Vertex &vertex, const Eigen::Vector6f &pose); + + /** \brief Return a pose estimate from one of the SLAM graph's vertices. + * \note Vertex descriptors are typecastable to int. + * \param[in] vertex The vertex descriptor of which to return the pose estimate. + * \return The current pose estimate of that vertex. + */ + inline Eigen::Vector6f + getPose (const Vertex &vertex) const; + + /** \brief Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix. + * \note Vertex descriptors are typecastable to int. + * \param[in] vertex The vertex descriptor of which to return the transformation matrix. + * \return The current transformation matrix of that vertex. + */ + inline Eigen::Affine3f + getTransformation (const Vertex &vertex) const; + + /** \brief Add/change a set of correspondences for one of the SLAM graph's edges. + * \details The edges in the SLAM graph are directional and point from source vertex to target vertex. + * The query indices of the correspondences, index the points at the source vertex' point cloud. + * The matching indices of the correspondences, index the points at the target vertex' point cloud. + * If no edge was present at the specified location, this method will add a new edge to the SLAM graph and attach the correspondences to that edge. + * If the edge was already present, this method will overwrite the correspondence information of that edge and will not alter the SLAM graph structure. + * \note Vertex descriptors are typecastable to int. + * \param[in] source_vertex The vertex descriptor of the correspondences' source point cloud. + * \param[in] target_vertex The vertex descriptor of the correspondences' target point cloud. + * \param[in] corrs The new set of correspondences for that edge. + */ + void + setCorrespondences (const Vertex &source_vertex, + const Vertex &target_vertex, + const pcl::CorrespondencesPtr &corrs); + + /** \brief Return a set of correspondences from one of the SLAM graph's edges. + * \note Vertex descriptors are typecastable to int. + * \param[in] source_vertex The vertex descriptor of the correspondences' source point cloud. + * \param[in] target_vertex The vertex descriptor of the correspondences' target point cloud. + * \return The current set of correspondences of that edge. + */ + inline pcl::CorrespondencesPtr + getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const; + + /** \brief Perform LUM's globally consistent scan matching. + * \details Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously. + *
+ * Things to keep in mind: + *
    + *
  • Only those parts of the graph connected to the reference pose will properly align to it.
    • + *
  • All sets of correspondences should span the same space and need to be sufficient to determine a rigid transformation.
    • + *
  • The algorithm draws it strength from loops in the graph because it will distribute errors evenly amongst those loops.
    • + *
+ * Computation ends when either of the following conditions hold: + *
    + *
  • The number of iterations reaches max_iterations. Use setMaxIterations() to change.
    • + *
  • The convergence criteria is met. Use setConvergenceThreshold() to change.
    • + *
+ * Computation will change the pose estimates for the vertices of the SLAM graph, not the point clouds attached to them. + * The results can be retrieved with getPose(), getTransformation(), getTransformedCloud() or getConcatenatedCloud(). + */ + void + compute (); + + /** \brief Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate. + * \note Vertex descriptors are typecastable to int. + * \param[in] vertex The vertex descriptor of which to return the transformed point cloud. + * \return The transformed point cloud of that vertex. + */ + PointCloudPtr + getTransformedCloud (const Vertex &vertex) const; + + /** \brief Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current pose estimates. + * \return The concatenated transformed point clouds of the entire SLAM graph. + */ + PointCloudPtr + getConcatenatedCloud () const; + + protected: + /** \brief Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_). */ + void + computeEdge (const Edge &e); + + /** \brief Returns a pose corrected 6DoF incidence matrix. */ + inline Eigen::Matrix6f + incidenceCorrection (const Eigen::Vector6f &pose); + + private: + /** \brief The internal SLAM graph structure. */ + SLAMGraphPtr slam_graph_; + + /** \brief The maximum number of iterations for the compute() method. */ + int max_iterations_; + + /** \brief The convergence threshold for the summed vector lengths of all poses. */ + float convergence_threshold_; + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/registration/matching_candidate.h b/deps_install/include/pcl-1.10/pcl/registration/matching_candidate.h new file mode 100755 index 0000000..8767957 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/matching_candidate.h @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel. + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief Container for matching candidate consisting of + * + * * fitness score value as a result of the matching algorithm + * * correspondences between source and target data set + * * transformation matrix calculated based on the correspondences + * + */ + struct MatchingCandidate + { + /** \brief Constructor. */ + MatchingCandidate () : + fitness_score (FLT_MAX), + transformation (Eigen::Matrix4f::Identity ()) + {}; + + /** \brief Value constructor. */ + MatchingCandidate (float s, const pcl::Correspondences &c, const Eigen::Matrix4f &m) : + fitness_score (s), + correspondences (c), + transformation (m) + {}; + + /** \brief Destructor. */ + ~MatchingCandidate () + {}; + + + /** \brief Fitness score of current candidate resulting from matching algorithm. */ + float fitness_score; + + /** \brief Correspondences between source <-> target. */ + pcl::Correspondences correspondences; + + /** \brief Corresponding transformation matrix retrieved using \a corrs. */ + Eigen::Matrix4f transformation; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + using MatchingCandidates = std::vector >; + + /** \brief Sorting of candidates based on fitness score value. */ + struct by_score + { + /** \brief Operator used to sort candidates based on fitness score. */ + bool operator () (MatchingCandidate const &left, MatchingCandidate const &right) + { + return (left.fitness_score < right.fitness_score); + } + }; + + }; // namespace registration +}; // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/registration/meta_registration.h b/deps_install/include/pcl-1.10/pcl/registration/meta_registration.h new file mode 100755 index 0000000..9cca7f3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/meta_registration.h @@ -0,0 +1,128 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015, Michael 'v4hn' Goerner + * Copyright (c) 2015-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +namespace pcl { + namespace registration { + + /** \brief Meta @ref Registration class + * \note This method accumulates all registered points and becomes more costly with each registered point cloud. + * + * This class provides a way to register a stream of clouds where each cloud + * will be aligned to the conglomerate of all previous clouds. + * + * \code + * IterativeClosestPoint::Ptr icp (new IterativeClosestPoint); + * icp->setMaxCorrespondenceDistance (0.05); + * icp->setMaximumIterations (50); + * + * MetaRegistration mreg; + * mreg.setRegistration (icp); + * + * while (true) + * { + * PointCloud::Ptr cloud (new PointCloud); + * read_cloud (*cloud); + * mreg.registerCloud (cloud); + * + * PointCloud::Ptr tmp (new PointCloud); + * transformPointCloud (*cloud, *tmp, mreg.getAbsoluteTransform ()); + * write_cloud (*tmp); + * } + * \endcode + * + * \author Michael 'v4hn' Goerner + * \ingroup registration + */ + template + class MetaRegistration { + public: + using PointCloudPtr = typename pcl::PointCloud::Ptr; + using PointCloudConstPtr = typename pcl::PointCloud::ConstPtr; + + using RegistrationPtr = typename pcl::Registration::Ptr; + using Matrix4 = typename pcl::Registration::Matrix4; + + MetaRegistration (); + + /** \brief Empty destructor */ + virtual ~MetaRegistration () {}; + + /** \brief Register new point cloud + * \note You have to set a valid registration object with @ref setICP before using this + * \param[in] cloud point cloud to register + * \param[in] delta_estimate estimated transform between last registered cloud and this one + * \return true if ICP converged + */ + bool + registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ()); + + /** \brief Get estimated transform of the last registered cloud */ + inline Matrix4 + getAbsoluteTransform () const; + + /** \brief Reset MetaICP without resetting registration_ */ + inline void + reset (); + + /** \brief Set registration instance used to align clouds */ + inline void + setRegistration (RegistrationPtr); + + /** \brief get accumulated meta point cloud */ + inline PointCloudConstPtr + getMetaCloud () const; + protected: + + /** \brief registered accumulated point cloud */ + PointCloudPtr full_cloud_; + + /** \brief registration instance to align clouds */ + RegistrationPtr registration_; + + /** \brief estimated transform */ + Matrix4 abs_transform_; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/ndt.h b/deps_install/include/pcl-1.10/pcl/registration/ndt.h new file mode 100755 index 0000000..b78aa2c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/ndt.h @@ -0,0 +1,466 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +#include + +namespace pcl +{ + /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data. + * \note For more information please see + * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — + * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. + * PhD thesis, Orebro University. Orebro Studies in Technology 36., + * More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease + * In ACM Transactions on Mathematical Software. and + * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100 + * \note Math refactored by Todor Stoyanov. + * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) + */ + template + class NormalDistributionsTransform : public Registration + { + protected: + + using PointCloudSource = typename Registration::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename Registration::PointCloudTarget; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + /** \brief Typename of searchable voxel grid containing mean and covariance. */ + using TargetGrid = VoxelGridCovariance; + /** \brief Typename of pointer to searchable voxel grid. */ + using TargetGridPtr = TargetGrid *; + /** \brief Typename of const pointer to searchable voxel grid. */ + using TargetGridConstPtr = const TargetGrid *; + /** \brief Typename of const pointer to searchable voxel grid leaf. */ + using TargetGridLeafConstPtr = typename TargetGrid::LeafConstPtr; + + + public: + + using Ptr = shared_ptr< NormalDistributionsTransform >; + using ConstPtr = shared_ptr< const NormalDistributionsTransform >; + + + /** \brief Constructor. + * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0 + */ + NormalDistributionsTransform (); + + /** \brief Empty destructor */ + ~NormalDistributionsTransform () {} + + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to). + * \param[in] cloud the input point cloud target + */ + inline void + setInputTarget (const PointCloudTargetConstPtr &cloud) override + { + Registration::setInputTarget (cloud); + init (); + } + + /** \brief Set/change the voxel grid resolution. + * \param[in] resolution side length of voxels + */ + inline void + setResolution (float resolution) + { + // Prevents unnessary voxel initiations + if (resolution_ != resolution) + { + resolution_ = resolution; + if (input_) + init (); + } + } + + /** \brief Get voxel grid resolution. + * \return side length of voxels + */ + inline float + getResolution () const + { + return (resolution_); + } + + /** \brief Get the newton line search maximum step length. + * \return maximum step length + */ + inline double + getStepSize () const + { + return (step_size_); + } + + /** \brief Set/change the newton line search maximum step length. + * \param[in] step_size maximum step length + */ + inline void + setStepSize (double step_size) + { + step_size_ = step_size; + } + + /** \brief Get the point cloud outlier ratio. + * \return outlier ratio + */ + inline double + getOulierRatio () const + { + return (outlier_ratio_); + } + + /** \brief Set/change the point cloud outlier ratio. + * \param[in] outlier_ratio outlier ratio + */ + inline void + setOulierRatio (double outlier_ratio) + { + outlier_ratio_ = outlier_ratio; + } + + /** \brief Get the registration alignment probability. + * \return transformation probability + */ + inline double + getTransformationProbability () const + { + return (trans_probability_); + } + + /** \brief Get the number of iterations required to calculate alignment. + * \return final number of iterations + */ + inline int + getFinalNumIteration () const + { + return (nr_iterations_); + } + + /** \brief Convert 6 element transformation vector to affine transformation. + * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] + * \param[out] trans affine transform corresponding to given transfomation vector + */ + static void + convertTransform (const Eigen::Matrix &x, Eigen::Affine3f &trans) + { + trans = Eigen::Translation(float (x (0)), float (x (1)), float (x (2))) * + Eigen::AngleAxis(float (x (3)), Eigen::Vector3f::UnitX ()) * + Eigen::AngleAxis(float (x (4)), Eigen::Vector3f::UnitY ()) * + Eigen::AngleAxis(float (x (5)), Eigen::Vector3f::UnitZ ()); + } + + /** \brief Convert 6 element transformation vector to transformation matrix. + * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] + * \param[out] trans 4x4 transformation matrix corresponding to given transfomation vector + */ + static void + convertTransform (const Eigen::Matrix &x, Eigen::Matrix4f &trans) + { + Eigen::Affine3f _affine; + convertTransform (x, _affine); + trans = _affine.matrix (); + } + + protected: + + using Registration::reg_name_; + using Registration::getClassName; + using Registration::input_; + using Registration::indices_; + using Registration::target_; + using Registration::nr_iterations_; + using Registration::max_iterations_; + using Registration::previous_transformation_; + using Registration::final_transformation_; + using Registration::transformation_; + using Registration::transformation_epsilon_; + using Registration::transformation_rotation_epsilon_; + using Registration::converged_; + using Registration::corr_dist_threshold_; + using Registration::inlier_threshold_; + + using Registration::update_visualizer_; + + /** \brief Estimate the transformation and returns the transformed source (input) as output. + * \param[out] output the resultant input transformed point cloud dataset + */ + virtual void + computeTransformation (PointCloudSource &output) + { + computeTransformation (output, Eigen::Matrix4f::Identity ()); + } + + /** \brief Estimate the transformation and returns the transformed source (input) as output. + * \param[out] output the resultant input transformed point cloud dataset + * \param[in] guess the initial gross estimation of the transformation + */ + void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override; + + /** \brief Initiate covariance voxel structure. */ + void inline + init () + { + target_cells_.setLeafSize (resolution_, resolution_, resolution_); + target_cells_.setInputCloud ( target_ ); + // Initiate voxel structure. + target_cells_.filter (true); + } + + /** \brief Compute derivatives of probability function w.r.t. the transformation vector. + * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. + * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector + * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector + * \param[in] trans_cloud transformed point cloud + * \param[in] p the current transform vector + * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. + */ + double + computeDerivatives (Eigen::Matrix &score_gradient, + Eigen::Matrix &hessian, + PointCloudSource &trans_cloud, + Eigen::Matrix &p, + bool compute_hessian = true); + + /** \brief Compute individual point contirbutions to derivatives of probability function w.r.t. the transformation vector. + * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. + * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector + * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector + * \param[in] x_trans transformed point minus mean of occupied covariance voxel + * \param[in] c_inv covariance of occupied covariance voxel + * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. + */ + double + updateDerivatives (Eigen::Matrix &score_gradient, + Eigen::Matrix &hessian, + Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv, + bool compute_hessian = true); + + /** \brief Precompute anglular components of derivatives. + * \note Equation 6.19 and 6.21 [Magnusson 2009]. + * \param[in] p the current transform vector + * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. + */ + void + computeAngleDerivatives (Eigen::Matrix &p, bool compute_hessian = true); + + /** \brief Compute point derivatives. + * \note Equation 6.18-21 [Magnusson 2009]. + * \param[in] x point from the input cloud + * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. + */ + void + computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian = true); + + /** \brief Compute hessian of probability function w.r.t. the transformation vector. + * \note Equation 6.13 [Magnusson 2009]. + * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector + * \param[in] trans_cloud transformed point cloud + * \param[in] p the current transform vector + */ + void + computeHessian (Eigen::Matrix &hessian, + PointCloudSource &trans_cloud, + Eigen::Matrix &p); + + /** \brief Compute individual point contirbutions to hessian of probability function w.r.t. the transformation vector. + * \note Equation 6.13 [Magnusson 2009]. + * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector + * \param[in] x_trans transformed point minus mean of occupied covariance voxel + * \param[in] c_inv covariance of occupied covariance voxel + */ + void + updateHessian (Eigen::Matrix &hessian, + Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv); + + /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method. + * \note Search Algorithm [More, Thuente 1994] + * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009] + * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009] + * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009] + * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994) + * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994) + * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009] + * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009] + * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009] + * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009] + * \return final step length + */ + double + computeStepLengthMT (const Eigen::Matrix &x, + Eigen::Matrix &step_dir, + double step_init, + double step_max, double step_min, + double &score, + Eigen::Matrix &score_gradient, + Eigen::Matrix &hessian, + PointCloudSource &trans_cloud); + + /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994) + * \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ + * and Modified Updating Algorithm from then on [More, Thuente 1994]. + * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) + * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm + * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm + * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) + * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm + * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm + * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) + * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm + * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm + * \return if interval converges + */ + bool + updateIntervalMT (double &a_l, double &f_l, double &g_l, + double &a_u, double &f_u, double &g_u, + double a_t, double f_t, double g_t); + + /** \brief Select new trial value for More-Thuente method. + * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$ + * until some value satisfies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ + * then \f$ \phi(\alpha_k) \f$ is used from then on. + * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100). + * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) + * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994) + * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994) + * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) + * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994) + * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994) + * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) + * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994) + * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994) + * \return new trial value + */ + double + trialValueSelectionMT (double a_l, double f_l, double g_l, + double a_u, double f_u, double g_u, + double a_t, double f_t, double g_t); + + /** \brief Auxiliary function used to determine endpoints of More-Thuente interval. + * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994) + * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994) + * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994) + * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994) + * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) + * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] + * \return sufficient decrease value + */ + inline double + auxilaryFunction_PsiMT (double a, double f_a, double f_0, double g_0, double mu = 1.e-4) + { + return (f_a - f_0 - mu * g_0 * a); + } + + /** \brief Auxiliary function derivative used to determine endpoints of More-Thuente interval. + * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994) + * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994) + * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) + * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] + * \return sufficient decrease derivative + */ + inline double + auxilaryFunction_dPsiMT (double g_a, double g_0, double mu = 1.e-4) + { + return (g_a - mu * g_0); + } + + /** \brief The voxel grid generated from target cloud containing point means and covariances. */ + TargetGrid target_cells_; + + //double fitness_epsilon_; + + /** \brief The side length of voxels. */ + float resolution_; + + /** \brief The maximum step length. */ + double step_size_; + + /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */ + double outlier_ratio_; + + /** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */ + double gauss_d1_, gauss_d2_; + + /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */ + double trans_probability_; + + /** \brief Precomputed Angular Gradient + * + * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009]. + */ + Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_; + + /** \brief Precomputed Angular Hessian + * + * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009]. + */ + Eigen::Vector3d h_ang_a2_, h_ang_a3_, + h_ang_b2_, h_ang_b3_, + h_ang_c2_, h_ang_c3_, + h_ang_d1_, h_ang_d2_, h_ang_d3_, + h_ang_e1_, h_ang_e2_, h_ang_e3_, + h_ang_f1_, h_ang_f2_, h_ang_f3_; + + /** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */ + Eigen::Matrix point_gradient_; + + /** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */ + Eigen::Matrix point_hessian_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/ndt_2d.h b/deps_install/include/pcl-1.10/pcl/registration/ndt_2d.h new file mode 100755 index 0000000..e514fe5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/ndt_2d.h @@ -0,0 +1,155 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b NormalDistributionsTransform2D provides an implementation of the + * Normal Distributions Transform algorithm for scan matching. + * + * This implementation is intended to match the definition: + * Peter Biber and Wolfgang Straßer. The normal distributions transform: A + * new approach to laser scan matching. In Proceedings of the IEEE In- + * ternational Conference on Intelligent Robots and Systems (IROS), pages + * 2743–2748, Las Vegas, USA, October 2003. + * + * \author James Crosby + */ + template + class NormalDistributionsTransform2D : public Registration + { + using PointCloudSource = typename Registration::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename Registration::PointCloudTarget; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + public: + + using Ptr = shared_ptr< NormalDistributionsTransform2D >; + using ConstPtr = shared_ptr< const NormalDistributionsTransform2D >; + + /** \brief Empty constructor. */ + NormalDistributionsTransform2D () + : Registration (), + grid_centre_ (0,0), grid_step_ (1,1), grid_extent_ (20,20), newton_lambda_ (1,1,1) + { + reg_name_ = "NormalDistributionsTransform2D"; + } + + /** \brief Empty destructor */ + ~NormalDistributionsTransform2D () {} + + /** \brief centre of the ndt grid (target coordinate system) + * \param centre value to set + */ + virtual void + setGridCentre (const Eigen::Vector2f& centre) { grid_centre_ = centre; } + + /** \brief Grid spacing (step) of the NDT grid + * \param[in] step value to set + */ + virtual void + setGridStep (const Eigen::Vector2f& step) { grid_step_ = step; } + + /** \brief NDT Grid extent (in either direction from the grid centre) + * \param[in] extent value to set + */ + virtual void + setGridExtent (const Eigen::Vector2f& extent) { grid_extent_ = extent; } + + /** \brief NDT Newton optimisation step size parameter + * \param[in] lambda step size: 1 is simple newton optimisation, smaller values may improve convergence + */ + virtual void + setOptimizationStepSize (const double& lambda) { newton_lambda_ = Eigen::Vector3d (lambda, lambda, lambda); } + + /** \brief NDT Newton optimisation step size parameter + * \param[in] lambda step size: (1,1,1) is simple newton optimisation, + * smaller values may improve convergence, or elements may be set to + * zero to prevent optimisation over some parameters + * + * This overload allows control of updates to the individual (x, y, + * theta) free parameters in the optimisation. If, for example, theta is + * believed to be close to the correct value a small value of lambda[2] + * should be used. + */ + virtual void + setOptimizationStepSize (const Eigen::Vector3d& lambda) { newton_lambda_ = lambda; } + + protected: + /** \brief Rigid transformation computation method with initial guess. + * \param[out] output the transformed input point cloud dataset using the rigid transformation found + * \param[in] guess the initial guess of the transformation to compute + */ + void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override; + + using Registration::reg_name_; + using Registration::target_; + using Registration::converged_; + using Registration::nr_iterations_; + using Registration::max_iterations_; + using Registration::transformation_epsilon_; + using Registration::transformation_rotation_epsilon_; + using Registration::transformation_; + using Registration::previous_transformation_; + using Registration::final_transformation_; + using Registration::update_visualizer_; + using Registration::indices_; + + Eigen::Vector2f grid_centre_; + Eigen::Vector2f grid_step_; + Eigen::Vector2f grid_extent_; + Eigen::Vector3d newton_lambda_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/pairwise_graph_registration.hpp b/deps_install/include/pcl-1.10/pcl/registration/pairwise_graph_registration.hpp new file mode 100755 index 0000000..2a57773 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/pairwise_graph_registration.hpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_ +#define PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PairwiseGraphRegistration::computeRegistration () +{ + if (!registration_method_) + { + PCL_ERROR ("[pcl::PairwiseGraphRegistration::computeRegistration] No registration method set!\n"); + return; + } + + typename std::vector::iterator last_vx_it = last_vertices_.begin (); + if (last_aligned_vertex_ == boost::graph_traits::null_vertex ()) + { + last_aligned_vertex_ = *last_vx_it; + ++last_vx_it; + } + + pcl::PointCloud fake_cloud; + registration_method_->setInputTarget (boost::get_cloud (last_aligned_vertex_, *(graph_handler_->getGraph ()))); + for(; last_vx_it < last_vertices_.end (); ++last_vx_it) + { + registration_method_->setInputCloud (boost::get_cloud (*last_vx_it, *(graph_handler_->getGraph ()))); + + const Eigen::Matrix4f last_aligned_vertex_pose = boost::get_pose (last_aligned_vertex_, *(graph_handler_->getGraph ())); + if (!incremental_) + { + const Eigen::Matrix4f guess = last_aligned_vertex_pose.transpose () * boost::get_pose (*last_vx_it, *(graph_handler_->getGraph ())); + registration_method_->align (fake_cloud, guess); + } else + registration_method_->align (fake_cloud); + + const Eigen::Matrix4f global_ref_final_tr = last_aligned_vertex_pose * registration_method_->getFinalTransformation (); + boost::set_estimate (*last_vx_it, global_ref_final_tr, *(graph_handler_->getGraph ())); + last_aligned_vertex_ = *last_vx_it; + registration_method_->setInputTarget (boost::get_cloud (last_aligned_vertex_, *(graph_handler_->getGraph ()))); + } +} +#endif //PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/registration/ppf_registration.h b/deps_install/include/pcl-1.10/pcl/registration/ppf_registration.h new file mode 100755 index 0000000..fd0aa7d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/ppf_registration.h @@ -0,0 +1,299 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +#include + +namespace pcl +{ + class PCL_EXPORTS PPFHashMapSearch + { + public: + /** \brief Data structure to hold the information for the key in the feature hash map of the + * PPFHashMapSearch class + * \note It uses multiple pair levels in order to enable the usage of the boost::hash function + * which has the std::pair implementation (i.e., does not require a custom hash function) + */ + struct HashKeyStruct : public std::pair > > + { + HashKeyStruct () = default; + + HashKeyStruct(int a, int b, int c, int d) + { + this->first = a; + this->second.first = b; + this->second.second.first = c; + this->second.second.second = d; + } + + std::size_t operator()(const HashKeyStruct& s) const noexcept + { + const std::size_t h1 = std::hash{} (s.first); + const std::size_t h2 = std::hash{} (s.second.first); + const std::size_t h3 = std::hash{} (s.second.second.first); + const std::size_t h4 = std::hash{} (s.second.second.second); + return h1 ^ (h2 << 1) ^ (h3 << 2) ^ (h4 << 3); + } + }; + using FeatureHashMapType = std::unordered_multimap, HashKeyStruct>; + using FeatureHashMapTypePtr = shared_ptr; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + + /** \brief Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data structure + * \param angle_discretization_step the step value between each bin of the hash map for the angular values + * \param distance_discretization_step the step value between each bin of the hash map for the distance values + */ + PPFHashMapSearch (float angle_discretization_step = 12.0f / 180.0f * static_cast (M_PI), + float distance_discretization_step = 0.01f) + : feature_hash_map_ (new FeatureHashMapType) + , internals_initialized_ (false) + , angle_discretization_step_ (angle_discretization_step) + , distance_discretization_step_ (distance_discretization_step) + , max_dist_ (-1.0f) + { + } + + /** \brief Method that sets the feature cloud to be inserted in the hash map + * \param feature_cloud a const smart pointer to the PPFSignature feature cloud + */ + void + setInputFeatureCloud (PointCloud::ConstPtr feature_cloud); + + /** \brief Function for finding the nearest neighbors for the given feature inside the discretized hash map + * \param f1 The 1st value describing the query PPFSignature feature + * \param f2 The 2nd value describing the query PPFSignature feature + * \param f3 The 3rd value describing the query PPFSignature feature + * \param f4 The 4th value describing the query PPFSignature feature + * \param indices a vector of pair indices representing the feature pairs that have been found in the bin + * corresponding to the query feature + */ + void + nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4, + std::vector > &indices); + + /** \brief Convenience method for returning a copy of the class instance as a shared_ptr */ + Ptr + makeShared() { return Ptr (new PPFHashMapSearch (*this)); } + + /** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */ + inline float + getAngleDiscretizationStep () { return angle_discretization_step_; } + + /** \brief Returns the distance discretization step parameter (the step value between each bin of the hash map for the distance values) */ + inline float + getDistanceDiscretizationStep () { return distance_discretization_step_; } + + /** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */ + inline float + getModelDiameter () { return max_dist_; } + + std::vector > alpha_m_; + private: + FeatureHashMapTypePtr feature_hash_map_; + bool internals_initialized_; + + float angle_discretization_step_, distance_discretization_step_; + float max_dist_; + }; + + /** \brief Class that registers two point clouds based on their sets of PPFSignatures. + * Please refer to the following publication for more details: + * B. Drost, M. Ulrich, N. Navab, S. Ilic + * Model Globally, Match Locally: Efficient and Robust 3D Object Recognition + * 2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR) + * 13-18 June 2010, San Francisco, CA + * + * \note This class works in tandem with the PPFEstimation class + * + * \author Alexandru-Eugen Ichim + */ + template + class PPFRegistration : public Registration + { + public: + /** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes + * \note initially used std::pair, but it proved problematic + * because of the Eigen structures alignment problems - std::pair does not have a custom allocator + */ + struct PoseWithVotes + { + PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes) + : pose (a_pose), + votes (a_votes) + {} + + Eigen::Affine3f pose; + unsigned int votes; + }; + using PoseWithVotesList = std::vector >; + + /// input_ is the model cloud + using Registration::input_; + /// target_ is the scene cloud + using Registration::target_; + using Registration::converged_; + using Registration::final_transformation_; + using Registration::transformation_; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + + /** \brief Empty constructor that initializes all the parameters of the algorithm with default values */ + PPFRegistration () + : Registration (), + scene_reference_point_sampling_rate_ (5), + clustering_position_diff_threshold_ (0.01f), + clustering_rotation_diff_threshold_ (20.0f / 180.0f * static_cast (M_PI)) + {} + + /** \brief Method for setting the position difference clustering parameter + * \param clustering_position_diff_threshold distance threshold below which two poses are + * considered close enough to be in the same cluster (for the clustering phase of the algorithm) + */ + inline void + setPositionClusteringThreshold (float clustering_position_diff_threshold) { clustering_position_diff_threshold_ = clustering_position_diff_threshold; } + + /** \brief Returns the parameter defining the position difference clustering parameter - + * distance threshold below which two poses are considered close enough to be in the same cluster + * (for the clustering phase of the algorithm) + */ + inline float + getPositionClusteringThreshold () { return clustering_position_diff_threshold_; } + + /** \brief Method for setting the rotation clustering parameter + * \param clustering_rotation_diff_threshold rotation difference threshold below which two + * poses are considered to be in the same cluster (for the clustering phase of the algorithm) + */ + inline void + setRotationClusteringThreshold (float clustering_rotation_diff_threshold) { clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold; } + + /** \brief Returns the parameter defining the rotation clustering threshold + */ + inline float + getRotationClusteringThreshold () { return clustering_rotation_diff_threshold_; } + + /** \brief Method for setting the scene reference point sampling rate + * \param scene_reference_point_sampling_rate sampling rate for the scene reference point + */ + inline void + setSceneReferencePointSamplingRate (unsigned int scene_reference_point_sampling_rate) { scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate; } + + /** \brief Returns the parameter for the scene reference point sampling rate of the algorithm */ + inline unsigned int + getSceneReferencePointSamplingRate () { return scene_reference_point_sampling_rate_; } + + /** \brief Function that sets the search method for the algorithm + * \note Right now, the only available method is the one initially proposed by + * the authors - by using a hash map with discretized feature vectors + * \param search_method smart pointer to the search method to be set + */ + inline void + setSearchMethod (PPFHashMapSearch::Ptr search_method) { search_method_ = search_method; } + + /** \brief Getter function for the search method of the class */ + inline PPFHashMapSearch::Ptr + getSearchMethod () { return search_method_; } + + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) + * \param cloud the input point cloud target + */ + void + setInputTarget (const PointCloudTargetConstPtr &cloud) override; + + + private: + /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */ + void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override; + + + /** \brief the search method that is going to be used to find matching feature pairs */ + PPFHashMapSearch::Ptr search_method_; + + /** \brief parameter for the sampling rate of the scene reference points */ + unsigned int scene_reference_point_sampling_rate_; + + /** \brief position and rotation difference thresholds below which two + * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */ + float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_; + + /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */ + typename pcl::KdTreeFLANN::Ptr scene_search_tree_; + + /** \brief static method used for the std::sort function to order two PoseWithVotes + * instances by their number of votes*/ + static bool + poseWithVotesCompareFunction (const PoseWithVotes &a, + const PoseWithVotes &b); + + /** \brief static method used for the std::sort function to order two pairs + * by the number of votes (unsigned integer value) */ + static bool + clusterVotesCompareFunction (const std::pair &a, + const std::pair &b); + + /** \brief Method that clusters a set of given poses by using the clustering thresholds + * and their corresponding number of votes (see publication for more details) */ + void + clusterPoses (PoseWithVotesList &poses, + PoseWithVotesList &result); + + /** \brief Method that checks whether two poses are close together - based on the clustering threshold parameters + * of the class */ + bool + posesWithinErrorBounds (Eigen::Affine3f &pose1, + Eigen::Affine3f &pose2); + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/pyramid_feature_matching.h b/deps_install/include/pcl-1.10/pcl/registration/pyramid_feature_matching.h new file mode 100755 index 0000000..92dbc35 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/pyramid_feature_matching.h @@ -0,0 +1,196 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** + * \brief Class that compares two sets of features by using a multiscale representation of the features inside a + * pyramid. Each level of the pyramid offers information about the similarity of the two feature sets. + * \note Works with any Point/Feature type which has a PointRepresentation implementation + * \note The only parameters it needs are the input dimension ranges and the output dimension ranges. The input + * dimension ranges represent the ranges in which each dimension of the feature vector lies. As described in the + * paper, a minimum inter-vector distance of sqrt(nr_dims)/2 is needed. As such, the target dimension range parameter + * is used in order to augment/reduce the range for each dimension in order to obtain the necessary minimal + * inter-vector distance and to add/subtract weight to/from certain dimensions of the feature vector. + * + * Follows the algorithm presented in the publication: + * Grauman, K. & Darrell, T. + * The Pyramid Match Kernel: Discriminative Classification with Sets of Image Features + * Tenth IEEE International Conference on Computer Vision ICCV05 Volume 1 + * October 2005 + * + * \author Alexandru-Eugen Ichim + */ + template + class PyramidFeatureHistogram : public PCLBase + { + public: + using PCLBase::input_; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + using PyramidFeatureHistogramPtr = Ptr; + using FeatureRepresentationConstPtr = shared_ptr >; + + + /** \brief Empty constructor that instantiates the feature representation variable */ + PyramidFeatureHistogram (); + + /** \brief Method for setting the input dimension range parameter. + * \note Please check the PyramidHistogram class description for more details about this parameter. + */ + inline void + setInputDimensionRange (std::vector > &dimension_range_input) + { dimension_range_input_ = dimension_range_input; } + + /** \brief Method for retrieving the input dimension range vector */ + inline std::vector > + getInputDimensionRange () { return dimension_range_input_; } + + /** \brief Method to set the target dimension range parameter. + * \note Please check the PyramidHistogram class description for more details about this parameter. + */ + inline void + setTargetDimensionRange (std::vector > &dimension_range_target) + { dimension_range_target_ = dimension_range_target; } + + /** \brief Method for retrieving the target dimension range vector */ + inline std::vector > + getTargetDimensionRange () { return dimension_range_target_; } + + /** \brief Provide a pointer to the feature representation to use to convert features to k-D vectors. + * \param feature_representation the const boost shared pointer to a PointRepresentation + */ + inline void + setPointRepresentation (const FeatureRepresentationConstPtr& feature_representation) { feature_representation_ = feature_representation; } + + /** \brief Get a pointer to the feature representation used when converting features into k-D vectors. */ + inline FeatureRepresentationConstPtr const + getPointRepresentation () { return feature_representation_; } + + /** \brief The central method for inserting the feature set inside the pyramid and obtaining the complete pyramid */ + void + compute (); + + /** \brief Checks whether the pyramid histogram has been computed */ + inline bool + isComputed () { return is_computed_; } + + /** \brief Static method for comparing two pyramid histograms that returns a floating point value between 0 and 1, + * representing the similarity between the feature sets on which the two pyramid histograms are based. + * \param pyramid_a Pointer to the first pyramid to be compared (needs to be computed already). + * \param pyramid_b Pointer to the second pyramid to be compared (needs to be computed already). + */ + static float + comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a, + const PyramidFeatureHistogramPtr &pyramid_b); + + + private: + std::size_t nr_dimensions, nr_levels, nr_features; + std::vector > dimension_range_input_, dimension_range_target_; + FeatureRepresentationConstPtr feature_representation_; + bool is_computed_; + + /** \brief Checks for input inconsistencies and initializes the underlying data structures */ + bool + initializeHistogram (); + + /** \brief Converts a feature in templated form to an STL vector. This is the point where the conversion from the + * input dimension range to the target dimension range is done. + */ + void + convertFeatureToVector (const PointFeature &feature, + std::vector &feature_vector); + + /** \brief Adds a feature vector to its corresponding bin at each level in the pyramid */ + void + addFeature (std::vector &feature); + + /** \brief Access the pyramid bin given the position of the bin at the given pyramid level + * and the pyramid level + * \param access index of the bin at the respective level + * \param level the level in the pyramid + */ + inline unsigned int& + at (std::vector &access, + std::size_t &level); + + /** \brief Access the pyramid bin given a feature vector and the pyramid level + * \param feature the feature in vectorized form + * \param level the level in the pyramid + */ + inline unsigned int& + at (std::vector &feature, + std::size_t &level); + + /** \brief Structure for representing a single pyramid histogram level */ + struct PyramidFeatureHistogramLevel + { + PyramidFeatureHistogramLevel () + { + } + + PyramidFeatureHistogramLevel (std::vector &a_bins_per_dimension, std::vector &a_bin_step) : + bins_per_dimension (a_bins_per_dimension), + bin_step (a_bin_step) + { + initializeHistogramLevel (); + } + + void + initializeHistogramLevel (); + + std::vector hist; + std::vector bins_per_dimension; + std::vector bin_step; + }; + std::vector hist_levels; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/registration/registration.h b/deps_install/include/pcl-1.10/pcl/registration/registration.h new file mode 100755 index 0000000..cf8ede4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/registration.h @@ -0,0 +1,616 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +// PCL includes +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief @b Registration represents the base registration class for general purpose, ICP-like methods. + * \author Radu B. Rusu, Michael Dixon + * \ingroup registration + */ + template + class Registration : public PCLBase + { + public: + using Matrix4 = Eigen::Matrix; + + // using PCLBase::initCompute; + using PCLBase::deinitCompute; + using PCLBase::input_; + using PCLBase::indices_; + + using Ptr = shared_ptr< Registration >; + using ConstPtr = shared_ptr< const Registration >; + + using CorrespondenceRejectorPtr = pcl::registration::CorrespondenceRejector::Ptr; + using KdTree = pcl::search::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using KdTreeReciprocal = pcl::search::KdTree; + using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr; + + using TransformationEstimation = typename pcl::registration::TransformationEstimation; + using TransformationEstimationPtr = typename TransformationEstimation::Ptr; + using TransformationEstimationConstPtr = typename TransformationEstimation::ConstPtr; + + using CorrespondenceEstimation = pcl::registration::CorrespondenceEstimationBase; + using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr; + using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr; + + /** \brief The callback signature to the function updating intermediate source point cloud position + * during it's registration to the target point cloud. + * \param[in] cloud_src - the point cloud which will be updated to match target + * \param[in] indices_src - a selector of points in cloud_src + * \param[in] cloud_tgt - the point cloud we want to register against + * \param[in] indices_tgt - a selector of points in cloud_tgt + */ + using UpdateVisualizerCallbackSignature = void (const pcl::PointCloud&, + const std::vector&, + const pcl::PointCloud&, + const std::vector&); + + /** \brief Empty constructor. */ + Registration () + : tree_ (new KdTree) + , tree_reciprocal_ (new KdTreeReciprocal) + , nr_iterations_ (0) + , max_iterations_ (10) + , ransac_iterations_ (0) + , target_ () + , final_transformation_ (Matrix4::Identity ()) + , transformation_ (Matrix4::Identity ()) + , previous_transformation_ (Matrix4::Identity ()) + , transformation_epsilon_ (0.0) + , transformation_rotation_epsilon_(0.0) + , euclidean_fitness_epsilon_ (-std::numeric_limits::max ()) + , corr_dist_threshold_ (std::sqrt (std::numeric_limits::max ())) + , inlier_threshold_ (0.05) + , converged_ (false) + , min_number_correspondences_ (3) + , correspondences_ (new Correspondences) + , transformation_estimation_ () + , correspondence_estimation_ () + , target_cloud_updated_ (true) + , source_cloud_updated_ (true) + , force_no_recompute_ (false) + , force_no_recompute_reciprocal_ (false) + , point_representation_ () + { + } + + /** \brief destructor. */ + ~Registration () {} + + /** \brief Provide a pointer to the transformation estimation object. + * (e.g., SVD, point to plane etc.) + * + * \param[in] te is the pointer to the corresponding transformation estimation object + * + * Code example: + * + * \code + * TransformationEstimationPointToPlaneLLS::Ptr trans_lls (new TransformationEstimationPointToPlaneLLS); + * icp.setTransformationEstimation (trans_lls); + * // or... + * TransformationEstimationSVD::Ptr trans_svd (new TransformationEstimationSVD); + * icp.setTransformationEstimation (trans_svd); + * \endcode + */ + void + setTransformationEstimation (const TransformationEstimationPtr &te) { transformation_estimation_ = te; } + + /** \brief Provide a pointer to the correspondence estimation object. + * (e.g., regular, reciprocal, normal shooting etc.) + * + * \param[in] ce is the pointer to the corresponding correspondence estimation object + * + * Code example: + * + * \code + * CorrespondenceEstimation::Ptr ce (new CorrespondenceEstimation); + * ce->setInputSource (source); + * ce->setInputTarget (target); + * icp.setCorrespondenceEstimation (ce); + * // or... + * CorrespondenceEstimationNormalShooting::Ptr cens (new CorrespondenceEstimationNormalShooting); + * ce->setInputSource (source); + * ce->setInputTarget (target); + * ce->setSourceNormals (source); + * ce->setTargetNormals (target); + * icp.setCorrespondenceEstimation (cens); + * \endcode + */ + void + setCorrespondenceEstimation (const CorrespondenceEstimationPtr &ce) { correspondence_estimation_ = ce; } + + /** \brief Provide a pointer to the input source + * (e.g., the point cloud that we want to align to the target) + * + * \param[in] cloud the input point cloud source + */ + virtual void + setInputSource (const PointCloudSourceConstPtr &cloud) + { + source_cloud_updated_ = true; + PCLBase::setInputCloud (cloud); + } + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudSourceConstPtr const + getInputSource () { return (input_ ); } + + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) + * \param[in] cloud the input point cloud target + */ + virtual inline void + setInputTarget (const PointCloudTargetConstPtr &cloud); + + /** \brief Get a pointer to the input point cloud dataset target. */ + inline PointCloudTargetConstPtr const + getInputTarget () { return (target_ ); } + + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + inline void + setSearchMethodTarget (const KdTreePtr &tree, + bool force_no_recompute = false) + { + tree_ = tree; + if (force_no_recompute) + { + force_no_recompute_ = true; + } + // Since we just set a new tree, we need to check for updates + target_cloud_updated_ = true; + } + + /** \brief Get a pointer to the search method used to find correspondences in the + * target cloud. */ + inline KdTreePtr + getSearchMethodTarget () const + { + return (tree_); + } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the source cloud (usually used by reciprocal correspondence finding). + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputSource. Only use if you are + * extremely confident that the tree will be set correctly. + */ + inline void + setSearchMethodSource (const KdTreeReciprocalPtr &tree, + bool force_no_recompute = false) + { + tree_reciprocal_ = tree; + if ( force_no_recompute ) + { + force_no_recompute_reciprocal_ = true; + } + // Since we just set a new tree, we need to check for updates + source_cloud_updated_ = true; + } + + /** \brief Get a pointer to the search method used to find correspondences in the + * source cloud. */ + inline KdTreeReciprocalPtr + getSearchMethodSource () const + { + return (tree_reciprocal_); + } + + /** \brief Get the final transformation matrix estimated by the registration method. */ + inline Matrix4 + getFinalTransformation () { return (final_transformation_); } + + /** \brief Get the last incremental transformation matrix estimated by the registration method. */ + inline Matrix4 + getLastIncrementalTransformation () { return (transformation_); } + + /** \brief Set the maximum number of iterations the internal optimization should run for. + * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for + */ + inline void + setMaximumIterations (int nr_iterations) { max_iterations_ = nr_iterations; } + + /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */ + inline int + getMaximumIterations () { return (max_iterations_); } + + /** \brief Set the number of iterations RANSAC should run for. + * \param[in] ransac_iterations is the number of iterations RANSAC should run for + */ + inline void + setRANSACIterations (int ransac_iterations) { ransac_iterations_ = ransac_iterations; } + + /** \brief Get the number of iterations RANSAC should run for, as set by the user. */ + inline double + getRANSACIterations () { return (ransac_iterations_); } + + /** \brief Set the inlier distance threshold for the internal RANSAC outlier rejection loop. + * + * The method considers a point to be an inlier, if the distance between the target data index and the transformed + * source index is smaller than the given inlier distance threshold. + * The value is set by default to 0.05m. + * \param[in] inlier_threshold the inlier distance threshold for the internal RANSAC outlier rejection loop + */ + inline void + setRANSACOutlierRejectionThreshold (double inlier_threshold) { inlier_threshold_ = inlier_threshold; } + + /** \brief Get the inlier distance threshold for the internal outlier rejection loop as set by the user. */ + inline double + getRANSACOutlierRejectionThreshold () { return (inlier_threshold_); } + + /** \brief Set the maximum distance threshold between two correspondent points in source <-> target. If the + * distance is larger than this threshold, the points will be ignored in the alignment process. + * \param[in] distance_threshold the maximum distance threshold between a point and its nearest neighbor + * correspondent in order to be considered in the alignment process + */ + inline void + setMaxCorrespondenceDistance (double distance_threshold) { corr_dist_threshold_ = distance_threshold; } + + /** \brief Get the maximum distance threshold between two correspondent points in source <-> target. If the + * distance is larger than this threshold, the points will be ignored in the alignment process. + */ + inline double + getMaxCorrespondenceDistance () { return (corr_dist_threshold_); } + + /** \brief Set the transformation epsilon (maximum allowable translation squared difference between two consecutive + * transformations) in order for an optimization to be considered as having converged to the final + * solution. + * \param[in] epsilon the transformation epsilon in order for an optimization to be considered as having + * converged to the final solution. + */ + inline void + setTransformationEpsilon (double epsilon) { transformation_epsilon_ = epsilon; } + + /** \brief Get the transformation epsilon (maximum allowable translation squared difference between two consecutive + * transformations) as set by the user. + */ + inline double + getTransformationEpsilon () { return (transformation_epsilon_); } + + /** \brief Set the transformation rotation epsilon (maximum allowable rotation difference between two consecutive + * transformations) in order for an optimization to be considered as having converged to the final + * solution. + * \param[in] epsilon the transformation rotation epsilon in order for an optimization to be considered as having + * converged to the final solution (epsilon is the cos(angle) in a axis-angle representation). + */ + inline void + setTransformationRotationEpsilon (double epsilon) { transformation_rotation_epsilon_ = epsilon; } + + /** \brief Get the transformation rotation epsilon (maximum allowable difference between two consecutive + * transformations) as set by the user (epsilon is the cos(angle) in a axis-angle representation). + */ + inline double + getTransformationRotationEpsilon () { return (transformation_rotation_epsilon_); } + + /** \brief Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before + * the algorithm is considered to have converged. + * The error is estimated as the sum of the differences between correspondences in an Euclidean sense, + * divided by the number of correspondences. + * \param[in] epsilon the maximum allowed distance error before the algorithm will be considered to have + * converged + */ + inline void + setEuclideanFitnessEpsilon (double epsilon) { euclidean_fitness_epsilon_ = epsilon; } + + /** \brief Get the maximum allowed distance error before the algorithm will be considered to have converged, + * as set by the user. See \ref setEuclideanFitnessEpsilon + */ + inline double + getEuclideanFitnessEpsilon () { return (euclidean_fitness_epsilon_); } + + /** \brief Provide a boost shared pointer to the PointRepresentation to be used when comparing points + * \param[in] point_representation the PointRepresentation to be used by the k-D tree + */ + inline void + setPointRepresentation (const PointRepresentationConstPtr &point_representation) + { + point_representation_ = point_representation; + } + + /** \brief Register the user callback function which will be called from registration thread + * in order to update point cloud obtained after each iteration + * \param[in] visualizerCallback reference of the user callback function + */ + inline bool + registerVisualizationCallback (std::function &visualizerCallback) + { + if (visualizerCallback) + { + update_visualizer_ = visualizerCallback; + return (true); + } + return (false); + } + + /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target) + * \param[in] max_range maximum allowable distance between a point and its correspondence in the target + * (default: double::max) + */ + inline double + getFitnessScore (double max_range = std::numeric_limits::max ()); + + /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target) + * from two sets of correspondence distances (distances between source and target points) + * \param[in] distances_a the first set of distances between correspondences + * \param[in] distances_b the second set of distances between correspondences + */ + inline double + getFitnessScore (const std::vector &distances_a, const std::vector &distances_b); + + /** \brief Return the state of convergence after the last align run */ + inline bool + hasConverged () const { return (converged_); } + + /** \brief Call the registration algorithm which estimates the transformation and returns the transformed source + * (input) as \a output. + * \param[out] output the resultant input transformed point cloud dataset + */ + inline void + align (PointCloudSource &output); + + /** \brief Call the registration algorithm which estimates the transformation and returns the transformed source + * (input) as \a output. + * \param[out] output the resultant input transformed point cloud dataset + * \param[in] guess the initial gross estimation of the transformation + */ + inline void + align (PointCloudSource &output, const Matrix4& guess); + + /** \brief Abstract class get name method. */ + inline const std::string& + getClassName () const { return (reg_name_); } + + /** \brief Internal computation initialization. */ + bool + initCompute (); + + /** \brief Internal computation when reciprocal lookup is needed */ + bool + initComputeReciprocal (); + + /** \brief Add a new correspondence rejector to the list + * \param[in] rejector the new correspondence rejector to concatenate + * + * Code example: + * + * \code + * CorrespondenceRejectorDistance rej; + * rej.setInputCloud (keypoints_src); + * rej.setInputTarget (keypoints_tgt); + * rej.setMaximumDistance (1); + * rej.setInputCorrespondences (all_correspondences); + * + * // or... + * + * \endcode + */ + inline void + addCorrespondenceRejector (const CorrespondenceRejectorPtr &rejector) + { + correspondence_rejectors_.push_back (rejector); + } + + /** \brief Get the list of correspondence rejectors. */ + inline std::vector + getCorrespondenceRejectors () + { + return (correspondence_rejectors_); + } + + /** \brief Remove the i-th correspondence rejector in the list + * \param[in] i the position of the correspondence rejector in the list to remove + */ + inline bool + removeCorrespondenceRejector (unsigned int i) + { + if (i >= correspondence_rejectors_.size ()) + return (false); + correspondence_rejectors_.erase (correspondence_rejectors_.begin () + i); + return (true); + } + + /** \brief Clear the list of correspondence rejectors. */ + inline void + clearCorrespondenceRejectors () + { + correspondence_rejectors_.clear (); + } + + protected: + /** \brief The registration method name. */ + std::string reg_name_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief A pointer to the spatial search object of the source. */ + KdTreeReciprocalPtr tree_reciprocal_; + + /** \brief The number of iterations the internal optimization ran for (used internally). */ + int nr_iterations_; + + /** \brief The maximum number of iterations the internal optimization should run for. + * The default value is 10. + */ + int max_iterations_; + + /** \brief The number of iterations RANSAC should run for. */ + int ransac_iterations_; + + /** \brief The input point cloud dataset target. */ + PointCloudTargetConstPtr target_; + + /** \brief The final transformation matrix estimated by the registration method after N iterations. */ + Matrix4 final_transformation_; + + /** \brief The transformation matrix estimated by the registration method. */ + Matrix4 transformation_; + + /** \brief The previous transformation matrix estimated by the registration method (used internally). */ + Matrix4 previous_transformation_; + + /** \brief The maximum difference between two consecutive transformations in order to consider convergence + * (user defined). + */ + double transformation_epsilon_; + + /** \brief The maximum rotation difference between two consecutive transformations in order to consider convergence + * (user defined). + */ + double transformation_rotation_epsilon_; + + /** \brief The maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the + * algorithm is considered to have converged. The error is estimated as the sum of the differences between + * correspondences in an Euclidean sense, divided by the number of correspondences. + */ + double euclidean_fitness_epsilon_; + + /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the + * distance is larger than this threshold, the points will be ignored in the alignment process. + */ + double corr_dist_threshold_; + + /** \brief The inlier distance threshold for the internal RANSAC outlier rejection loop. + * The method considers a point to be an inlier, if the distance between the target data index and the transformed + * source index is smaller than the given inlier distance threshold. The default value is 0.05. + */ + double inlier_threshold_; + + /** \brief Holds internal convergence state, given user parameters. */ + bool converged_; + + /** \brief The minimum number of correspondences that the algorithm needs before attempting to estimate the + * transformation. The default value is 3. + */ + int min_number_correspondences_; + + /** \brief The set of correspondences determined at this ICP step. */ + CorrespondencesPtr correspondences_; + + /** \brief A TransformationEstimation object, used to calculate the 4x4 rigid transformation. */ + TransformationEstimationPtr transformation_estimation_; + + /** \brief A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. */ + CorrespondenceEstimationPtr correspondence_estimation_; + + /** \brief The list of correspondence rejectors to use. */ + std::vector correspondence_rejectors_; + + /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. + * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method + * is called. */ + bool target_cloud_updated_; + /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again. + * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method + * is called. */ + bool source_cloud_updated_; + /** \brief A flag which, if set, means the tree operating on the target cloud + * will never be recomputed*/ + bool force_no_recompute_; + + /** \brief A flag which, if set, means the tree operating on the source cloud + * will never be recomputed*/ + bool force_no_recompute_reciprocal_; + + /** \brief Callback function to update intermediate source point cloud position during it's registration + * to the target point cloud. + */ + std::function update_visualizer_; + + /** \brief Search for the closest nearest neighbor of a given point. + * \param cloud the point cloud dataset to use for nearest neighbor search + * \param index the index of the query point + * \param indices the resultant vector of indices representing the k-nearest neighbors + * \param distances the resultant distances from the query point to the k-nearest neighbors + */ + inline bool + searchForNeighbors (const PointCloudSource &cloud, int index, + std::vector &indices, std::vector &distances) + { + int k = tree_->nearestKSearch (cloud, index, 1, indices, distances); + if (k == 0) + return (false); + return (true); + } + + /** \brief Abstract transformation computation method with initial guess */ + virtual void + computeTransformation (PointCloudSource &output, const Matrix4& guess) = 0; + + private: + /** \brief The point representation used (internal). */ + PointRepresentationConstPtr point_representation_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/sample_consensus_prerejective.h b/deps_install/include/pcl-1.10/pcl/registration/sample_consensus_prerejective.h new file mode 100755 index 0000000..0f0a2ea --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/sample_consensus_prerejective.h @@ -0,0 +1,317 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Pose estimation and alignment class using a prerejective RANSAC routine. + * + * This class inserts a simple, yet effective "prerejection" step into the standard + * RANSAC pose estimation loop in order to avoid verification of pose hypotheses + * that are likely to be wrong. This is achieved by local pose-invariant geometric + * constraints, as also implemented in the class + * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly". + * + * In order to robustly align partial/occluded models, this routine performs + * fit error evaluation using only inliers, i.e. points closer than a + * Euclidean threshold, which is specifiable using \ref setInlierFraction(). + * + * The amount of prerejection or "greedyness" of the algorithm can be specified + * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled, + * and 1 is maximally rejective. + * + * If you use this in academic work, please cite: + * + * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger. + * Pose Estimation using Local Structure-Specific Shape and Appearance Context. + * International Conference on Robotics and Automation (ICRA), 2013. + * + * \author Anders Glent Buch (andersgb1@gmail.com) + * \ingroup registration + */ + template + class SampleConsensusPrerejective : public Registration + { + public: + using Matrix4 = typename Registration::Matrix4; + + using Registration::reg_name_; + using Registration::getClassName; + using Registration::input_; + using Registration::target_; + using Registration::tree_; + using Registration::max_iterations_; + using Registration::corr_dist_threshold_; + using Registration::transformation_; + using Registration::final_transformation_; + using Registration::transformation_estimation_; + using Registration::getFitnessScore; + using Registration::converged_; + + using PointCloudSource = typename Registration::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename Registration::PointCloudTarget; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + using FeatureCloud = pcl::PointCloud; + using FeatureCloudPtr = typename FeatureCloud::Ptr; + using FeatureCloudConstPtr = typename FeatureCloud::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using FeatureKdTreePtr = typename KdTreeFLANN::Ptr; + + using CorrespondenceRejectorPoly = pcl::registration::CorrespondenceRejectorPoly; + using CorrespondenceRejectorPolyPtr = typename CorrespondenceRejectorPoly::Ptr; + using CorrespondenceRejectorPolyConstPtr = typename CorrespondenceRejectorPoly::ConstPtr; + + /** \brief Constructor */ + SampleConsensusPrerejective () + : input_features_ () + , target_features_ () + , nr_samples_(3) + , k_correspondences_ (2) + , feature_tree_ (new pcl::KdTreeFLANN) + , correspondence_rejector_poly_ (new CorrespondenceRejectorPoly) + , inlier_fraction_ (0.0f) + { + reg_name_ = "SampleConsensusPrerejective"; + correspondence_rejector_poly_->setSimilarityThreshold (0.6f); + max_iterations_ = 5000; + transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD); + }; + + /** \brief Destructor */ + ~SampleConsensusPrerejective () + { + } + + /** \brief Provide a boost shared pointer to the source point cloud's feature descriptors + * \param features the source point cloud's features + */ + void + setSourceFeatures (const FeatureCloudConstPtr &features); + + /** \brief Get a pointer to the source point cloud's features */ + inline const FeatureCloudConstPtr + getSourceFeatures () const + { + return (input_features_); + } + + /** \brief Provide a boost shared pointer to the target point cloud's feature descriptors + * \param features the target point cloud's features + */ + void + setTargetFeatures (const FeatureCloudConstPtr &features); + + /** \brief Get a pointer to the target point cloud's features */ + inline const FeatureCloudConstPtr + getTargetFeatures () const + { + return (target_features_); + } + + /** \brief Set the number of samples to use during each iteration + * \param nr_samples the number of samples to use during each iteration + */ + inline void + setNumberOfSamples (int nr_samples) + { + nr_samples_ = nr_samples; + } + + /** \brief Get the number of samples to use during each iteration, as set by the user */ + inline int + getNumberOfSamples () const + { + return (nr_samples_); + } + + /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will + * add more randomness to the feature matching. + * \param k the number of neighbors to use when selecting a random feature correspondence. + */ + inline void + setCorrespondenceRandomness (int k) + { + k_correspondences_ = k; + } + + /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */ + inline int + getCorrespondenceRandomness () const + { + return (k_correspondences_); + } + + /** \brief Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object, + * where 1 is a perfect match + * \param similarity_threshold edge length similarity threshold + */ + inline void + setSimilarityThreshold (float similarity_threshold) + { + correspondence_rejector_poly_->setSimilarityThreshold (similarity_threshold); + } + + /** \brief Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object, + * \return edge length similarity threshold + */ + inline float + getSimilarityThreshold () const + { + return correspondence_rejector_poly_->getSimilarityThreshold (); + } + + /** \brief Set the required inlier fraction (of the input) + * \param inlier_fraction required inlier fraction, must be in [0,1] + */ + inline void + setInlierFraction (float inlier_fraction) + { + inlier_fraction_ = inlier_fraction; + } + + /** \brief Get the required inlier fraction + * \return required inlier fraction in [0,1] + */ + inline float + getInlierFraction () const + { + return inlier_fraction_; + } + + /** \brief Get the inlier indices of the source point cloud under the final transformation + * @return inlier indices + */ + inline const std::vector& + getInliers () const + { + return inliers_; + } + + protected: + /** \brief Choose a random index between 0 and n-1 + * \param n the number of possible indices to choose from + */ + inline int + getRandomIndex (int n) const + { + return (static_cast (n * (rand () / (RAND_MAX + 1.0)))); + }; + + /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are + * greater than a user-defined minimum distance, \a min_sample_distance. + * \param cloud the input point cloud + * \param nr_samples the number of samples to select + * \param sample_indices the resulting sample indices + */ + void + selectSamples (const PointCloudSource &cloud, int nr_samples, std::vector &sample_indices); + + /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to + * the sample points' features. From these, select one randomly which will be considered that sample point's + * correspondence. + * \param sample_indices the indices of each sample point + * \param similar_features correspondence cache, which is used to read/write already computed correspondences + * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud + */ + void + findSimilarFeatures (const std::vector &sample_indices, + std::vector >& similar_features, + std::vector &corresponding_indices); + + /** \brief Rigid transformation computation method. + * \param output the transformed input point cloud dataset using the rigid transformation found + * \param guess The computed transformation + */ + void + computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override; + + /** \brief Obtain the fitness of a transformation + * The following metrics are calculated, based on + * \b final_transformation_ and \b corr_dist_threshold_: + * - Inliers: the number of transformed points which are closer than threshold to NN + * - Error score: the MSE of the inliers + * \param inliers indices of source point cloud inliers + * \param fitness_score output fitness score as RMSE + */ + void + getFitness (std::vector& inliers, float& fitness_score); + + /** \brief The source point cloud's feature descriptors. */ + FeatureCloudConstPtr input_features_; + + /** \brief The target point cloud's feature descriptors. */ + FeatureCloudConstPtr target_features_; + + /** \brief The number of samples to use during each iteration. */ + int nr_samples_; + + /** \brief The number of neighbors to use when selecting a random feature correspondence. */ + int k_correspondences_; + + /** \brief The KdTree used to compare feature descriptors. */ + FeatureKdTreePtr feature_tree_; + + /** \brief The polygonal correspondence rejector used for prerejection */ + CorrespondenceRejectorPolyPtr correspondence_rejector_poly_; + + /** \brief The fraction [0,1] of inlier points required for accepting a transformation */ + float inlier_fraction_; + + /** \brief Inlier points of final transformation as indices into source */ + std::vector inliers_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation.h new file mode 100755 index 0000000..1411444 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation.h @@ -0,0 +1,127 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief TransformationEstimation represents the base class for methods for transformation estimation based on: + * - correspondence vectors + * - two point clouds (source and target) of the same size + * - a point cloud with a set of indices (source), and another point cloud (target) + * - two point clouds with two sets of indices (source and target) of the same size + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Dirk Holz, Radu B. Rusu + * \ingroup registration + */ + template + class TransformationEstimation + { + public: + using Matrix4 = Eigen::Matrix; + + TransformationEstimation () {}; + virtual ~TransformationEstimation () {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const = 0; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const = 0; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const = 0; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const = 0; + + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_2D.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_2D.h new file mode 100755 index 0000000..3ced4e0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_2D.h @@ -0,0 +1,153 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** @b TransformationEstimation2D implements a simple 2D rigid transformation + * estimation (x, y, theta) for a given pair of datasets. + * + * The two datasets should already be transformed so that the reference plane + * equals z = 0. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * + * \author Suat Gedikli + * \ingroup registration + */ + template + class TransformationEstimation2D : public TransformationEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = typename TransformationEstimation::Matrix4; + + TransformationEstimation2D () {}; + virtual ~TransformationEstimation2D () {}; + + /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + virtual void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const; + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const; + + /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt' + * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format + * \param[in] centroid_src the input source centroid, in Eigen format + * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format + * \param[in] centroid_tgt the input target cloud, in Eigen format + * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix + */ + void + getTransformationFromCorrelation ( + const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, + Matrix4 &transformation_matrix) const; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_3point.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_3point.h new file mode 100755 index 0000000..f9b8029 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_3point.h @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** \brief TransformationEstimation3Points represents the class for transformation estimation based on: + * - correspondence vectors of 3 pairs (planar case) + * - two point clouds (source and target) of size 3 + * - a point cloud with a set of 3 indices (source), and another point cloud (target) + * - two point clouds with two sets of indices (source and target) of the size 3 + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix + * (i.e., float or double). Default: float. + * + * \author P.W.Theiler + * \ingroup registration + */ + template + class TransformationEstimation3Point : public TransformationEstimation + { + public: + /** \cond */ + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using Matrix4 = typename TransformationEstimation::Matrix4; + /** \endcond */ + + /** \brief Constructor */ + TransformationEstimation3Point () + {}; + + /** \brief Destructor */ + ~TransformationEstimation3Point () + {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const override; + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const; + }; + }; // namespace registration +}; // namespace registration + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_dual_quaternion.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_dual_quaternion.h new file mode 100755 index 0000000..1da67c8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_dual_quaternion.h @@ -0,0 +1,140 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** @b TransformationEstimationDualQuaternion implements dual quaternion based estimation of + * the transformation aligning the given correspondences. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Sergey Zagoruyko + * \ingroup registration + */ + template + class TransformationEstimationDualQuaternion : public TransformationEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = typename TransformationEstimation::Matrix4; + + TransformationEstimationDualQuaternion () {}; + ~TransformationEstimationDualQuaternion () {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using + * dual quaternion optimization + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using + * dual quaternion optimization + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using + * dual quaternion optimization + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using + * dual quaternion optimization + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const override; + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_lm.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_lm.h new file mode 100755 index 0000000..21c29a8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_lm.h @@ -0,0 +1,354 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** @b TransformationEstimationLM implements Levenberg Marquardt-based + * estimation of the transformation aligning the given correspondences. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Radu B. Rusu + * \ingroup registration + */ + template + class TransformationEstimationLM : public TransformationEstimation + { + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using VectorX = Eigen::Matrix; + using Vector4 = Eigen::Matrix; + using Matrix4 = typename TransformationEstimation::Matrix4; + + /** \brief Constructor. */ + TransformationEstimationLM (); + + /** \brief Copy constructor. + * \param[in] src the TransformationEstimationLM object to copy into this + */ + TransformationEstimationLM (const TransformationEstimationLM &src) : + tmp_src_ (src.tmp_src_), + tmp_tgt_ (src.tmp_tgt_), + tmp_idx_src_ (src.tmp_idx_src_), + tmp_idx_tgt_ (src.tmp_idx_tgt_), + warp_point_ (src.warp_point_) + {}; + + /** \brief Copy operator. + * \param[in] src the TransformationEstimationLM object to copy into this + */ + TransformationEstimationLM& + operator = (const TransformationEstimationLM &src) + { + tmp_src_ = src.tmp_src_; + tmp_tgt_ = src.tmp_tgt_; + tmp_idx_src_ = src.tmp_idx_src_; + tmp_idx_tgt_ = src.tmp_idx_tgt_; + warp_point_ = src.warp_point_; + } + + /** \brief Destructor. */ + ~TransformationEstimationLM () {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from + * \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const override; + + /** \brief Set the function we use to warp points. Defaults to rigid 6D warp. + * \param[in] warp_fcn a shared pointer to an object that warps points + */ + void + setWarpFunction (const typename WarpPointRigid::Ptr &warp_fcn) + { + warp_point_ = warp_fcn; + } + + protected: + /** \brief Compute the distance between a source point and its corresponding target point + * \param[in] p_src The source point + * \param[in] p_tgt The target point + * \return The distance between \a p_src and \a p_tgt + * + * \note Older versions of PCL used this method internally for calculating the + * optimization gradient. Since PCL 1.7, a switch has been made to the + * computeDistance method using Vector4 types instead. This method is only + * kept for API compatibility reasons. + */ + virtual MatScalar + computeDistance (const PointSource &p_src, const PointTarget &p_tgt) const + { + Vector4 s (p_src.x, p_src.y, p_src.z, 0); + Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0); + return ((s - t).norm ()); + } + + /** \brief Compute the distance between a source point and its corresponding target point + * \param[in] p_src The source point + * \param[in] p_tgt The target point + * \return The distance between \a p_src and \a p_tgt + * + * \note A different distance function can be defined by creating a subclass of + * TransformationEstimationLM and overriding this method. + * (See \a TransformationEstimationPointToPlane) + */ + virtual MatScalar + computeDistance (const Vector4 &p_src, const PointTarget &p_tgt) const + { + Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0); + return ((p_src - t).norm ()); + } + + /** \brief Temporary pointer to the source dataset. */ + mutable const PointCloudSource *tmp_src_; + + /** \brief Temporary pointer to the target dataset. */ + mutable const PointCloudTarget *tmp_tgt_; + + /** \brief Temporary pointer to the source dataset indices. */ + mutable const std::vector *tmp_idx_src_; + + /** \brief Temporary pointer to the target dataset indices. */ + mutable const std::vector *tmp_idx_tgt_; + + /** \brief The parameterized function used to warp the source to the target. */ + typename pcl::registration::WarpPointRigid::Ptr warp_point_; + + /** Base functor all the models that need non linear optimization must + * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec) + * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar + */ + template + struct Functor + { + using Scalar = _Scalar; + enum + { + InputsAtCompileTime = NX, + ValuesAtCompileTime = NY + }; + using InputType = Eigen::Matrix<_Scalar,InputsAtCompileTime,1>; + using ValueType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,1>; + using JacobianType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime>; + + /** \brief Empty Constructor. */ + Functor () : m_data_points_ (ValuesAtCompileTime) {} + + /** \brief Constructor + * \param[in] m_data_points number of data points to evaluate. + */ + Functor (int m_data_points) : m_data_points_ (m_data_points) {} + + /** \brief Destructor. */ + virtual ~Functor () {} + + /** \brief Get the number of values. */ + int + values () const { return (m_data_points_); } + + protected: + int m_data_points_; + }; + + struct OptimizationFunctor : public Functor + { + using Functor::values; + + /** Functor constructor + * \param[in] m_data_points the number of data points to evaluate + * \param[in,out] estimator pointer to the estimator object + */ + OptimizationFunctor (int m_data_points, + const TransformationEstimationLM *estimator) + : Functor (m_data_points), estimator_ (estimator) + {} + + /** Copy constructor + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctor (const OptimizationFunctor &src) : + Functor (src.m_data_points_), estimator_ () + { + *this = src; + } + + /** Copy operator + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctor& + operator = (const OptimizationFunctor &src) + { + Functor::operator=(src); + estimator_ = src.estimator_; + return (*this); + } + + /** \brief Destructor. */ + ~OptimizationFunctor () {} + + /** Fill fvec from x. For the current state vector x fill the f values + * \param[in] x state vector + * \param[out] fvec f values vector + */ + int + operator () (const VectorX &x, VectorX &fvec) const; + + const TransformationEstimationLM *estimator_; + }; + + struct OptimizationFunctorWithIndices : public Functor + { + using Functor::values; + + /** Functor constructor + * \param[in] m_data_points the number of data points to evaluate + * \param[in,out] estimator pointer to the estimator object + */ + OptimizationFunctorWithIndices (int m_data_points, + const TransformationEstimationLM *estimator) + : Functor (m_data_points), estimator_ (estimator) + {} + + /** Copy constructor + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src) + : Functor (src.m_data_points_), estimator_ () + { + *this = src; + } + + /** Copy operator + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctorWithIndices& + operator = (const OptimizationFunctorWithIndices &src) + { + Functor::operator=(src); + estimator_ = src.estimator_; + return (*this); + } + + /** \brief Destructor. */ + ~OptimizationFunctorWithIndices () {} + + /** Fill fvec from x. For the current state vector x fill the f values + * \param[in] x state vector + * \param[out] fvec f values vector + */ + int + operator () (const VectorX &x, VectorX &fvec) const; + + const TransformationEstimationLM *estimator_; + }; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane.h new file mode 100755 index 0000000..3d26df7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane.h @@ -0,0 +1,98 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** @b TransformationEstimationPointToPlane uses Levenberg Marquardt optimization to find the + * transformation that minimizes the point-to-plane distance between the given correspondences. + * + * \author Michael Dixon + * \ingroup registration + */ + template + class TransformationEstimationPointToPlane : public TransformationEstimationLM + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + using PointCloudTarget = pcl::PointCloud; + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + using Vector4 = Eigen::Matrix; + + TransformationEstimationPointToPlane () {}; + ~TransformationEstimationPointToPlane () {}; + + protected: + Scalar + computeDistance (const PointSource &p_src, const PointTarget &p_tgt) const override + { + // Compute the point-to-plane distance + Vector4 s (p_src.x, p_src.y, p_src.z, 0); + Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0); + Vector4 n (p_tgt.normal_x, p_tgt.normal_y, p_tgt.normal_z, 0); + return ((s - t).dot (n)); + } + + Scalar + computeDistance (const Vector4 &p_src, const PointTarget &p_tgt) const override + { + // Compute the point-to-plane distance + Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0); + Vector4 n (p_tgt.normal_x, p_tgt.normal_y, p_tgt.normal_z, 0); + return ((p_src - t).dot (n)); + } + + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_lls.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_lls.h new file mode 100755 index 0000000..0733eb1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_lls.h @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b TransformationEstimationPointToPlaneLLS implements a Linear Least Squares (LLS) approximation + * for minimizing the point-to-plane distance between two clouds of corresponding points with normals. + * + * For additional details, see + * "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004 + * + * \note The class is templated on the source and target point types as well as on the output scalar of the + * transformation matrix (i.e., float or double). Default: float. + * \author Michael Dixon + * \ingroup registration + */ + template + class TransformationEstimationPointToPlaneLLS : public TransformationEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = typename TransformationEstimation::Matrix4; + + TransformationEstimationPointToPlaneLLS () {}; + ~TransformationEstimationPointToPlaneLLS () {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const override; + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const; + + /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation. + * \param[in] alpha the rotation about the x-axis + * \param[in] beta the rotation about the y-axis + * \param[in] gamma the rotation about the z-axis + * \param[in] tx the x translation + * \param[in] ty the y translation + * \param[in] tz the z translation + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, + const double & tx, const double & ty, const double & tz, + Matrix4 &transformation_matrix) const; + + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h new file mode 100755 index 0000000..0229f9e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h @@ -0,0 +1,167 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b TransformationEstimationPointToPlaneLLSWeighted implements a Linear Least Squares (LLS) approximation + * for minimizing the point-to-plane distance between two clouds of corresponding points with normals, with the + * possibility of assigning weights to the correspondences. + * + * For additional details, see + * "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004 + * + * \note The class is templated on the source and target point types as well as on the output scalar of the + * transformation matrix (i.e., float or double). Default: float. + * \author Alex Ichim + * \ingroup registration + */ + template + class TransformationEstimationPointToPlaneLLSWeighted : public TransformationEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = typename TransformationEstimation::Matrix4; + + TransformationEstimationPointToPlaneLLSWeighted () { }; + virtual ~TransformationEstimationPointToPlaneLLSWeighted () { }; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const; + + + /** \brief Set the weights for the correspondences. + * \param[in] weights the weights for each correspondence + */ + inline void + setCorrespondenceWeights (const std::vector &weights) + { weights_ = weights; } + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param weights_it + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + typename std::vector::const_iterator& weights_it, + Matrix4 &transformation_matrix) const; + + /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation. + * \param[in] alpha the rotation about the x-axis + * \param[in] beta the rotation about the y-axis + * \param[in] gamma the rotation about the z-axis + * \param[in] tx the x translation + * \param[in] ty the y translation + * \param[in] tz the z translation + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, + const double & tx, const double & ty, const double & tz, + Matrix4 &transformation_matrix) const; + + std::vector weights_; + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_weighted.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_weighted.h new file mode 100755 index 0000000..165812b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_point_to_plane_weighted.h @@ -0,0 +1,337 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) Alexandru-Eugen Ichim + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** @b TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transformation + * that minimizes the point-to-plane distance between the given correspondences. In addition to the + * TransformationEstimationPointToPlane class, this version takes per-correspondence weights and optimizes accordingly. + * + * \author Alexandru-Eugen Ichim + * \ingroup registration + */ + template + class TransformationEstimationPointToPlaneWeighted : public TransformationEstimationPointToPlane + { + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using VectorX = Eigen::Matrix; + using Vector4 = Eigen::Matrix; + using Matrix4 = typename TransformationEstimation::Matrix4; + + /** \brief Constructor. */ + TransformationEstimationPointToPlaneWeighted (); + + /** \brief Copy constructor. + * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this + */ + TransformationEstimationPointToPlaneWeighted (const TransformationEstimationPointToPlaneWeighted &src) : + tmp_src_ (src.tmp_src_), + tmp_tgt_ (src.tmp_tgt_), + tmp_idx_src_ (src.tmp_idx_src_), + tmp_idx_tgt_ (src.tmp_idx_tgt_), + warp_point_ (src.warp_point_), + correspondence_weights_ (src.correspondence_weights_), + use_correspondence_weights_ (src.use_correspondence_weights_) + {}; + + /** \brief Copy operator. + * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this + */ + TransformationEstimationPointToPlaneWeighted& + operator = (const TransformationEstimationPointToPlaneWeighted &src) + { + tmp_src_ = src.tmp_src_; + tmp_tgt_ = src.tmp_tgt_; + tmp_idx_src_ = src.tmp_idx_src_; + tmp_idx_tgt_ = src.tmp_idx_tgt_; + warp_point_ = src.warp_point_; + correspondence_weights_ = src.correspondence_weights_; + use_correspondence_weights_ = src.use_correspondence_weights_; + } + + /** \brief Destructor. */ + virtual ~TransformationEstimationPointToPlaneWeighted () {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + * \note Uses the weights given by setWeights. + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + * \note Uses the weights given by setWeights. + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from + * \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + * \note Uses the weights given by setWeights. + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + * \note Uses the weights given by setWeights. + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const; + + + inline void + setWeights (const std::vector &weights) + { correspondence_weights_ = weights; } + + /// use the weights given in the pcl::CorrespondencesPtr for one of the estimateTransformation (...) methods + inline void + setUseCorrespondenceWeights (bool use_correspondence_weights) + { use_correspondence_weights_ = use_correspondence_weights; } + + /** \brief Set the function we use to warp points. Defaults to rigid 6D warp. + * \param[in] warp_fcn a shared pointer to an object that warps points + */ + void + setWarpFunction (const typename WarpPointRigid::Ptr &warp_fcn) + { warp_point_ = warp_fcn; } + + protected: + bool use_correspondence_weights_; + mutable std::vector correspondence_weights_; + + /** \brief Temporary pointer to the source dataset. */ + mutable const PointCloudSource *tmp_src_; + + /** \brief Temporary pointer to the target dataset. */ + mutable const PointCloudTarget *tmp_tgt_; + + /** \brief Temporary pointer to the source dataset indices. */ + mutable const std::vector *tmp_idx_src_; + + /** \brief Temporary pointer to the target dataset indices. */ + mutable const std::vector *tmp_idx_tgt_; + + /** \brief The parameterized function used to warp the source to the target. */ + typename pcl::registration::WarpPointRigid::Ptr warp_point_; + + /** Base functor all the models that need non linear optimization must + * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec) + * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar + */ + template + struct Functor + { + using Scalar = _Scalar; + enum + { + InputsAtCompileTime = NX, + ValuesAtCompileTime = NY + }; + using InputType = Eigen::Matrix<_Scalar,InputsAtCompileTime,1>; + using ValueType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,1>; + using JacobianType = Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime>; + + /** \brief Empty Constructor. */ + Functor () : m_data_points_ (ValuesAtCompileTime) {} + + /** \brief Constructor + * \param[in] m_data_points number of data points to evaluate. + */ + Functor (int m_data_points) : m_data_points_ (m_data_points) {} + + /** \brief Destructor. */ + virtual ~Functor () {} + + /** \brief Get the number of values. */ + int + values () const { return (m_data_points_); } + + protected: + int m_data_points_; + }; + + struct OptimizationFunctor : public Functor + { + using Functor::values; + + /** Functor constructor + * \param[in] m_data_points the number of data points to evaluate + * \param[in,out] estimator pointer to the estimator object + */ + OptimizationFunctor (int m_data_points, + const TransformationEstimationPointToPlaneWeighted *estimator) + : Functor (m_data_points), estimator_ (estimator) + {} + + /** Copy constructor + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctor (const OptimizationFunctor &src) : + Functor (src.m_data_points_), estimator_ () + { + *this = src; + } + + /** Copy operator + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctor& + operator = (const OptimizationFunctor &src) + { + Functor::operator=(src); + estimator_ = src.estimator_; + return (*this); + } + + /** \brief Destructor. */ + virtual ~OptimizationFunctor () {} + + /** Fill fvec from x. For the current state vector x fill the f values + * \param[in] x state vector + * \param[out] fvec f values vector + */ + int + operator () (const VectorX &x, VectorX &fvec) const; + + const TransformationEstimationPointToPlaneWeighted *estimator_; + }; + + struct OptimizationFunctorWithIndices : public Functor + { + using Functor::values; + + /** Functor constructor + * \param[in] m_data_points the number of data points to evaluate + * \param[in,out] estimator pointer to the estimator object + */ + OptimizationFunctorWithIndices (int m_data_points, + const TransformationEstimationPointToPlaneWeighted *estimator) + : Functor (m_data_points), estimator_ (estimator) + {} + + /** Copy constructor + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src) + : Functor (src.m_data_points_), estimator_ () + { + *this = src; + } + + /** Copy operator + * \param[in] src the optimization functor to copy into this + */ + inline OptimizationFunctorWithIndices& + operator = (const OptimizationFunctorWithIndices &src) + { + Functor::operator=(src); + estimator_ = src.estimator_; + return (*this); + } + + /** \brief Destructor. */ + virtual ~OptimizationFunctorWithIndices () {} + + /** Fill fvec from x. For the current state vector x fill the f values + * \param[in] x state vector + * \param[out] fvec f values vector + */ + int + operator () (const VectorX &x, VectorX &fvec) const; + + const TransformationEstimationPointToPlaneWeighted *estimator_; + }; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_svd.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_svd.h new file mode 100755 index 0000000..60150d4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_svd.h @@ -0,0 +1,159 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** @b TransformationEstimationSVD implements SVD-based estimation of + * the transformation aligning the given correspondences. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Dirk Holz, Radu B. Rusu + * \ingroup registration + */ + template + class TransformationEstimationSVD : public TransformationEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = typename TransformationEstimation::Matrix4; + + /** \brief Constructor + * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/ + TransformationEstimationSVD (bool use_umeyama=true): + use_umeyama_ (use_umeyama) + {} + + ~TransformationEstimationSVD () {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const override; + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const; + + /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt' + * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format + * \param[in] centroid_src the input source centroid, in Eigen format + * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format + * \param[in] centroid_tgt the input target cloud, in Eigen format + * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix + */ + virtual void + getTransformationFromCorrelation ( + const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, + Matrix4 &transformation_matrix) const; + + bool use_umeyama_; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_svd_scale.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_svd_scale.h new file mode 100755 index 0000000..1df9a9f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_svd_scale.h @@ -0,0 +1,90 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** @b TransformationEstimationSVD implements SVD-based estimation of + * the transformation aligning the given correspondences. + * Optionally the scale is estimated. Note that the similarity transform might not be optimal for the underlying Frobenius Norm. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Suat Gedikli + * \ingroup registration + */ + template + class TransformationEstimationSVDScale : public TransformationEstimationSVD + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = typename TransformationEstimationSVD::Matrix4; + + /** \brief Inherits from TransformationEstimationSVD, but forces it to not use the Umeyama method */ + TransformationEstimationSVDScale (): + TransformationEstimationSVD (false) + {} + + protected: + /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt' + * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format + * \param[in] centroid_src the input source centroid, in Eigen format + * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format + * \param[in] centroid_tgt the input target cloud, in Eigen format + * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix + */ + void + getTransformationFromCorrelation (const Eigen::Matrix &cloud_src_demean, + const Eigen::Matrix ¢roid_src, + const Eigen::Matrix &cloud_tgt_demean, + const Eigen::Matrix ¢roid_tgt, + Matrix4 &transformation_matrix) const; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h new file mode 100755 index 0000000..8db6f18 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2019-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b TransformationEstimationSymmetricPointToPlaneLLS implements a Linear Least Squares (LLS) approximation + * for minimizing the symmetric point-to-plane distance between two clouds of corresponding points with normals. + * + * For additional details, see + * "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004 + * "A Symmetric Objective Function for ICP", Szymon Rusinkiewicz, 2019 + * + * \note The class is templated on the source and target point types as well as on the output scalar of the + * transformation matrix (i.e., float or double). Default: float. + * \author Matthew Cong + * \ingroup registration + */ + template + class TransformationEstimationSymmetricPointToPlaneLLS : public TransformationEstimation + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using Matrix4 = typename TransformationEstimation::Matrix4; + using Vector6 = Eigen::Matrix; + + TransformationEstimationSymmetricPointToPlaneLLS () : enforce_same_direction_normals_ (true) {}; + ~TransformationEstimationSymmetricPointToPlaneLLS () {}; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Matrix4 &transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation ( + const pcl::PointCloud &cloud_src, + const pcl::PointCloud &cloud_tgt, + const pcl::Correspondences &correspondences, + Matrix4 &transformation_matrix) const override; + + /** \brief Set whether or not to negate source or target normals on a per-point basis such that they point in the same direction. + * \param[in] enforce_same_direction_normals whether to negate source or target normals on a per-point basis such that they point in the same direction. + */ + inline void + setEnforceSameDirectionNormals (bool enforce_same_direction_normals); + + /** \brief Obtain whether source or target normals are negated on a per-point basis such that they point in the same direction or not */ + inline bool + getEnforceSameDirectionNormals (); + + protected: + + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4 &transformation_matrix) const; + + /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation. + * \param[in] (alpha, beta, gamma, tx, ty, tz) specifying rotation about the x, y, and z-axis and translation along the the x, y, and z-axis respectively + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + constructTransformationMatrix (const Vector6 ¶meters, + Matrix4 &transformation_matrix) const; + + /** \brief Whether or not to negate source and/or target normals such that they point in the same direction */ + bool enforce_same_direction_normals_; + + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_validation.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_validation.h new file mode 100755 index 0000000..586473c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_validation.h @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief TransformationValidation represents the base class for methods + * that validate the correctness of a transformation found through \ref TransformationEstimation. + * + * The inputs for a validation estimation can take any or all of the following: + * + * - source point cloud + * - target point cloud + * - estimated transformation between source and target + * + * The output is in the form of a score or a confidence measure. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Radu B. Rusu + * \ingroup registration + */ + template + class TransformationValidation + { + public: + using Matrix4 = Eigen::Matrix; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudSource = pcl::PointCloud; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = pcl::PointCloud; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + + TransformationValidation () {}; + virtual ~TransformationValidation () {}; + + /** \brief Validate the given transformation with respect to the input cloud data, and return a score. Pure virtual. + * + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the transformation matrix + * + * \return the score or confidence measure for the given + * transformation_matrix with respect to the input data + */ + virtual double + validateTransformation ( + const PointCloudSourceConstPtr &cloud_src, + const PointCloudTargetConstPtr &cloud_tgt, + const Matrix4 &transformation_matrix) const = 0; + + /** \brief Comparator function for deciding which score is better after running the + * validation on multiple transforms. Pure virtual. + * + * \note For example, for Euclidean distances smaller is better, for inliers the opposite. + * + * \param[in] score1 the first value + * \param[in] score2 the second value + * + * \return true if score1 is better than score2 + */ + virtual bool + operator() (const double &score1, const double &score2) const = 0; + + /** \brief Check if the score is valid for a specific transformation. Pure virtual. + * + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the transformation matrix + * + * \return true if the transformation is valid, false otherwise. + */ + virtual bool + isValid ( + const PointCloudSourceConstPtr &cloud_src, + const PointCloudTargetConstPtr &cloud_tgt, + const Matrix4 &transformation_matrix) const = 0; + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/transformation_validation_euclidean.h b/deps_install/include/pcl-1.10/pcl/registration/transformation_validation_euclidean.h new file mode 100755 index 0000000..838f604 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transformation_validation_euclidean.h @@ -0,0 +1,267 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief TransformationValidationEuclidean computes an L2SQR norm between a source and target + * dataset. + * + * To prevent points with bad correspondences to contribute to the overall score, the class also + * accepts a maximum_range parameter given via \ref setMaxRange that is used as a cutoff value for + * nearest neighbor distance comparisons. + * + * The output score is normalized with respect to the number of valid correspondences found. + * + * Usage example: + * \code + * pcl::TransformationValidationEuclidean tve; + * tve.setMaxRange (0.01); // 1cm + * double score = tve.validateTransformation (source, target, transformation); + * \endcode + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Radu B. Rusu + * \ingroup registration + */ + template + class TransformationValidationEuclidean + { + public: + using Matrix4 = typename TransformationValidation::Matrix4; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using KdTree = pcl::search::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr; + + using PointCloudSourceConstPtr = typename TransformationValidation::PointCloudSourceConstPtr; + using PointCloudTargetConstPtr = typename TransformationValidation::PointCloudTargetConstPtr; + + /** \brief Constructor. + * Sets the \a max_range parameter to double::max, \a threshold_ to NaN + * and initializes the internal search \a tree to a FLANN kd-tree. + */ + TransformationValidationEuclidean () : + max_range_ (std::numeric_limits::max ()), + threshold_ (std::numeric_limits::quiet_NaN ()), + tree_ (new pcl::search::KdTree), + force_no_recompute_ (false) + { + } + + virtual ~TransformationValidationEuclidean () {}; + + /** \brief Set the maximum allowable distance between a point and its correspondence in the + * target in order for a correspondence to be considered \a valid. Default: double::max. + * \param[in] max_range the new maximum allowable distance + */ + inline void + setMaxRange (double max_range) + { + max_range_ = max_range; + } + + /** \brief Get the maximum allowable distance between a point and its + * correspondence, as set by the user. + */ + inline double + getMaxRange () + { + return (max_range_); + } + + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + inline void + setSearchMethodTarget (const KdTreePtr &tree, + bool force_no_recompute = false) + { + tree_ = tree; + if (force_no_recompute) + { + force_no_recompute_ = true; + } + } + + /** \brief Set a threshold for which a specific transformation is considered valid. + * + * \note Since we're using MSE (Mean Squared Error) as a metric, the threshold + * represents the mean Euclidean distance threshold over all nearest neighbors + * up to max_range. + * + * \param[in] threshold the threshold for which a transformation is vali + */ + inline void + setThreshold (double threshold) + { + threshold_ = threshold; + } + + /** \brief Get the threshold for which a specific transformation is valid. */ + inline double + getThreshold () + { + return (threshold_); + } + + /** \brief Validate the given transformation with respect to the input cloud data, and return a score. + * + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + * + * \return the score or confidence measure for the given + * transformation_matrix with respect to the input data + */ + double + validateTransformation ( + const PointCloudSourceConstPtr &cloud_src, + const PointCloudTargetConstPtr &cloud_tgt, + const Matrix4 &transformation_matrix) const; + + /** \brief Comparator function for deciding which score is better after running the + * validation on multiple transforms. + * + * \param[in] score1 the first value + * \param[in] score2 the second value + * + * \return true if score1 is better than score2 + */ + virtual bool + operator() (const double &score1, const double &score2) const + { + return (score1 < score2); + } + + /** \brief Check if the score is valid for a specific transformation. + * + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the transformation matrix + * + * \return true if the transformation is valid, false otherwise. + */ + virtual bool + isValid ( + const PointCloudSourceConstPtr &cloud_src, + const PointCloudTargetConstPtr &cloud_tgt, + const Matrix4 &transformation_matrix) const + { + if (std::isnan (threshold_)) + { + PCL_ERROR ("[pcl::TransformationValidationEuclidean::isValid] Threshold not set! Please use setThreshold () before continuing."); + return (false); + } + + return (validateTransformation (cloud_src, cloud_tgt, transformation_matrix) < threshold_); + } + + protected: + /** \brief The maximum allowable distance between a point and its correspondence in the target + * in order for a correspondence to be considered \a valid. Default: double::max. + */ + double max_range_; + + /** \brief The threshold for which a specific transformation is valid. + * Set to NaN by default, as we must require the user to set it. + */ + double threshold_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief A flag which, if set, means the tree operating on the target cloud + * will never be recomputed*/ + bool force_no_recompute_; + + + /** \brief Internal point representation uses only 3D coordinates for L2 */ + class MyPointRepresentation: public pcl::PointRepresentation + { + using pcl::PointRepresentation::nr_dimensions_; + using pcl::PointRepresentation::trivial_; + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + MyPointRepresentation () + { + nr_dimensions_ = 3; + trivial_ = true; + } + + /** \brief Empty destructor */ + virtual ~MyPointRepresentation () {} + + virtual void + copyToFloatArray (const PointTarget &p, float * out) const + { + out[0] = p.x; + out[1] = p.y; + out[2] = p.z; + } + }; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/transforms.h b/deps_install/include/pcl-1.10/pcl/registration/transforms.h new file mode 100755 index 0000000..3101974 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/transforms.h @@ -0,0 +1,43 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include diff --git a/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid.h b/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid.h new file mode 100755 index 0000000..c96626c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid.h @@ -0,0 +1,131 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief Base warp point class. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Radu B. Rusu + * \ingroup registration + */ + template + class WarpPointRigid + { + public: + using Matrix4 = Eigen::Matrix; + using VectorX = Eigen::Matrix; + using Vector4 = Eigen::Matrix; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor + * \param[in] nr_dim the number of dimensions + */ + WarpPointRigid (int nr_dim) + : nr_dim_ (nr_dim) + , transform_matrix_ (Matrix4::Zero ()) + { + transform_matrix_ (3, 3) = 1.0; + }; + + /** \brief Destructor. */ + virtual ~WarpPointRigid () {}; + + /** \brief Set warp parameters. Pure virtual. + * \param[in] p warp parameters + */ + virtual void + setParam (const VectorX& p) = 0; + + /** \brief Warp a point given a transformation matrix + * \param[in] pnt_in the point to warp (transform) + * \param[out] pnt_out the warped (transformed) point + */ + inline void + warpPoint (const PointSourceT& pnt_in, PointSourceT& pnt_out) const + { + pnt_out.x = static_cast (transform_matrix_ (0, 0) * pnt_in.x + transform_matrix_ (0, 1) * pnt_in.y + transform_matrix_ (0, 2) * pnt_in.z + transform_matrix_ (0, 3)); + pnt_out.y = static_cast (transform_matrix_ (1, 0) * pnt_in.x + transform_matrix_ (1, 1) * pnt_in.y + transform_matrix_ (1, 2) * pnt_in.z + transform_matrix_ (1, 3)); + pnt_out.z = static_cast (transform_matrix_ (2, 0) * pnt_in.x + transform_matrix_ (2, 1) * pnt_in.y + transform_matrix_ (2, 2) * pnt_in.z + transform_matrix_ (2, 3)); + //pnt_out.getVector3fMap () = transform_matrix_.topLeftCorner (3, 3) * + // pnt_in.getVector3fMap () + + // transform_matrix_.block (0, 3, 3, 1); + //pnt_out.data[3] = pnt_in.data[3]; + } + + /** \brief Warp a point given a transformation matrix + * \param[in] pnt_in the point to warp (transform) + * \param[out] pnt_out the warped (transformed) point + */ + inline void + warpPoint (const PointSourceT& pnt_in, Vector4& pnt_out) const + { + pnt_out[0] = static_cast (transform_matrix_ (0, 0) * pnt_in.x + transform_matrix_ (0, 1) * pnt_in.y + transform_matrix_ (0, 2) * pnt_in.z + transform_matrix_ (0, 3)); + pnt_out[1] = static_cast (transform_matrix_ (1, 0) * pnt_in.x + transform_matrix_ (1, 1) * pnt_in.y + transform_matrix_ (1, 2) * pnt_in.z + transform_matrix_ (1, 3)); + pnt_out[2] = static_cast (transform_matrix_ (2, 0) * pnt_in.x + transform_matrix_ (2, 1) * pnt_in.y + transform_matrix_ (2, 2) * pnt_in.z + transform_matrix_ (2, 3)); + pnt_out[3] = 0.0; + } + + /** \brief Get the number of dimensions. */ + inline int + getDimension () const { return (nr_dim_); } + + /** \brief Get the Transform used. */ + inline const Matrix4& + getTransform () const { return (transform_matrix_); } + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + + protected: + int nr_dim_; + Matrix4 transform_matrix_; + }; + } // namespace registration +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid_3d.h b/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid_3d.h new file mode 100755 index 0000000..cc3cd7c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid_3d.h @@ -0,0 +1,95 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b WarpPointRigid3D enables 3D (1D rotation + 2D translation) + * transformations for points. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Radu B. Rusu + * \ingroup registration + */ + template + class WarpPointRigid3D : public WarpPointRigid + { + public: + using Matrix4 = typename WarpPointRigid::Matrix4; + using VectorX = typename WarpPointRigid::VectorX; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor. */ + WarpPointRigid3D () : WarpPointRigid (3) {} + + /** \brief Empty destructor */ + ~WarpPointRigid3D () {} + + /** \brief Set warp parameters. + * \param[in] p warp parameters (tx ty rz) + */ + void + setParam (const VectorX & p) override + { + assert (p.rows () == this->getDimension ()); + Matrix4 &trans = this->transform_matrix_; + + trans = Matrix4::Zero (); + trans (3, 3) = 1; + trans (2, 2) = 1; // Rotation around the Z-axis + + // Copy the rotation and translation components + trans.block (0, 3, 4, 1) = Eigen::Matrix (p[0], p[1], 0, 1.0); + + // Compute w from the unit quaternion + Eigen::Rotation2D r (p[2]); + trans.topLeftCorner (2, 2) = r.toRotationMatrix (); + } + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid_6d.h b/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid_6d.h new file mode 100755 index 0000000..a8aaa65 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/registration/warp_point_rigid_6d.h @@ -0,0 +1,97 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace registration + { + /** \brief @b WarpPointRigid3D enables 6D (3D rotation + 3D translation) + * transformations for points. + * + * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Radu B. Rusu + * \ingroup registration + */ + template + class WarpPointRigid6D : public WarpPointRigid + { + public: + using WarpPointRigid::transform_matrix_; + + using Matrix4 = typename WarpPointRigid::Matrix4; + using VectorX = typename WarpPointRigid::VectorX; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + WarpPointRigid6D () : WarpPointRigid (6) {} + + /** \brief Empty destructor */ + ~WarpPointRigid6D () {} + + /** \brief Set warp parameters. + * \note Assumes the quaternion parameters are normalized. + * \param[in] p warp parameters (tx ty tz qx qy qz) + */ + void + setParam (const VectorX& p) override + { + assert (p.rows () == this->getDimension ()); + + // Copy the rotation and translation components + transform_matrix_.setZero (); + transform_matrix_ (0, 3) = p[0]; + transform_matrix_ (1, 3) = p[1]; + transform_matrix_ (2, 3) = p[2]; + transform_matrix_ (3, 3) = 1; + + // Compute w from the unit quaternion + Eigen::Quaternion q (0, p[3], p[4], p[5]); + q.w () = static_cast (std::sqrt (1 - q.dot (q))); + q.normalize (); + transform_matrix_.topLeftCorner (3, 3) = q.toRotationMatrix (); + } + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/boost.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/boost.h new file mode 100755 index 0000000..4afdf89 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/boost.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/eigen.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/eigen.h new file mode 100755 index 0000000..6d4f649 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/eigen.h @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/lmeds.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/lmeds.hpp new file mode 100755 index 0000000..dd4809e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/lmeds.hpp @@ -0,0 +1,180 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_ + +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) +{ + // Warn and exit if no threshold was set + if (threshold_ == std::numeric_limits::max()) + { + PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] No threshold set!\n"); + return (false); + } + + iterations_ = 0; + double d_best_penalty = std::numeric_limits::max(); + + std::vector selection; + Eigen::VectorXf model_coefficients; + std::vector distances; + + int n_inliers_count = 0; + + unsigned skipped_count = 0; + // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! + const unsigned max_skip = max_iterations_ * 10; + + // Iterate + while (iterations_ < max_iterations_ && 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; + } + + double d_cur_penalty = 0; + // d_cur_penalty = sum (min (dist, threshold)) + + // Iterate through the 3d points and calculate the distances from them to the model + sac_model_->getDistancesToModel (model_coefficients, distances); + + // No distances? The model must not respect the user given constraints + if (distances.empty ()) + { + //iterations_++; + ++skipped_count; + continue; + } + + std::sort (distances.begin (), distances.end ()); + // d_cur_penalty = median (distances) + std::size_t mid = sac_model_->getIndices ()->size () / 2; + if (mid >= distances.size ()) + { + //iterations_++; + ++skipped_count; + continue; + } + + // Do we have a "middle" point or should we "estimate" one ? + if (sac_model_->getIndices ()->size () % 2 == 0) + d_cur_penalty = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2; + else + d_cur_penalty = sqrt (distances[mid]); + + // 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; + } + + ++iterations_; + if (debug_verbosity_level > 1) + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, max_iterations_, d_best_penalty); + } + + if (model_.empty ()) + { + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Unable to find a solution!\n"); + return (false); + } + + // Classify the data points into inliers and outliers + // Sigma = 1.4826 * (1 + 5 / (n-d)) * sqrt (M) + // @note: See "Robust Regression Methods for Computer Vision: A Review" + //double sigma = 1.4826 * (1 + 5 / (sac_model_->getIndices ()->size () - best_model.size ())) * sqrt (d_best_penalty); + //double threshold = 2.5 * sigma; + + // Iterate through the 3d points and calculate the distances from them to the model again + sac_model_->getDistancesToModel (model_coefficients_, distances); + // No distances? The model must not respect the user given constraints + if (distances.empty ()) + { + PCL_ERROR ("[pcl::LeastMedianSquares::computeModel] The model found failed to verify against the given constraints!\n"); + return (false); + } + + std::vector &indices = *sac_model_->getIndices (); + + if (distances.size () != indices.size ()) + { + PCL_ERROR ("[pcl::LeastMedianSquares::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 (std::size_t i = 0; i < distances.size (); ++i) + if (distances[i] <= threshold_) + inliers_[n_inliers_count++] = indices[i]; + + // Resize the inliers vector + inliers_.resize (n_inliers_count); + + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); + + return (true); +} + +#define PCL_INSTANTIATE_LeastMedianSquares(T) template class PCL_EXPORTS pcl::LeastMedianSquares; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/mlesac.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/mlesac.hpp new file mode 100755 index 0000000..0a2ba29 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/mlesac.hpp @@ -0,0 +1,301 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::MaximumLikelihoodSampleConsensus::computeModel (int debug_verbosity_level) +{ + // Warn and exit if no threshold was set + if (threshold_ == std::numeric_limits::max()) + { + PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n"); + return (false); + } + + iterations_ = 0; + double d_best_penalty = std::numeric_limits::max(); + double k = 1.0; + + std::vector selection; + Eigen::VectorXf model_coefficients; + std::vector 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; + std::size_t indices_size; + unsigned skipped_count = 0; + // suppress 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 p_inlier_prob (indices_size); + for (int j = 0; j < iterations_EM_; ++j) + { + // Likelihood of a datum given that it is an inlier + for (std::size_t i = 0; i < indices_size; ++i) + p_inlier_prob[i] = gamma * std::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 (std::size_t i = 0; i < indices_size; ++i) + gamma += p_inlier_prob [i] / (p_inlier_prob[i] + p_outlier_prob); + gamma /= static_cast(sac_model_->getIndices ()->size ()); + } + + // Find the std::log likelihood of the model -L = -sum [std::log (pInlierProb + pOutlierProb)] + double d_cur_penalty = 0; + for (std::size_t i = 0; i < indices_size; ++i) + d_cur_penalty += std::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 (const double &distance : distances) + if (distance <= 2 * sigma_) + n_inliers_count++; + + // Compute the k parameter (k=std::log(z)/std::log(1-w^n)) + double w = static_cast (n_inliers_count) / static_cast (sac_model_->getIndices ()->size ()); + double p_no_outliers = 1 - std::pow (w, static_cast (selection.size ())); + p_no_outliers = (std::max) (std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by -Inf + p_no_outliers = (std::min) (1 - std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by 0. + k = std::log (1 - probability_) / std::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 (std::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 &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 (std::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 double +pcl::MaximumLikelihoodSampleConsensus::computeMedianAbsoluteDeviation ( + const PointCloudConstPtr &cloud, + const IndicesPtr &indices, + double sigma) const +{ + std::vector distances (indices->size ()); + + Eigen::Vector4f median; + // median (dist (x - median (x))) + computeMedian (cloud, indices, median); + + for (std::size_t i = 0; i < indices->size (); ++i) + { + pcl::Vector4fMapConst pt = cloud->points[(*indices)[i]].getVector4fMap (); + Eigen::Vector4f ptdiff = pt - median; + ptdiff[3] = 0; + distances[i] = ptdiff.dot (ptdiff); + } + + std::sort (distances.begin (), distances.end ()); + + double result; + std::size_t mid = indices->size () / 2; + // Do we have a "middle" point or should we "estimate" one ? + if (indices->size () % 2 == 0) + result = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2; + else + result = sqrt (distances[mid]); + return (sigma * result); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::MaximumLikelihoodSampleConsensus::getMinMax ( + const PointCloudConstPtr &cloud, + const IndicesPtr &indices, + Eigen::Vector4f &min_p, + Eigen::Vector4f &max_p) const +{ + min_p.setConstant (FLT_MAX); + max_p.setConstant (-FLT_MAX); + min_p[3] = max_p[3] = 0; + + for (std::size_t i = 0; i < indices->size (); ++i) + { + if (cloud->points[(*indices)[i]].x < min_p[0]) min_p[0] = cloud->points[(*indices)[i]].x; + if (cloud->points[(*indices)[i]].y < min_p[1]) min_p[1] = cloud->points[(*indices)[i]].y; + if (cloud->points[(*indices)[i]].z < min_p[2]) min_p[2] = cloud->points[(*indices)[i]].z; + + if (cloud->points[(*indices)[i]].x > max_p[0]) max_p[0] = cloud->points[(*indices)[i]].x; + if (cloud->points[(*indices)[i]].y > max_p[1]) max_p[1] = cloud->points[(*indices)[i]].y; + if (cloud->points[(*indices)[i]].z > max_p[2]) max_p[2] = cloud->points[(*indices)[i]].z; + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::MaximumLikelihoodSampleConsensus::computeMedian ( + const PointCloudConstPtr &cloud, + const IndicesPtr &indices, + Eigen::Vector4f &median) const +{ + // Copy the values to vectors for faster sorting + std::vector x (indices->size ()); + std::vector y (indices->size ()); + std::vector z (indices->size ()); + for (std::size_t i = 0; i < indices->size (); ++i) + { + x[i] = cloud->points[(*indices)[i]].x; + y[i] = cloud->points[(*indices)[i]].y; + z[i] = cloud->points[(*indices)[i]].z; + } + std::sort (x.begin (), x.end ()); + std::sort (y.begin (), y.end ()); + std::sort (z.begin (), z.end ()); + + std::size_t mid = indices->size () / 2; + if (indices->size () % 2 == 0) + { + median[0] = (x[mid-1] + x[mid]) / 2; + median[1] = (y[mid-1] + y[mid]) / 2; + median[2] = (z[mid-1] + z[mid]) / 2; + } + else + { + median[0] = x[mid]; + median[1] = y[mid]; + median[2] = z[mid]; + } + median[3] = 0; +} + +#define PCL_INSTANTIATE_MaximumLikelihoodSampleConsensus(T) template class PCL_EXPORTS pcl::MaximumLikelihoodSampleConsensus; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/msac.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/msac.hpp new file mode 100755 index 0000000..dd2ed9f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/msac.hpp @@ -0,0 +1,165 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_MSAC_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_MSAC_H_ + +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::MEstimatorSampleConsensus::computeModel (int debug_verbosity_level) +{ + // Warn and exit if no threshold was set + if (threshold_ == std::numeric_limits::max()) + { + PCL_ERROR ("[pcl::MEstimatorSampleConsensus::computeModel] No threshold set!\n"); + return (false); + } + + iterations_ = 0; + double d_best_penalty = std::numeric_limits::max(); + double k = 1.0; + + std::vector selection; + Eigen::VectorXf model_coefficients; + std::vector distances; + + int n_inliers_count = 0; + unsigned skipped_count = 0; + // suppress 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; + } + + double d_cur_penalty = 0; + // Iterate through the 3d points and calculate the distances from them to the model + sac_model_->getDistancesToModel (model_coefficients, distances); + + if (distances.empty () && k > 1.0) + continue; + + for (const double &distance : distances) + d_cur_penalty += (std::min) (distance, threshold_); + + // 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 (const double &distance : distances) + if (distance <= threshold_) + ++n_inliers_count; + + // Compute the k parameter (k=std::log(z)/std::log(1-w^n)) + double w = static_cast (n_inliers_count) / static_cast (sac_model_->getIndices ()->size ()); + double p_no_outliers = 1.0 - std::pow (w, static_cast (selection.size ())); + p_no_outliers = (std::max) (std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by -Inf + p_no_outliers = (std::min) (1.0 - std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by 0. + k = std::log (1.0 - probability_) / std::log (p_no_outliers); + } + + ++iterations_; + if (debug_verbosity_level > 1) + PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, static_cast (std::ceil (k)), d_best_penalty); + if (iterations_ > max_iterations_) + { + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] MSAC reached the maximum number of trials.\n"); + break; + } + } + + if (model_.empty ()) + { + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::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 &indices = *sac_model_->getIndices (); + + if (distances.size () != indices.size ()) + { + PCL_ERROR ("[pcl::MEstimatorSampleConsensus::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 (std::size_t i = 0; i < distances.size (); ++i) + if (distances[i] <= threshold_) + inliers_[n_inliers_count++] = indices[i]; + + // Resize the inliers vector + inliers_.resize (n_inliers_count); + + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::MEstimatorSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); + + return (true); +} + +#define PCL_INSTANTIATE_MEstimatorSampleConsensus(T) template class PCL_EXPORTS pcl::MEstimatorSampleConsensus; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_MSAC_H_ diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/prosac.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/prosac.hpp new file mode 100755 index 0000000..41c08ce --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/prosac.hpp @@ -0,0 +1,239 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_ + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +////////////////////////////////////////////////////////////////////////// +// Variable naming uses capital letters to make the comparison with the original paper easier +template bool +pcl::ProgressiveSampleConsensus::computeModel (int debug_verbosity_level) +{ + // Warn and exit if no threshold was set + if (threshold_ == DBL_MAX) + { + PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No threshold set!\n"); + return (false); + } + + // Initialize some PROSAC constants + const int T_N = 200000; + const std::size_t N = sac_model_->indices_->size (); + const std::size_t m = sac_model_->getSampleSize (); + float T_n = static_cast (T_N); + for (unsigned int i = 0; i < m; ++i) + T_n *= static_cast (m - i) / static_cast (N - i); + float T_prime_n = 1.0f; + std::size_t I_N_best = 0; + float n = static_cast (m); + + // Define the n_Start coefficients from Section 2.2 + float n_star = static_cast (N); + float epsilon_n_star = 0.0; + std::size_t k_n_star = T_N; + + // Compute the I_n_star_min of Equation 8 + std::vector I_n_star_min (N); + + // Initialize the usual RANSAC parameters + iterations_ = 0; + + std::vector inliers; + std::vector selection; + Eigen::VectorXf model_coefficients; + + // We will increase the pool so the indices_ vector can only contain m elements at first + std::vector index_pool; + index_pool.reserve (N); + for (unsigned int i = 0; i < n; ++i) + index_pool.push_back (sac_model_->indices_->operator[](i)); + + // Iterate + while (static_cast (iterations_) < k_n_star) + { + // Choose the samples + + // Step 1 + // According to Equation 5 in the text text, not the algorithm + if ((iterations_ == T_prime_n) && (n < n_star)) + { + // Increase the pool + ++n; + if (n >= N) + break; + index_pool.push_back (sac_model_->indices_->at(static_cast (n - 1))); + // Update other variables + float T_n_minus_1 = T_n; + T_n *= (static_cast(n) + 1.0f) / (static_cast(n) + 1.0f - static_cast(m)); + T_prime_n += std::ceil (T_n - T_n_minus_1); + } + + // Step 2 + sac_model_->indices_->swap (index_pool); + selection.clear (); + sac_model_->getSamples (iterations_, selection); + if (T_prime_n < iterations_) + { + selection.pop_back (); + selection.push_back (sac_model_->indices_->at(static_cast (n - 1))); + } + + // Make sure we use the right indices for testing + sac_model_->indices_->swap (index_pool); + + if (selection.empty ()) + { + PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No samples could be selected!\n"); + break; + } + + // Search for inliers in the point cloud for the current model + if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) + { + ++iterations_; + continue; + } + + // Select the inliers that are within threshold_ from the model + inliers.clear (); + sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers); + + std::size_t I_N = inliers.size (); + + // If we find more inliers than before + if (I_N > I_N_best) + { + I_N_best = I_N; + + // Save the current model/inlier/coefficients selection as being the best so far + inliers_ = inliers; + model_ = selection; + model_coefficients_ = model_coefficients; + + // We estimate I_n_star for different possible values of n_star by using the inliers + std::sort (inliers.begin (), inliers.end ()); + + // Try to find a better n_star + // We minimize k_n_star and therefore maximize epsilon_n_star = I_n_star / n_star + std::size_t possible_n_star_best = N, I_possible_n_star_best = I_N; + float epsilon_possible_n_star_best = static_cast(I_possible_n_star_best) / static_cast(possible_n_star_best); + + // We only need to compute possible better epsilon_n_star for when _n is just about to be removed an inlier + std::size_t I_possible_n_star = I_N; + for (std::vector::const_reverse_iterator last_inlier = inliers.rbegin (), + inliers_end = inliers.rend (); + last_inlier != inliers_end; + ++last_inlier, --I_possible_n_star) + { + // The best possible_n_star for a given I_possible_n_star is the index of the last inlier + unsigned int possible_n_star = (*last_inlier) + 1; + if (possible_n_star <= m) + break; + + // If we find a better epsilon_n_star + float epsilon_possible_n_star = static_cast(I_possible_n_star) / static_cast(possible_n_star); + // Make sure we have a better epsilon_possible_n_star + if ((epsilon_possible_n_star > epsilon_n_star) && (epsilon_possible_n_star > epsilon_possible_n_star_best)) + { + // Typo in Equation 7, not (n-m choose i-m) but (n choose i-m) + std::size_t I_possible_n_star_min = m + + static_cast (std::ceil (boost::math::quantile (boost::math::complement (boost::math::binomial_distribution(static_cast (possible_n_star), 0.1f), 0.05)))); + // If Equation 9 is not verified, exit + if (I_possible_n_star < I_possible_n_star_min) + break; + + possible_n_star_best = possible_n_star; + I_possible_n_star_best = I_possible_n_star; + epsilon_possible_n_star_best = epsilon_possible_n_star; + } + } + + // Check if we get a better epsilon + if (epsilon_possible_n_star_best > epsilon_n_star) + { + // update the best value + epsilon_n_star = epsilon_possible_n_star_best; + + // Compute the new k_n_star + float bottom_log = 1 - std::pow (epsilon_n_star, static_cast(m)); + if (bottom_log == 0) + k_n_star = 1; + else if (bottom_log == 1) + k_n_star = T_N; + else + k_n_star = static_cast (std::ceil (std::log (0.05) / std::log (bottom_log))); + // It seems weird to have very few iterations, so do have a few (totally empirical) + k_n_star = (std::max)(k_n_star, 2 * m); + } + } + + ++iterations_; + if (debug_verbosity_level > 1) + PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Trial %d out of %d: %d inliers (best is: %d so far).\n", iterations_, k_n_star, I_N, I_N_best); + if (iterations_ > max_iterations_) + { + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n"); + break; + } + } + + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), I_N_best); + + if (model_.empty ()) + { + inliers_.clear (); + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_ProgressiveSampleConsensus(T) template class PCL_EXPORTS pcl::ProgressiveSampleConsensus; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_ diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/ransac.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/ransac.hpp new file mode 100755 index 0000000..6cf00fe --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/ransac.hpp @@ -0,0 +1,223 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_ + +#include +#ifdef _OPENMP +#include +#endif + +#if defined _OPENMP && _OPENMP >= 201107 // We need OpenMP 3.1 for the atomic constructs +#define OPENMP_AVAILABLE_RANSAC true +#else +#define OPENMP_AVAILABLE_RANSAC false +#endif + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::RandomSampleConsensus::computeModel (int) +{ + // Warn and exit if no threshold was set + if (threshold_ == std::numeric_limits::max()) + { + PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No threshold set!\n"); + return (false); + } + + iterations_ = 0; + std::size_t n_best_inliers_count = 0; + double k = std::numeric_limits::max(); + + std::vector selection; + Eigen::VectorXf model_coefficients; + + const double log_probability = std::log (1.0 - probability_); + const double one_over_indices = 1.0 / static_cast (sac_model_->getIndices ()->size ()); + + std::size_t n_inliers_count; + unsigned skipped_count = 0; + + // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! + const unsigned max_skip = max_iterations_ * 10; + + int threads = threads_; + if (threads >= 0) + { +#if OPENMP_AVAILABLE_RANSAC + if (threads == 0) + { + threads = omp_get_num_procs(); + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Automatic number of threads requested, choosing %i threads.\n", threads); + } +#else + // Parallelization desired, but not available + PCL_WARN ("[pcl::RandomSampleConsensus::computeModel] Parallelization is requested, but OpenMP 3.1 is not available! Continuing without parallelization.\n"); + threads = -1; +#endif + } + +#if OPENMP_AVAILABLE_RANSAC +#pragma omp parallel if(threads > 0) num_threads(threads) shared(k, skipped_count, n_best_inliers_count) private(selection, model_coefficients, n_inliers_count) // would be nice to have a default(none)-clause here, but then some compilers complain about the shared const variables +#endif + { +#if OPENMP_AVAILABLE_RANSAC + if (omp_in_parallel()) +#pragma omp master + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Computing in parallel with up to %i threads.\n", omp_get_num_threads()); + else +#endif + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Computing not parallel.\n"); + + // Iterate + while (true) // infinite loop with four possible breaks + { + // Get X samples which satisfy the model criteria +#if OPENMP_AVAILABLE_RANSAC +#pragma omp critical(samples) +#endif + { + sac_model_->getSamples (iterations_, selection); // The random number generator used when choosing the samples should not be called in parallel + } + + if (selection.empty ()) + { + PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No samples could be selected!\n"); + break; + } + + // Search for inliers in the point cloud for the current plane model M + if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) // This function has to be thread-safe + { + //++iterations_; + unsigned skipped_count_tmp; +#if OPENMP_AVAILABLE_RANSAC +#pragma omp atomic capture +#endif + skipped_count_tmp = ++skipped_count; + if (skipped_count_tmp < max_skip) + continue; + else + break; + } + + // Select the inliers that are within threshold_ from the model + //sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers); + //if (inliers.empty () && k > 1.0) + // continue; + + n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_); // This functions has to be thread-safe. Most work is done here + + std::size_t n_best_inliers_count_tmp; +#if OPENMP_AVAILABLE_RANSAC +#pragma omp atomic read +#endif + n_best_inliers_count_tmp = n_best_inliers_count; + + if (n_inliers_count > n_best_inliers_count_tmp) // This condition is false most of the time, and the critical region is not entered, hopefully leading to more efficient concurrency + { +#if OPENMP_AVAILABLE_RANSAC +#pragma omp critical(update) // n_best_inliers_count, model_, model_coefficients_, k are shared and read/write must be protected +#endif + { + // Better match ? + if (n_inliers_count > n_best_inliers_count) + { + n_best_inliers_count = n_inliers_count; // This write and the previous read of n_best_inliers_count must be consecutive and must not be interrupted! + n_best_inliers_count_tmp = n_best_inliers_count; + + // Save the current model/inlier/coefficients selection as being the best so far + model_ = selection; + model_coefficients_ = model_coefficients; + + // Compute the k parameter (k=std::log(z)/std::log(1-w^n)) + const double w = static_cast (n_best_inliers_count) * one_over_indices; + double p_no_outliers = 1.0 - std::pow (w, static_cast (selection.size ())); + p_no_outliers = (std::max) (std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by -Inf + p_no_outliers = (std::min) (1.0 - std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by 0. + k = log_probability / std::log (p_no_outliers); + } + } // omp critical + } + + int iterations_tmp; + double k_tmp; +#if OPENMP_AVAILABLE_RANSAC +#pragma omp atomic capture +#endif + iterations_tmp = ++iterations_; +#if OPENMP_AVAILABLE_RANSAC +#pragma omp atomic read +#endif + k_tmp = k; +#if OPENMP_AVAILABLE_RANSAC + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %u inliers (best is: %u so far) (thread %d).\n", iterations_tmp, k_tmp, n_inliers_count, n_best_inliers_count_tmp, omp_get_thread_num()); +#else + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %u inliers (best is: %u so far).\n", iterations_tmp, k_tmp, n_inliers_count, n_best_inliers_count_tmp); +#endif + if (iterations_tmp > k_tmp) + break; + if (iterations_tmp > max_iterations_) + { + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n"); + break; + } + } // while + } // omp parallel + + PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %lu size, %u inliers.\n", model_.size (), n_best_inliers_count); + + if (model_.empty ()) + { + PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] RANSAC found no model.\n"); + inliers_.clear (); + return (false); + } + + // Get the set of inliers that correspond to the best model found so far + sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_); + return (true); +} + +#define PCL_INSTANTIATE_RandomSampleConsensus(T) template class PCL_EXPORTS pcl::RandomSampleConsensus; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/rmsac.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/rmsac.hpp new file mode 100755 index 0000000..50c9402 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/rmsac.hpp @@ -0,0 +1,183 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_RMSAC_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_RMSAC_H_ + +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::RandomizedMEstimatorSampleConsensus::computeModel (int debug_verbosity_level) +{ + // Warn and exit if no threshold was set + if (threshold_ == std::numeric_limits::max()) + { + PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] No threshold set!\n"); + return (false); + } + + iterations_ = 0; + double d_best_penalty = std::numeric_limits::max(); + double k = 1.0; + + std::vector selection; + Eigen::VectorXf model_coefficients; + std::vector distances; + std::set indices_subset; + + int n_inliers_count = 0; + unsigned skipped_count = 0; + // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! + const unsigned max_skip = max_iterations_ * 10; + + // Number of samples to try randomly + std::size_t fraction_nr_points = pcl_lrint (static_cast(sac_model_->getIndices ()->size ()) * fraction_nr_pretest_ / 100.0); + + // 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; + } + + // RMSAC addon: verify a random fraction of the data + // Get X random samples which satisfy the model criterion + this->getRandomSamples (sac_model_->getIndices (), fraction_nr_points, indices_subset); + + if (!sac_model_->doSamplesVerifyModel (indices_subset, model_coefficients, threshold_)) + { + // Unfortunately we cannot "continue" after the first iteration, because k might not be set, while iterations gets incremented + if (k != 1.0) + { + ++iterations_; + continue; + } + } + + double d_cur_penalty = 0; + // Iterate through the 3d points and calculate the distances from them to the model + sac_model_->getDistancesToModel (model_coefficients, distances); + + if (distances.empty () && k > 1.0) + continue; + + for (const double &distance : distances) + d_cur_penalty += std::min (distance, threshold_); + + // 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 (const double &distance : distances) + if (distance <= threshold_) + n_inliers_count++; + + // Compute the k parameter (k=std::log(z)/std::log(1-w^n)) + double w = static_cast (n_inliers_count) / static_cast(sac_model_->getIndices ()->size ()); + double p_no_outliers = 1 - std::pow (w, static_cast (selection.size ())); + p_no_outliers = (std::max) (std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by -Inf + p_no_outliers = (std::min) (1 - std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by 0. + k = std::log (1 - probability_) / std::log (p_no_outliers); + } + + ++iterations_; + if (debug_verbosity_level > 1) + PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, static_cast (std::ceil (k)), d_best_penalty); + if (iterations_ > max_iterations_) + { + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] MSAC reached the maximum number of trials.\n"); + break; + } + } + + if (model_.empty ()) + { + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::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 &indices = *sac_model_->getIndices (); + if (distances.size () != indices.size ()) + { + PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::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 (std::size_t i = 0; i < distances.size (); ++i) + if (distances[i] <= threshold_) + inliers_[n_inliers_count++] = indices[i]; + + // Resize the inliers vector + inliers_.resize (n_inliers_count); + + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); + + return (true); +} + +#define PCL_INSTANTIATE_RandomizedMEstimatorSampleConsensus(T) template class PCL_EXPORTS pcl::RandomizedMEstimatorSampleConsensus; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_RMSAC_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/rransac.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/rransac.hpp new file mode 100755 index 0000000..7a839ff --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/rransac.hpp @@ -0,0 +1,165 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_RRANSAC_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_RRANSAC_H_ + +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::RandomizedRandomSampleConsensus::computeModel (int debug_verbosity_level) +{ + // Warn and exit if no threshold was set + if (threshold_ == std::numeric_limits::max()) + { + PCL_ERROR ("[pcl::RandomizedRandomSampleConsensus::computeModel] No threshold set!\n"); + return (false); + } + + iterations_ = 0; + std::size_t n_best_inliers_count = 0; + double k = std::numeric_limits::max(); + + std::vector selection; + Eigen::VectorXf model_coefficients; + std::set indices_subset; + + const double log_probability = std::log (1.0 - probability_); + const double one_over_indices = 1.0 / static_cast (sac_model_->getIndices ()->size ()); + + std::size_t n_inliers_count; + unsigned skipped_count = 0; + // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! + const unsigned max_skip = max_iterations_ * 10; + + // Number of samples to try randomly + const std::size_t fraction_nr_points = pcl_lrint (static_cast(sac_model_->getIndices ()->size ()) * fraction_nr_pretest_ / 100.0); + + // Iterate + while (iterations_ < k) + { + // Get X samples which satisfy the model criteria + sac_model_->getSamples (iterations_, selection); + + if (selection.empty ()) + { + PCL_ERROR ("[pcl::RandomizedRandomSampleConsensus::computeModel] No samples could be selected!\n"); + break; + } + + // Search for inliers in the point cloud for the current plane model M + if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) + { + //iterations_++; + ++skipped_count; + if (skipped_count < max_skip) + { + PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] The function computeModelCoefficients failed, so continue with next iteration.\n"); + continue; + } + else + { + PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] The function computeModelCoefficients failed, and RRANSAC reached the maximum number of trials.\n"); + break; + } + } + + // RRANSAC addon: verify a random fraction of the data + // Get X random samples which satisfy the model criterion + this->getRandomSamples (sac_model_->getIndices (), fraction_nr_points, indices_subset); + if (!sac_model_->doSamplesVerifyModel (indices_subset, model_coefficients, threshold_)) + { + ++iterations_; + PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] The function doSamplesVerifyModel failed, so continue with next iteration.\n"); + continue; + } + + // Select the inliers that are within threshold_ from the model + n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_); + + // Better match ? + if (n_inliers_count > n_best_inliers_count) + { + n_best_inliers_count = n_inliers_count; + + // Save the current model/inlier/coefficients selection as being the best so far + model_ = selection; + model_coefficients_ = model_coefficients; + + // Compute the k parameter (k=std::log(z)/std::log(1-w^n)) + const double w = static_cast (n_inliers_count) * one_over_indices; + double p_no_outliers = 1.0 - std::pow (w, static_cast (selection.size ())); + p_no_outliers = (std::max) (std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by -Inf + p_no_outliers = (std::min) (1.0 - std::numeric_limits::epsilon (), p_no_outliers); // Avoid division by 0. + k = log_probability / std::log (p_no_outliers); + } + + ++iterations_; + + if (debug_verbosity_level > 1) + PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] Trial %d out of %d: %u inliers (best is: %u so far).\n", iterations_, static_cast (std::ceil (k)), n_inliers_count, n_best_inliers_count); + if (iterations_ > max_iterations_) + { + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] RRANSAC reached the maximum number of trials.\n"); + break; + } + } + + if (debug_verbosity_level > 0) + PCL_DEBUG ("[pcl::RandomizedRandomSampleConsensus::computeModel] Model: %lu size, %u inliers.\n", model_.size (), n_best_inliers_count); + + if (model_.empty ()) + { + PCL_ERROR ("[pcl::RandomizedRandomSampleConsensus::computeModel] RRANSAC found no model.\n"); + inliers_.clear (); + return (false); + } + + // Get the set of inliers that correspond to the best model found so far + sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_); + return (true); +} + +#define PCL_INSTANTIATE_RandomizedRandomSampleConsensus(T) template class PCL_EXPORTS pcl::RandomizedRandomSampleConsensus; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_RRANSAC_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_circle.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_circle.hpp new file mode 100755 index 0000000..9ba524a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_circle.hpp @@ -0,0 +1,338 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle2D::isSampleGood(const std::vector &samples) const +{ + // Get the values at the two points + Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y); + Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y); + Eigen::Array2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y); + + // Compute the segment values (in 2d) between p1 and p0 + p1 -= p0; + // Compute the segment values (in 2d) between p2 and p0 + p2 -= p0; + + Eigen::Array2d dy1dy2 = p1 / p2; + + return (dy1dy2[0] != dy1dy2[1]); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle2D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 3 samples + if (samples.size () != 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + model_coefficients.resize (3); + + Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y); + Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y); + Eigen::Vector2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y); + + Eigen::Vector2d u = (p0 + p1) / 2.0; + Eigen::Vector2d v = (p1 + p2) / 2.0; + + Eigen::Vector2d p1p0dif = p1 - p0; + Eigen::Vector2d p2p1dif = p2 - p1; + Eigen::Vector2d uvdif = u - v; + + Eigen::Vector2d m (- p1p0dif[0] / p1p0dif[1], - p2p1dif[0] / p2p1dif[1]); + + // Center (x, y) + model_coefficients[0] = static_cast ((m[0] * u[0] - m[1] * v[0] - uvdif[1] ) / (m[0] - m[1])); + model_coefficients[1] = static_cast ((m[0] * m[1] * uvdif[0] + m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1])); + + // Radius + model_coefficients[2] = static_cast (sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) + + (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1]))); + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle2D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + distances.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the circle + for (std::size_t i = 0; i < indices_->size (); ++i) + // Calculate the distance from the point to the circle as the difference between + // dist(point,circle_origin) and circle_radius + distances[i] = std::abs (std::sqrt ( + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + + + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) + ) - model_coefficients[2]); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle2D::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, + std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the circle + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the circle as the difference between + // dist(point,circle_origin) and circle_radius + float distance = std::abs (std::sqrt ( + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + + + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) + ) - model_coefficients[2]); + if (distance < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = static_cast (distance); + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelCircle2D::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + std::size_t nr_p = 0; + + // Iterate through the 3d points and calculate the distances from them to the circle + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the circle as the difference between + // dist(point,circle_origin) and circle_radius + float distance = std::abs (std::sqrt ( + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + + + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) + ) - model_coefficients[2]); + if (distance < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + optimized_coefficients = model_coefficients; + + // Needs a set of valid model coefficients + if (model_coefficients.size () != 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + // Need at least 3 samples + if (inliers.size () <= 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + return; + } + + OptimizationFunctor functor (this, inliers); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, float> lm (num_diff); + int info = lm.minimize (optimized_coefficients); + + // Compute the L2 norm of the residuals + PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g\n", + info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2]); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle2D::projectPoints ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + // 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; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + + // Iterate through the points and project them to the circle + for (const int &inlier : inliers) + { + float dx = input_->points[inlier].x - model_coefficients[0]; + float dy = input_->points[inlier].y - model_coefficients[1]; + float a = std::sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) ); + + projected_points.points[inlier].x = a * dx + model_coefficients[0]; + projected_points.points[inlier].y = a * dy + model_coefficients[1]; + } + } + else + { + // Allocate enough space and copy the basics + projected_points.points.resize (inliers.size ()); + projected_points.width = static_cast (inliers.size ()); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + + // Iterate through the points and project them to the circle + for (std::size_t i = 0; i < inliers.size (); ++i) + { + float dx = input_->points[inliers[i]].x - model_coefficients[0]; + float dy = input_->points[inliers[i]].y - model_coefficients[1]; + float a = std::sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) ); + + projected_points.points[i].x = a * dx + model_coefficients[0]; + projected_points.points[i].y = a * dy + model_coefficients[1]; + } + } +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid model coefficients + if (model_coefficients.size () != 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return (false); + } + + for (const int &index : indices) + // Calculate the distance from the point to the circle as the difference between + //dist(point,circle_origin) and circle_radius + if (std::abs (std::sqrt ( + ( input_->points[index].x - model_coefficients[0] ) * + ( input_->points[index].x - model_coefficients[0] ) + + ( input_->points[index].y - model_coefficients[1] ) * + ( input_->points[index].y - model_coefficients[1] ) + ) - model_coefficients[2]) > threshold) + return (false); + + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle2D::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + if (radius_min_ != -std::numeric_limits::max() && model_coefficients[2] < radius_min_) + return (false); + if (radius_max_ != std::numeric_limits::max() && model_coefficients[2] > radius_max_) + return (false); + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelCircle2D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle2D; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_circle3d.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_circle3d.hpp new file mode 100755 index 0000000..4273a89 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_circle3d.hpp @@ -0,0 +1,451 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle3D::isSampleGood ( + const std::vector &samples) const +{ + // Get the values at the three points + Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z); + Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z); + Eigen::Vector3d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z); + + // calculate vectors between points + p1 -= p0; + p2 -= p0; + + return (p1.dot (p2) < 0.000001); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle3D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 3 samples + if (samples.size () != 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + model_coefficients.resize (7); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ + + Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z); + Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z); + Eigen::Vector3d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z); + + + Eigen::Vector3d helper_vec01 = p0 - p1; + Eigen::Vector3d helper_vec02 = p0 - p2; + Eigen::Vector3d helper_vec10 = p1 - p0; + Eigen::Vector3d helper_vec12 = p1 - p2; + Eigen::Vector3d helper_vec20 = p2 - p0; + Eigen::Vector3d helper_vec21 = p2 - p1; + + Eigen::Vector3d common_helper_vec = helper_vec01.cross (helper_vec12); + + double commonDividend = 2.0 * common_helper_vec.squaredNorm (); + + double alpha = (helper_vec12.squaredNorm () * helper_vec01.dot (helper_vec02)) / commonDividend; + double beta = (helper_vec02.squaredNorm () * helper_vec10.dot (helper_vec12)) / commonDividend; + double gamma = (helper_vec01.squaredNorm () * helper_vec20.dot (helper_vec21)) / commonDividend; + + Eigen::Vector3d circle_center = alpha * p0 + beta * p1 + gamma * p2; + + Eigen::Vector3d circle_radiusVector = circle_center - p0; + double circle_radius = circle_radiusVector.norm (); + Eigen::Vector3d circle_normal = common_helper_vec.normalized (); + + model_coefficients[0] = static_cast (circle_center[0]); + model_coefficients[1] = static_cast (circle_center[1]); + model_coefficients[2] = static_cast (circle_center[2]); + model_coefficients[3] = static_cast (circle_radius); + model_coefficients[4] = static_cast (circle_normal[0]); + model_coefficients[5] = static_cast (circle_normal[1]); + model_coefficients[6] = static_cast (circle_normal[2]); + + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle3D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + distances.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + // Calculate the distance from the point to the circle: + // 1. calculate intersection point of the plane in which the circle lies and the + // line from the sample point with the direction of the plane normal (projected point) + // 2. calculate the intersection point of the line from the circle center to the projected point + // with the circle + // 3. calculate distance from corresponding point on the circle to the sample point + { + // what i have: + // P : Sample Point + Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z); + // C : Circle Center + Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + // N : Circle (Plane) Normal + Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); + // r : Radius + double r = model_coefficients[3]; + + Eigen::Vector3d helper_vectorPC = P - C; + // 1.1. get line parameter + double lambda = (helper_vectorPC.dot (N)) / N.squaredNorm (); + + // Projected Point on plane + Eigen::Vector3d P_proj = P + lambda * N; + Eigen::Vector3d helper_vectorP_projC = P_proj - C; + + // K : Point on Circle + Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); + Eigen::Vector3d distanceVector = P - K; + + distances[i] = distanceVector.norm (); + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle3D::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, + std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + int nr_p = 0; + inliers.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // what i have: + // P : Sample Point + Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z); + // C : Circle Center + Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + // N : Circle (Plane) Normal + Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); + // r : Radius + double r = model_coefficients[3]; + + Eigen::Vector3d helper_vectorPC = P - C; + // 1.1. get line parameter + double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); + // Projected Point on plane + Eigen::Vector3d P_proj = P + lambda * N; + Eigen::Vector3d helper_vectorP_projC = P_proj - C; + + // K : Point on Circle + Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); + Eigen::Vector3d distanceVector = P - K; + + if (distanceVector.norm () < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + nr_p++; + } + } + inliers.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelCircle3D::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + std::size_t nr_p = 0; + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // what i have: + // P : Sample Point + Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z); + // C : Circle Center + Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + // N : Circle (Plane) Normal + Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); + // r : Radius + double r = model_coefficients[3]; + + Eigen::Vector3d helper_vectorPC = P - C; + // 1.1. get line parameter + double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); + + // Projected Point on plane + Eigen::Vector3d P_proj = P + lambda * N; + Eigen::Vector3d helper_vectorP_projC = P_proj - C; + + // K : Point on Circle + Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); + Eigen::Vector3d distanceVector = P - K; + + if (distanceVector.norm () < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( + const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const +{ + optimized_coefficients = model_coefficients; + + // Needs a set of valid model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + // Need at least 3 samples + if (inliers.size () <= 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + return; + } + + OptimizationFunctor functor (this, inliers); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, double> lm (num_diff); + Eigen::VectorXd coeff; + int info = lm.minimize (coeff); + for (Eigen::Index i = 0; i < coeff.size (); ++i) + optimized_coefficients[i] = static_cast (coeff[i]); + + // Compute the L2 norm of the residuals + PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n", + info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCircle3D::projectPoints ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + // 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; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < inliers.size (); ++i) + { + // what i have: + // P : Sample Point + Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z); + // C : Circle Center + Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + // N : Circle (Plane) Normal + Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); + // r : Radius + double r = model_coefficients[3]; + + Eigen::Vector3d helper_vectorPC = P - C; + // 1.1. get line parameter + //float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ; + double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); + // Projected Point on plane + Eigen::Vector3d P_proj = P + lambda * N; + Eigen::Vector3d helper_vectorP_projC = P_proj - C; + + // K : Point on Circle + Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); + + projected_points.points[i].x = static_cast (K[0]); + projected_points.points[i].y = static_cast (K[1]); + projected_points.points[i].z = static_cast (K[2]); + } + } + else + { + // Allocate enough space and copy the basics + projected_points.points.resize (inliers.size ()); + projected_points.width = std::uint32_t (inliers.size ()); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < inliers.size (); ++i) + { + // what i have: + // P : Sample Point + Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z); + // C : Circle Center + Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + // N : Circle (Plane) Normal + Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); + // r : Radius + double r = model_coefficients[3]; + + Eigen::Vector3d helper_vectorPC = P - C; + // 1.1. get line parameter + double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); + // Projected Point on plane + Eigen::Vector3d P_proj = P + lambda * N; + Eigen::Vector3d helper_vectorP_projC = P_proj - C; + + // K : Point on Circle + Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); + + projected_points.points[i].x = static_cast (K[0]); + projected_points.points[i].y = static_cast (K[1]); + projected_points.points[i].z = static_cast (K[2]); + } + } +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel ( + const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const +{ + // Needs a valid model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return (false); + } + + for (const int &index : indices) + { + // Calculate the distance from the point to the sphere as the difference between + //dist(point,sphere_origin) and sphere_radius + + // what i have: + // P : Sample Point + Eigen::Vector3d P (input_->points[index].x, input_->points[index].y, input_->points[index].z); + // C : Circle Center + Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + // N : Circle (Plane) Normal + Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]); + // r : Radius + double r = model_coefficients[3]; + Eigen::Vector3d helper_vectorPC = P - C; + // 1.1. get line parameter + double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N); + // Projected Point on plane + Eigen::Vector3d P_proj = P + lambda * N; + Eigen::Vector3d helper_vectorP_projC = P_proj - C; + + // K : Point on Circle + Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized (); + Eigen::Vector3d distanceVector = P - K; + + if (distanceVector.norm () > threshold) + return (false); + } + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCircle3D::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + if (radius_min_ != -DBL_MAX && model_coefficients[3] < radius_min_) + return (false); + if (radius_max_ != DBL_MAX && model_coefficients[3] > radius_max_) + return (false); + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelCircle3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle3D; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE3D_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_cone.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_cone.hpp new file mode 100755 index 0000000..dabe69c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_cone.hpp @@ -0,0 +1,529 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCone::isSampleGood(const std::vector &) const +{ + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCone::computeModelCoefficients ( + const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 3 samples + if (samples.size () != 3) + { + PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] No input dataset containing normals was given!\n"); + 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 p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].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 n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0); + + //calculate apex (intersection of the three planes defined by points and belonging normals + Eigen::Vector4f ortho12 = n1.cross3(n2); + Eigen::Vector4f ortho23 = n2.cross3(n3); + Eigen::Vector4f ortho31 = n3.cross3(n1); + + float denominator = n1.dot(ortho23); + + float d1 = p1.dot (n1); + float d2 = p2.dot (n2); + float d3 = p3.dot (n3); + + Eigen::Vector4f apex = (d1 * ortho23 + d2 * ortho31 + d3 * ortho12) / denominator; + + //compute axis (normal of plane defined by: { apex+(p1-apex)/(||p1-apex||), apex+(p2-apex)/(||p2-apex||), apex+(p3-apex)/(||p3-apex||)} + Eigen::Vector4f ap1 = p1 - apex; + Eigen::Vector4f ap2 = p2 - apex; + Eigen::Vector4f ap3 = p3 - apex; + + Eigen::Vector4f np1 = apex + (ap1/ap1.norm ()); + Eigen::Vector4f np2 = apex + (ap2/ap2.norm ()); + Eigen::Vector4f np3 = apex + (ap3/ap3.norm ()); + + Eigen::Vector4f np1np2 = np2 - np1; + Eigen::Vector4f np1np3 = np3 - np1; + + Eigen::Vector4f axis_dir = np1np2.cross3 (np1np3); + axis_dir.normalize (); + + // normalize the vector (apex->p) for opening angle calculation + ap1.normalize (); + ap2.normalize (); + ap3.normalize (); + + //compute opening angle + float opening_angle = ( std::acos (ap1.dot (axis_dir)) + std::acos (ap2.dot (axis_dir)) + std::acos (ap3.dot (axis_dir)) ) / 3.0f; + + model_coefficients.resize (7); + // model_coefficients.template head<3> () = line_pt.template head<3> (); + model_coefficients[0] = apex[0]; + model_coefficients[1] = apex[1]; + model_coefficients[2] = apex[2]; + // model_coefficients.template segment<3> (3) = line_dir.template head<3> (); + model_coefficients[3] = axis_dir[0]; + model_coefficients[4] = axis_dir[1]; + model_coefficients[5] = axis_dir[2]; + // cone radius + model_coefficients[6] = opening_angle; + + if (model_coefficients[6] != -std::numeric_limits::max() && model_coefficients[6] < min_angle_) + return (false); + if (model_coefficients[6] != std::numeric_limits::max() && model_coefficients[6] > max_angle_) + return (false); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCone::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + distances.resize (indices_->size ()); + + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + float opening_angle = model_coefficients[6]; + + float apexdotdir = apex.dot (axis_dir); + float dirdotdir = 1.0f / axis_dir.dot (axis_dir); + // Iterate through the 3d points and calculate the distances from them to the cone + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + + // Calculate the point's projection on the cone axis + float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = apex + k * axis_dir; + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the actual radius of the cone at the level of the projected point + Eigen::Vector4f height = apex - pt_proj; + float actual_cone_radius = tanf (opening_angle) * height.norm (); + height.normalize (); + + // Calculate the cones perfect normals + Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * dir; + + // Approximate the distance from the point to the cone as the difference between + // dist(point,cone_axis) and actual cone radius + double d_euclid = std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius); + + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double d_normal = std::abs (getAngle3D (n, cone_normal)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCone::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + float opening_angle = model_coefficients[6]; + + float apexdotdir = apex.dot (axis_dir); + float dirdotdir = 1.0f / axis_dir.dot (axis_dir); + // Iterate through the 3d points and calculate the distances from them to the cone + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + + // Calculate the point's projection on the cone axis + float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = apex + k * axis_dir; + + // Calculate the direction of the point from center + Eigen::Vector4f pp_pt_dir = pt - pt_proj; + pp_pt_dir.normalize (); + + // Calculate the actual radius of the cone at the level of the projected point + Eigen::Vector4f height = apex - pt_proj; + double actual_cone_radius = tan(opening_angle) * height.norm (); + height.normalize (); + + // Calculate the cones perfect normals + Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir; + + // Approximate the distance from the point to the cone as the difference between + // dist(point,cone_axis) and actual cone radius + double d_euclid = std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius); + + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double d_normal = std::abs (getAngle3D (n, cone_normal)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + + if (distance < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = distance; + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelCone::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + std::size_t nr_p = 0; + + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + float opening_angle = model_coefficients[6]; + + float apexdotdir = apex.dot (axis_dir); + float dirdotdir = 1.0f / axis_dir.dot (axis_dir); + // Iterate through the 3d points and calculate the distances from them to the cone + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + + // Calculate the point's projection on the cone axis + float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = apex + k * axis_dir; + + // Calculate the direction of the point from center + Eigen::Vector4f pp_pt_dir = pt - pt_proj; + pp_pt_dir.normalize (); + + // Calculate the actual radius of the cone at the level of the projected point + Eigen::Vector4f height = apex - pt_proj; + double actual_cone_radius = tan(opening_angle) * height.norm (); + height.normalize (); + + // Calculate the cones perfect normals + Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir; + + // Approximate the distance from the point to the cone as the difference between + // dist(point,cone_axis) and actual cone radius + double d_euclid = std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius); + + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double d_normal = std::abs (getAngle3D (n, cone_normal)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCone::optimizeModelCoefficients ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + optimized_coefficients = model_coefficients; + + // Needs a set of valid model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + if (inliers.empty ()) + { + PCL_DEBUG ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n"); + return; + } + + OptimizationFunctor functor (this, inliers); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, float> lm (num_diff); + int info = lm.minimize (optimized_coefficients); + + // Compute the L2 norm of the residuals + PCL_DEBUG ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n", + info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], + model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]); + + Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]); + line_dir.normalize (); + optimized_coefficients[3] = line_dir[0]; + optimized_coefficients[4] = line_dir[1]; + optimized_coefficients[5] = line_dir[2]; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCone::projectPoints ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + float opening_angle = model_coefficients[6]; + + float apexdotdir = apex.dot (axis_dir); + float dirdotdir = 1.0f / axis_dir.dot (axis_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; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cone + for (const int &inlier : inliers) + { + Eigen::Vector4f pt (input_->points[inlier].x, + input_->points[inlier].y, + input_->points[inlier].z, + 1); + + float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; + + pcl::Vector4fMap pp = projected_points.points[inlier].getVector4fMap (); + pp.matrix () = apex + k * axis_dir; + + Eigen::Vector4f dir = pt - pp; + dir.normalize (); + + // Calculate the actual radius of the cone at the level of the projected point + Eigen::Vector4f height = apex - pp; + float actual_cone_radius = tanf (opening_angle) * height.norm (); + + // Calculate the projection of the point onto the cone + pp += dir * actual_cone_radius; + } + } + else + { + // Allocate enough space and copy the basics + projected_points.points.resize (inliers.size ()); + projected_points.width = static_cast (inliers.size ()); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cone + for (std::size_t i = 0; i < inliers.size (); ++i) + { + pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap (); + pcl::Vector4fMapConst pt = input_->points[inliers[i]].getVector4fMap (); + + float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; + // Calculate the projection of the point on the line + pp.matrix () = apex + k * axis_dir; + + Eigen::Vector4f dir = pt - pp; + dir.normalize (); + + // Calculate the actual radius of the cone at the level of the projected point + Eigen::Vector4f height = apex - pp; + float actual_cone_radius = tanf (opening_angle) * height.norm (); + + // Calculate the projection of the point onto the cone + pp += dir * actual_cone_radius; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCone::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return (false); + } + + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + float openning_angle = model_coefficients[6]; + + float apexdotdir = apex.dot (axis_dir); + float dirdotdir = 1.0f / axis_dir.dot (axis_dir); + + // Iterate through the 3d points and calculate the distances from them to the cone + for (const int &index : indices) + { + Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0); + + // Calculate the point's projection on the cone axis + float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = apex + k * axis_dir; + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the actual radius of the cone at the level of the projected point + Eigen::Vector4f height = apex - pt_proj; + double actual_cone_radius = tan (openning_angle) * height.norm (); + + // Approximate the distance from the point to the cone as the difference between + // dist(point,cone_axis) and actual cone radius + if (std::abs (static_cast(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold) + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::SampleConsensusModelCone::pointToAxisDistance ( + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const +{ + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + return sqrt(pcl::sqrPointToLineDistance (pt, apex, axis_dir)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCone::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + // Check against template, if given + if (eps_angle_ > 0.0) + { + // Obtain the cone direction + Eigen::Vector4f coeff; + coeff[0] = model_coefficients[3]; + coeff[1] = model_coefficients[4]; + coeff[2] = model_coefficients[5]; + coeff[3] = 0; + + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + double angle_diff = std::abs (getAngle3D (axis, coeff)); + angle_diff = (std::min) (angle_diff, M_PI - angle_diff); + // Check whether the current cone model satisfies our angle threshold criterion with respect to the given axis + if (angle_diff > eps_angle_) + return (false); + } + + if (model_coefficients[6] != -std::numeric_limits::max() && model_coefficients[6] < min_angle_) + return (false); + if (model_coefficients[6] != std::numeric_limits::max() && model_coefficients[6] > max_angle_) + return (false); + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelCone(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCone; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_cylinder.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_cylinder.hpp new file mode 100755 index 0000000..733f8d7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_cylinder.hpp @@ -0,0 +1,476 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCylinder::isSampleGood (const std::vector &) const +{ + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCylinder::computeModelCoefficients ( + const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 2 samples + if (samples.size () != 2) + { + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n"); + return (false); + } + + if (std::abs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits::epsilon () && + std::abs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits::epsilon () && + std::abs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits::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 (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir))); + + if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_) + return (false); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCylinder::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + distances.resize (indices_->size ()); + + 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); + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Approximate the distance from the point to the cylinder as the difference between + // dist(point,cylinder_axis) and cylinder radius + // @note need to revise this. + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + + double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); + + // Calculate the point's projection on the cylinder axis + float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = line_pt + k * line_dir; + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double d_normal = std::abs (getAngle3D (n, dir)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCylinder::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + 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); + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Approximate the distance from the point to the cylinder as the difference between + // dist(point,cylinder_axis) and cylinder radius + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); + + // Calculate the point's projection on the cylinder axis + float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = line_pt + k * line_dir; + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double d_normal = std::abs (getAngle3D (n, dir)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + if (distance < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = distance; + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelCylinder::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + std::size_t nr_p = 0; + + 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); + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Approximate the distance from the point to the cylinder as the difference between + // dist(point,cylinder_axis) and cylinder radius + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); + + // Calculate the point's projection on the cylinder axis + float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = line_pt + k * line_dir; + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector + double d_normal = std::abs (getAngle3D (n, dir)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + optimized_coefficients = model_coefficients; + + // Needs a set of valid model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + if (inliers.empty ()) + { + PCL_DEBUG ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n"); + return; + } + + OptimizationFunctor functor (this, inliers); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, float> lm (num_diff); + int info = lm.minimize (optimized_coefficients); + + // Compute the L2 norm of the residuals + PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n", + info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], + model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]); + + Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]); + line_dir.normalize (); + optimized_coefficients[3] = line_dir[0]; + optimized_coefficients[4] = line_dir[1]; + optimized_coefficients[5] = line_dir[2]; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCylinder::projectPoints ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%lu)!\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; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cylinder + for (const int &inlier : inliers) + { + Eigen::Vector4f p (input_->points[inlier].x, + input_->points[inlier].y, + input_->points[inlier].z, + 1); + + float k = (p.dot (line_dir) - ptdotdir) * dirdotdir; + + pcl::Vector4fMap pp = projected_points.points[inlier].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 (inliers.size ()); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the cylinder + for (std::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]; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCylinder::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid model coefficients + if (model_coefficients.size () != 7) + { + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return (false); + } + + for (const int &index : indices) + { + // Approximate the distance from the point to the cylinder as the difference between + // dist(point,cylinder_axis) and cylinder radius + // @note need to revise this. + Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0); + if (std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) + return (false); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::SampleConsensusModelCylinder::pointToLineDistance ( + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const +{ + 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); + return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelCylinder::projectPointToCylinder ( + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const +{ + 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 k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir); + pt_proj = line_pt + k * line_dir; + + Eigen::Vector4f dir = pt - pt_proj; + dir.normalize (); + + // Calculate the projection of the point onto the cylinder + pt_proj += dir * model_coefficients[6]; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelCylinder::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + // Check against template, if given + if (eps_angle_ > 0.0) + { + // Obtain the cylinder direction + Eigen::Vector4f coeff; + coeff[0] = model_coefficients[3]; + coeff[1] = model_coefficients[4]; + coeff[2] = model_coefficients[5]; + coeff[3] = 0; + + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + double angle_diff = std::abs (getAngle3D (axis, coeff)); + angle_diff = (std::min) (angle_diff, M_PI - angle_diff); + // Check whether the current cylinder model satisfies our angle threshold criterion with respect to the given axis + if (angle_diff > eps_angle_) + return (false); + } + + if (radius_min_ != -std::numeric_limits::max() && model_coefficients[6] < radius_min_) + return (false); + if (radius_max_ != std::numeric_limits::max() && model_coefficients[6] > radius_max_) + return (false); + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelCylinder(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCylinder; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_line.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_line.hpp new file mode 100755 index 0000000..30a1588 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_line.hpp @@ -0,0 +1,338 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelLine::isSampleGood (const std::vector &samples) const +{ + // Make sure that the two sample points are not identical + if ( + (input_->points[samples[0]].x != input_->points[samples[1]].x) + || + (input_->points[samples[0]].y != input_->points[samples[1]].y) + || + (input_->points[samples[0]].z != input_->points[samples[1]].z)) + { + return (true); + } + + return (false); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelLine::computeModelCoefficients ( + const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 2 samples + if (samples.size () != 2) + { + PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + if (std::abs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits::epsilon () && + std::abs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits::epsilon () && + std::abs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits::epsilon ()) + { + return (false); + } + + model_coefficients.resize (6); + model_coefficients[0] = input_->points[samples[0]].x; + model_coefficients[1] = input_->points[samples[0]].y; + model_coefficients[2] = input_->points[samples[0]].z; + + model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0]; + model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1]; + model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2]; + + model_coefficients.template tail<3> ().normalize (); + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelLine::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return; + + distances.resize (indices_->size ()); + + // Obtain the line point and direction + 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); + line_dir.normalize (); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + // Need to estimate sqrt here to keep MSAC and friends general + distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ()); + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelLine::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return; + + double sqr_threshold = threshold * threshold; + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + // Obtain the line point and direction + 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); + line_dir.normalize (); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); + + if (sqr_distance < sqr_threshold) + { + // Returns the indices of the points whose squared distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = sqr_distance; + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelLine::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return (0); + + double sqr_threshold = threshold * threshold; + + std::size_t nr_p = 0; + + // Obtain the line point and direction + 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); + line_dir.normalize (); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); + + if (sqr_distance < sqr_threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelLine::optimizeModelCoefficients ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + { + optimized_coefficients = model_coefficients; + return; + } + + // Need at least 2 points to estimate a line + if (inliers.size () <= 2) + { + PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + optimized_coefficients = model_coefficients; + return; + } + + optimized_coefficients.resize (6); + + // Compute the 3x3 covariance matrix + Eigen::Vector4f centroid; + compute3DCentroid (*input_, inliers, centroid); + Eigen::Matrix3f covariance_matrix; + computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix); + optimized_coefficients[0] = centroid[0]; + optimized_coefficients[1] = centroid[1]; + optimized_coefficients[2] = centroid[2]; + + // Extract the eigenvalues and eigenvectors + EIGEN_ALIGN16 Eigen::Vector3f eigen_values; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_values); + pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector); + //pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); + + optimized_coefficients.template tail<3> ().matrix () = eigen_vector; +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelLine::projectPoints ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid model coefficients + if (!isModelValid (model_coefficients)) + return; + + // Obtain the line point and direction + 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); + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + // 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; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the line + for (const int &inlier : inliers) + { + Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0); + // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); + + Eigen::Vector4f pp = line_pt + k * line_dir; + // Calculate the projection of the point on the line (pointProj = A + k * B) + projected_points.points[inlier].x = pp[0]; + projected_points.points[inlier].y = pp[1]; + projected_points.points[inlier].z = pp[2]; + } + } + else + { + // Allocate enough space and copy the basics + projected_points.points.resize (inliers.size ()); + projected_points.width = static_cast (inliers.size ()); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < inliers.size (); ++i) + { + Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); + // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); + + Eigen::Vector4f pp = line_pt + k * line_dir; + // Calculate the projection of the point on the line (pointProj = A + k * B) + projected_points.points[i].x = pp[0]; + projected_points.points[i].y = pp[1]; + projected_points.points[i].z = pp[2]; + } + } +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelLine::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return (false); + + // Obtain the line point and direction + 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); + line_dir.normalize (); + + double sqr_threshold = threshold * threshold; + // Iterate through the 3d points and calculate the distances from them to the line + for (const int &index : indices) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + if ((line_pt - input_->points[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold) + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp new file mode 100755 index 0000000..bee55e1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PARALLEL_PLANE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PARALLEL_PLANE_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelNormalParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + // Check against template, if given + if (eps_angle_ > 0.0) + { + // Obtain the plane normal + Eigen::Vector4f coeff = model_coefficients; + coeff[3] = 0; + coeff.normalize (); + + if (std::abs (axis_.dot (coeff)) < cos_angle_) + return (false); + } + + if (eps_dist_ > 0.0) + { + if (std::abs (-model_coefficients[3] - distance_from_origin_) > eps_dist_) + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelNormalParallelPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalParallelPlane; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PARALLEL_PLANE_H_ + + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_plane.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_plane.hpp new file mode 100755 index 0000000..e8ed669 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_plane.hpp @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelNormalPlane::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n"); + inliers.clear (); + return; + } + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + // Obtain the plane normal + Eigen::Vector4f coeff = model_coefficients; + coeff[3] = 0; + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < indices_->size (); ++i) + { + const PointT &pt = input_->points[(*indices_)[i]]; + const PointNT &nt = normals_->points[(*indices_)[i]]; + // Calculate the distance from the point to the plane normal as the dot product + // D = (P-A).N/|N| + Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); + Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); + double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]); + + // Calculate the angular distance between the point normal and the plane normal + double d_normal = std::abs (getAngle3D (n, coeff)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence + double weight = normal_distance_weight_ * (1.0 - nt.curvature); + + double distance = std::abs (weight * d_normal + (1.0 - weight) * d_euclid); + if (distance < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = distance; + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelNormalPlane::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n"); + return (0); + } + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + // Obtain the plane normal + Eigen::Vector4f coeff = model_coefficients; + coeff[3] = 0; + + std::size_t nr_p = 0; + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < indices_->size (); ++i) + { + const PointT &pt = input_->points[(*indices_)[i]]; + const PointNT &nt = normals_->points[(*indices_)[i]]; + // Calculate the distance from the point to the plane normal as the dot product + // D = (P-A).N/|N| + Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); + Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); + double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]); + + // Calculate the angular distance between the point normal and the plane normal + double d_normal = std::abs (getAngle3D (n, coeff)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence + double weight = normal_distance_weight_ * (1.0 - nt.curvature); + + if (std::abs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelNormalPlane::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n"); + return; + } + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + // Obtain the plane normal + Eigen::Vector4f coeff = model_coefficients; + coeff[3] = 0; + + distances.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < indices_->size (); ++i) + { + const PointT &pt = input_->points[(*indices_)[i]]; + const PointNT &nt = normals_->points[(*indices_)[i]]; + // Calculate the distance from the point to the plane normal as the dot product + // D = (P-A).N/|N| + Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); + Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); + double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]); + + // Calculate the angular distance between the point normal and the plane normal + double d_normal = std::abs (getAngle3D (n, coeff)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence + double weight = normal_distance_weight_ * (1.0 - nt.curvature); + + distances[i] = std::abs (weight * d_normal + (1.0 - weight) * d_euclid); + } +} + +#define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp new file mode 100755 index 0000000..c33f064 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp @@ -0,0 +1,226 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: sac_model_normal_sphere.hpp schrandt $ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelNormalSphere::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] No input dataset containing normals was given!\n"); + inliers.clear (); + return; + } + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + // Obtain the sphere center + Eigen::Vector4f center = model_coefficients; + center[3] = 0; + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the sphere center as the difference between + // dist(point,sphere_origin) and sphere_radius + Eigen::Vector4f p (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, + 0); + + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], + normals_->points[(*indices_)[i]].normal[1], + normals_->points[(*indices_)[i]].normal[2], + 0); + + Eigen::Vector4f n_dir = p - center; + double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]); + + // Calculate the angular distance between the point normal and the sphere normal + double d_normal = std::abs (getAngle3D (n, n_dir)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + if (distance < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = static_cast (distance); + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelNormalSphere::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n"); + return (0); + } + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return(0); + + + // Obtain the sphere centroid + Eigen::Vector4f center = model_coefficients; + center[3] = 0; + + std::size_t nr_p = 0; + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the sphere centroid as the difference between + // dist(point,sphere_origin) and sphere_radius + Eigen::Vector4f p (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, + 0); + + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], + normals_->points[(*indices_)[i]].normal[1], + normals_->points[(*indices_)[i]].normal[2], + 0); + + Eigen::Vector4f n_dir = (p-center); + double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]); + // + // Calculate the angular distance between the point normal and the sphere normal + double d_normal = std::abs (getAngle3D (n, n_dir)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelNormalSphere::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + if (!normals_) + { + PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n"); + return; + } + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + // Obtain the sphere centroid + Eigen::Vector4f center = model_coefficients; + center[3] = 0; + + distances.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the sphere as the difference between + // dist(point,sphere_origin) and sphere_radius + Eigen::Vector4f p (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, + 0); + + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], + normals_->points[(*indices_)[i]].normal[1], + normals_->points[(*indices_)[i]].normal[2], + 0); + + Eigen::Vector4f n_dir = (p-center); + double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]); + // + // Calculate the angular distance between the point normal and the sphere normal + double d_normal = std::abs (getAngle3D (n, n_dir)); + d_normal = (std::min) (d_normal, M_PI - d_normal); + + distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelNormalSphere::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + if (radius_min_ != -std::numeric_limits::max() && model_coefficients[3] < radius_min_) + return (false); + if (radius_max_ != std::numeric_limits::max() && model_coefficients[3] > radius_max_) + return (false); + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelNormalSphere(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalSphere; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_parallel_line.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_parallel_line.hpp new file mode 100755 index 0000000..1ab1a43 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_parallel_line.hpp @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_HPP_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_HPP_ + +#include +#include + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelParallelLine::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + SampleConsensusModelLine::selectWithinDistance (model_coefficients, threshold, inliers); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelParallelLine::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + return (SampleConsensusModelLine::countWithinDistance (model_coefficients, threshold)); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelParallelLine::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + SampleConsensusModelLine::getDistancesToModel (model_coefficients, distances); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelParallelLine::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + // Check against template, if given + if (eps_angle_ > 0.0) + { + // Obtain the line direction + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + double angle_diff = std::abs (getAngle3D (axis, line_dir)); + angle_diff = (std::min) (angle_diff, M_PI - angle_diff); + // Check whether the current line model satisfies our angle threshold criterion with respect to the given axis + if (angle_diff > eps_angle_) + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelParallelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelParallelLine; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_LINE_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp new file mode 100755 index 0000000..3de6be6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp @@ -0,0 +1,114 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + a + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_PLANE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_PLANE_H_ + +#include + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelParallelPlane::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + SampleConsensusModelPlane::selectWithinDistance (model_coefficients, threshold, inliers); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelParallelPlane::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + return (SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold)); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelParallelPlane::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + SampleConsensusModelPlane::getDistancesToModel (model_coefficients, distances); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + // Check against template, if given + if (eps_angle_ > 0.0) + { + // Obtain the plane normal + Eigen::Vector4f coeff = model_coefficients; + coeff[3] = 0; + coeff.normalize (); + + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + if (std::abs (axis.dot (coeff)) > sin_angle_) + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelParallelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelParallelPlane; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PARALLEL_PLANE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp new file mode 100755 index 0000000..e7bc7f0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PERPENDICULAR_PLANE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PERPENDICULAR_PLANE_H_ + +#include + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelPerpendicularPlane::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + SampleConsensusModelPlane::selectWithinDistance (model_coefficients, threshold, inliers); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelPerpendicularPlane::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + return (SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold)); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelPerpendicularPlane::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + + SampleConsensusModelPlane::getDistancesToModel (model_coefficients, distances); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelPerpendicularPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const +{ + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + // Check against template, if given + if (eps_angle_ > 0.0) + { + // Obtain the plane normal + Eigen::Vector4f coeff = model_coefficients; + coeff[3] = 0; + + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + double angle_diff = std::abs (getAngle3D (axis, coeff)); + angle_diff = (std::min) (angle_diff, M_PI - angle_diff); + // Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis + if (angle_diff > eps_angle_) + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelPerpendicularPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPerpendicularPlane; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PERPENDICULAR_PLANE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_plane.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_plane.hpp new file mode 100755 index 0000000..5b99792 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_plane.hpp @@ -0,0 +1,371 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_ + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelPlane::isSampleGood (const std::vector &samples) const +{ + // Need an extra check in case the sample selection is empty + if (samples.empty ()) + return (false); + // Get the values at the two points + pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); + pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); + pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); + + Eigen::Array4f dy1dy2 = (p1-p0) / (p2-p0); + + return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelPlane::computeModelCoefficients ( + const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 3 samples + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); + pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); + pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); + + // Compute the segment values (in 3d) between p1 and p0 + Eigen::Array4f p1p0 = p1 - p0; + // Compute the segment values (in 3d) between p2 and p0 + Eigen::Array4f p2p0 = p2 - p0; + + // Avoid some crashes by checking for collinearity here + Eigen::Array4f dy1dy2 = p1p0 / p2p0; + if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ) // Check for collinearity + return (false); + + // Compute the plane coefficients from the 3 given points in a straightforward manner + // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1) + model_coefficients.resize (4); + model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1]; + model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2]; + model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0]; + model_coefficients[3] = 0; + + // Normalize + model_coefficients.normalize (); + + // ... + d = 0 + model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ())); + + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelPlane::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != model_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + distances.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the plane normal as the dot product + // D = (P-A).N/|N| + /*distances[i] = std::abs (model_coefficients[0] * input_->points[(*indices_)[i]].x + + model_coefficients[1] * input_->points[(*indices_)[i]].y + + model_coefficients[2] * input_->points[(*indices_)[i]].z + + model_coefficients[3]);*/ + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, + 1); + distances[i] = std::abs (model_coefficients.dot (pt)); + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelPlane::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != model_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the plane normal as the dot product + // D = (P-A).N/|N| + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, + 1); + + float distance = std::abs (model_coefficients.dot (pt)); + + if (distance < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = static_cast (distance); + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelPlane::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != model_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return (0); + } + + std::size_t nr_p = 0; + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the plane normal as the dot product + // D = (P-A).N/|N| + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, + 1); + if (std::abs (model_coefficients.dot (pt)) < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != model_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + optimized_coefficients = model_coefficients; + return; + } + + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); + optimized_coefficients = model_coefficients; + return; + } + + Eigen::Vector4f plane_parameters; + + // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f xyz_centroid; + + computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, xyz_centroid); + + // Compute the model coefficients + EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + + // Hessian form (D = nc . p_plane (centroid here) + p) + optimized_coefficients.resize (4); + optimized_coefficients[0] = eigen_vector [0]; + optimized_coefficients[1] = eigen_vector [1]; + optimized_coefficients[2] = eigen_vector [2]; + optimized_coefficients[3] = 0; + optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid); + + // Make sure it results in a valid model + if (!isModelValid (optimized_coefficients)) + { + optimized_coefficients = model_coefficients; + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelPlane::projectPoints ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != model_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + + // normalize the vector perpendicular to the plane... + mc.normalize (); + // ... and store the resulting normal as a local copy of the model coefficients + Eigen::Vector4f tmp_mc = model_coefficients; + tmp_mc[0] = mc[0]; + tmp_mc[1] = mc[1]; + tmp_mc[2] = mc[2]; + + // 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; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < input_->points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (const int &inlier : inliers) + { + // Calculate the distance from the point to the plane + Eigen::Vector4f p (input_->points[inlier].x, + input_->points[inlier].y, + input_->points[inlier].z, + 1); + // use normalized coefficients to calculate the scalar projection + float distance_to_plane = tmp_mc.dot (p); + + pcl::Vector4fMap pp = projected_points.points[inlier].getVector4fMap (); + pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe + } + } + else + { + // Allocate enough space and copy the basics + projected_points.points.resize (inliers.size ()); + projected_points.width = static_cast (inliers.size ()); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the plane + for (std::size_t i = 0; i < inliers.size (); ++i) + { + // Calculate the distance from the point to the plane + Eigen::Vector4f p (input_->points[inliers[i]].x, + input_->points[inliers[i]].y, + input_->points[inliers[i]].z, + 1); + // use normalized coefficients to calculate the scalar projection + float distance_to_plane = tmp_mc.dot (p); + + pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap (); + pp.matrix () = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe + } + } +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelPlane::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid set of model coefficients + if (model_coefficients.size () != model_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return (false); + } + + for (const int &index : indices) + { + Eigen::Vector4f pt (input_->points[index].x, + input_->points[index].y, + input_->points[index].z, + 1); + if (std::abs (model_coefficients.dot (pt)) > threshold) + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_registration.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_registration.hpp new file mode 100755 index 0000000..5b9d1f7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_registration.hpp @@ -0,0 +1,304 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelRegistration::isSampleGood (const std::vector &samples) const +{ + using namespace pcl::common; + using namespace pcl::traits; + + PointT p10 = input_->points[samples[1]] - input_->points[samples[0]]; + PointT p20 = input_->points[samples[2]] - input_->points[samples[0]]; + PointT p21 = input_->points[samples[2]] - input_->points[samples[1]]; + + return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ && + (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ && + (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelRegistration::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + if (!target_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n"); + return (false); + } + // Need 3 samples + if (samples.size () != 3) + return (false); + + std::vector indices_tgt (3); + for (int i = 0; i < 3; ++i) + indices_tgt[i] = correspondences_.at (samples[i]); + + estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients); + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelRegistration::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + if (indices_->size () != indices_tgt_->size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); + distances.clear (); + return; + } + if (!target_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n"); + return; + } + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + distances.resize (indices_->size ()); + + // Get the 4x4 transformation + Eigen::Matrix4f transform; + transform.row (0).matrix () = model_coefficients.segment<4>(0); + transform.row (1).matrix () = model_coefficients.segment<4>(4); + transform.row (2).matrix () = model_coefficients.segment<4>(8); + transform.row (3).matrix () = model_coefficients.segment<4>(12); + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, 1); + Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, + target_->points[(*indices_tgt_)[i]].y, + target_->points[(*indices_tgt_)[i]].z, 1); + + Eigen::Vector4f p_tr (transform * pt_src); + // Calculate the distance from the transformed point to its correspondence + // need to compute the real norm here to keep MSAC and friends general + distances[i] = (p_tr - pt_tgt).norm (); + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelRegistration::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + if (indices_->size () != indices_tgt_->size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); + inliers.clear (); + return; + } + if (!target_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n"); + return; + } + + double thresh = threshold * threshold; + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + Eigen::Matrix4f transform; + transform.row (0).matrix () = model_coefficients.segment<4>(0); + transform.row (1).matrix () = model_coefficients.segment<4>(4); + transform.row (2).matrix () = model_coefficients.segment<4>(8); + transform.row (3).matrix () = model_coefficients.segment<4>(12); + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, 1); + Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, + target_->points[(*indices_tgt_)[i]].y, + target_->points[(*indices_tgt_)[i]].z, 1); + + Eigen::Vector4f p_tr (transform * pt_src); + + float distance = (p_tr - pt_tgt).squaredNorm (); + // Calculate the distance from the transformed point to its correspondence + if (distance < thresh) + { + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = static_cast (distance); + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelRegistration::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + if (indices_->size () != indices_tgt_->size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); + return (0); + } + if (!target_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n"); + return (0); + } + + double thresh = threshold * threshold; + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + Eigen::Matrix4f transform; + transform.row (0).matrix () = model_coefficients.segment<4>(0); + transform.row (1).matrix () = model_coefficients.segment<4>(4); + transform.row (2).matrix () = model_coefficients.segment<4>(8); + transform.row (3).matrix () = model_coefficients.segment<4>(12); + + std::size_t nr_p = 0; + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, 1); + Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, + target_->points[(*indices_tgt_)[i]].y, + target_->points[(*indices_tgt_)[i]].z, 1); + + Eigen::Vector4f p_tr (transform * pt_src); + // Calculate the distance from the transformed point to its correspondence + if ((p_tr - pt_tgt).squaredNorm () < thresh) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + if (indices_->size () != indices_tgt_->size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); + optimized_coefficients = model_coefficients; + return; + } + + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients) || !target_) + { + optimized_coefficients = model_coefficients; + return; + } + + std::vector indices_src (inliers.size ()); + std::vector indices_tgt (inliers.size ()); + for (std::size_t i = 0; i < inliers.size (); ++i) + { + indices_src[i] = inliers[i]; + indices_tgt[i] = correspondences_.at (indices_src[i]); + } + + estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelRegistration::estimateRigidTransformationSVD ( + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Eigen::VectorXf &transform) const +{ + transform.resize (16); + + Eigen::Matrix src (3, indices_src.size ()); + Eigen::Matrix tgt (3, indices_tgt.size ()); + + for (std::size_t i = 0; i < indices_src.size (); ++i) + { + src (0, i) = cloud_src[indices_src[i]].x; + src (1, i) = cloud_src[indices_src[i]].y; + src (2, i) = cloud_src[indices_src[i]].z; + + tgt (0, i) = cloud_tgt[indices_tgt[i]].x; + tgt (1, i) = cloud_tgt[indices_tgt[i]].y; + tgt (2, i) = cloud_tgt[indices_tgt[i]].z; + } + + // Call Umeyama directly from Eigen + Eigen::Matrix4d transformation_matrix = pcl::umeyama (src, tgt, false); + + // Return the correct transformation + transform.segment<4> (0).matrix () = transformation_matrix.cast ().row (0); + transform.segment<4> (4).matrix () = transformation_matrix.cast ().row (1); + transform.segment<4> (8).matrix () = transformation_matrix.cast ().row (2); + transform.segment<4> (12).matrix () = transformation_matrix.cast ().row (3); +} + +#define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_registration_2d.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_registration_2d.hpp new file mode 100755 index 0000000..0a12544 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_registration_2d.hpp @@ -0,0 +1,230 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelRegistration2D::isSampleGood (const std::vector&) const +{ + return (true); + //using namespace pcl::common; + //using namespace pcl::traits; + + //PointT p10 = input_->points[samples[1]] - input_->points[samples[0]]; + //PointT p20 = input_->points[samples[2]] - input_->points[samples[0]]; + //PointT p21 = input_->points[samples[2]] - input_->points[samples[1]]; + + //return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ && + // (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ && + // (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + PCL_INFO ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel]\n"); + if (indices_->size () != indices_tgt_->size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); + distances.clear (); + return; + } + if (!target_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::getDistanceToModel] No target dataset given!\n"); + return; + } + + distances.resize (indices_->size ()); + + // Get the 4x4 transformation + Eigen::Matrix4f transform; + transform.row (0).matrix () = model_coefficients.segment<4>(0); + transform.row (1).matrix () = model_coefficients.segment<4>(4); + transform.row (2).matrix () = model_coefficients.segment<4>(8); + transform.row (3).matrix () = model_coefficients.segment<4>(12); + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, 1); + + Eigen::Vector4f p_tr (transform * pt_src); + + // Project the point on the image plane + Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); + Eigen::Vector3f uv (projection_matrix_ * p_tr3); + + if (uv[2] < 0) + continue; + + uv /= uv[2]; + + // Calculate the distance from the transformed point to its correspondence + // need to compute the real norm here to keep MSAC and friends general + distances[i] = std::sqrt ((uv[0] - target_->points[(*indices_tgt_)[i]].u) * + (uv[0] - target_->points[(*indices_tgt_)[i]].u) + + (uv[1] - target_->points[(*indices_tgt_)[i]].v) * + (uv[1] - target_->points[(*indices_tgt_)[i]].v)); + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + if (indices_->size () != indices_tgt_->size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); + inliers.clear (); + return; + } + if (!target_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::selectWithinDistance] No target dataset given!\n"); + return; + } + + double thresh = threshold * threshold; + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + Eigen::Matrix4f transform; + transform.row (0).matrix () = model_coefficients.segment<4>(0); + transform.row (1).matrix () = model_coefficients.segment<4>(4); + transform.row (2).matrix () = model_coefficients.segment<4>(8); + transform.row (3).matrix () = model_coefficients.segment<4>(12); + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, 1); + + Eigen::Vector4f p_tr (transform * pt_src); + + // Project the point on the image plane + Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); + Eigen::Vector3f uv (projection_matrix_ * p_tr3); + + if (uv[2] < 0) + continue; + + uv /= uv[2]; + + double distance = ((uv[0] - target_->points[(*indices_tgt_)[i]].u) * + (uv[0] - target_->points[(*indices_tgt_)[i]].u) + + (uv[1] - target_->points[(*indices_tgt_)[i]].v) * + (uv[1] - target_->points[(*indices_tgt_)[i]].v)); + + // Calculate the distance from the transformed point to its correspondence + if (distance < thresh) + { + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = distance; + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelRegistration2D::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + if (indices_->size () != indices_tgt_->size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ()); + return (0); + } + if (!target_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::countWithinDistance] No target dataset given!\n"); + return (0); + } + + double thresh = threshold * threshold; + + Eigen::Matrix4f transform; + transform.row (0).matrix () = model_coefficients.segment<4>(0); + transform.row (1).matrix () = model_coefficients.segment<4>(4); + transform.row (2).matrix () = model_coefficients.segment<4>(8); + transform.row (3).matrix () = model_coefficients.segment<4>(12); + + std::size_t nr_p = 0; + + for (std::size_t i = 0; i < indices_->size (); ++i) + { + Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, + input_->points[(*indices_)[i]].y, + input_->points[(*indices_)[i]].z, 1); + + Eigen::Vector4f p_tr (transform * pt_src); + + // Project the point on the image plane + Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); + Eigen::Vector3f uv (projection_matrix_ * p_tr3); + + if (uv[2] < 0) + continue; + + uv /= uv[2]; + + // Calculate the distance from the transformed point to its correspondence + if (((uv[0] - target_->points[(*indices_tgt_)[i]].u) * + (uv[0] - target_->points[(*indices_tgt_)[i]].u) + + (uv[1] - target_->points[(*indices_tgt_)[i]].v) * + (uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh) + ++nr_p; + } + return (nr_p); +} + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_sphere.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_sphere.hpp new file mode 100755 index 0000000..fb392ef --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_sphere.hpp @@ -0,0 +1,308 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelSphere::isSampleGood (const std::vector &) const +{ + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelSphere::computeModelCoefficients ( + const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 4 samples + if (samples.size () != 4) + { + PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + Eigen::Matrix4f temp; + for (int i = 0; i < 4; i++) + { + temp (i, 0) = input_->points[samples[i]].x; + temp (i, 1) = input_->points[samples[i]].y; + temp (i, 2) = input_->points[samples[i]].z; + temp (i, 3) = 1; + } + float m11 = temp.determinant (); + if (m11 == 0) + return (false); // the points don't define a sphere! + + for (int i = 0; i < 4; ++i) + temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) + + (input_->points[samples[i]].y) * (input_->points[samples[i]].y) + + (input_->points[samples[i]].z) * (input_->points[samples[i]].z); + float m12 = temp.determinant (); + + for (int i = 0; i < 4; ++i) + { + temp (i, 1) = temp (i, 0); + temp (i, 0) = input_->points[samples[i]].x; + } + float m13 = temp.determinant (); + + for (int i = 0; i < 4; ++i) + { + temp (i, 2) = temp (i, 1); + temp (i, 1) = input_->points[samples[i]].y; + } + float m14 = temp.determinant (); + + for (int i = 0; i < 4; ++i) + { + temp (i, 0) = temp (i, 2); + temp (i, 1) = input_->points[samples[i]].x; + temp (i, 2) = input_->points[samples[i]].y; + temp (i, 3) = input_->points[samples[i]].z; + } + float m15 = temp.determinant (); + + // Center (x , y, z) + model_coefficients.resize (4); + model_coefficients[0] = 0.5f * m12 / m11; + model_coefficients[1] = 0.5f * m13 / m11; + model_coefficients[2] = 0.5f * m14 / m11; + // Radius + model_coefficients[3] = std::sqrt (model_coefficients[0] * model_coefficients[0] + + model_coefficients[1] * model_coefficients[1] + + model_coefficients[2] * model_coefficients[2] - m15 / m11); + + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelSphere::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + distances.clear (); + return; + } + distances.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + // Calculate the distance from the point to the sphere as the difference between + //dist(point,sphere_origin) and sphere_radius + distances[i] = std::abs (std::sqrt ( + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + + + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) + + + ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) * + ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) + ) - model_coefficients[3]); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelSphere::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + { + inliers.clear (); + return; + } + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + double distance = std::abs (std::sqrt ( + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + + + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) + + + ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) * + ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) + ) - model_coefficients[3]); + // Calculate the distance from the point to the sphere as the difference between + // dist(point,sphere_origin) and sphere_radius + if (distance < threshold) + { + // Returns the indices of the points whose distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = static_cast (distance); + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelSphere::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Check if the model is valid given the user constraints + if (!isModelValid (model_coefficients)) + return (0); + + std::size_t nr_p = 0; + + // Iterate through the 3d points and calculate the distances from them to the sphere + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the sphere as the difference between + // dist(point,sphere_origin) and sphere_radius + if (std::abs (std::sqrt ( + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) * + ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) + + + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) * + ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) + + + ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) * + ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) + ) - model_coefficients[3]) < threshold) + nr_p++; + } + return (nr_p); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + optimized_coefficients = model_coefficients; + + // Needs a set of valid model coefficients + if (model_coefficients.size () != 4) + { + PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + // Need at least 4 samples + if (inliers.size () <= 4) + { + PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + return; + } + + OptimizationFunctor functor (this, inliers); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, float> lm (num_diff); + int info = lm.minimize (optimized_coefficients); + + // Compute the L2 norm of the residuals + PCL_DEBUG ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g \nFinal solution: %g %g %g %g\n", + info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3]); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelSphere::projectPoints ( + const std::vector &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const +{ + // Needs a valid model coefficients + if (model_coefficients.size () != 4) + { + PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return; + } + + // Allocate enough space and copy the basics + projected_points.points.resize (input_->points.size ()); + projected_points.header = input_->header; + projected_points.width = input_->width; + projected_points.height = input_->height; + projected_points.is_dense = input_->is_dense; + + PCL_WARN ("[pcl::SampleConsensusModelSphere::projectPoints] Not implemented yet.\n"); + projected_points.points = input_->points; +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelSphere::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid model coefficients + if (model_coefficients.size () != 4) + { + PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + return (false); + } + + for (const int &index : indices) + // Calculate the distance from the point to the sphere as the difference between + //dist(point,sphere_origin) and sphere_radius + if (std::abs (sqrt ( + ( input_->points[index].x - model_coefficients[0] ) * + ( input_->points[index].x - model_coefficients[0] ) + + ( input_->points[index].y - model_coefficients[1] ) * + ( input_->points[index].y - model_coefficients[1] ) + + ( input_->points[index].z - model_coefficients[2] ) * + ( input_->points[index].z - model_coefficients[2] ) + ) - model_coefficients[3]) > threshold) + return (false); + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelSphere(T) template class PCL_EXPORTS pcl::SampleConsensusModelSphere; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_stick.hpp b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_stick.hpp new file mode 100755 index 0000000..8d030ea --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/impl/sac_model_stick.hpp @@ -0,0 +1,366 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: sac_model_line.hpp 2328 2011-08-31 08:11:00Z rusu $ + * + */ + +#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ +#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelStick::isSampleGood (const std::vector &samples) const +{ + if ( + (input_->points[samples[0]].x != input_->points[samples[1]].x) + && + (input_->points[samples[0]].y != input_->points[samples[1]].y) + && + (input_->points[samples[0]].z != input_->points[samples[1]].z)) + return (true); + + return (false); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelStick::computeModelCoefficients ( + const std::vector &samples, Eigen::VectorXf &model_coefficients) const +{ + // Need 2 samples + if (samples.size () != 2) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); + return (false); + } + + model_coefficients.resize (7); + model_coefficients[0] = input_->points[samples[0]].x; + model_coefficients[1] = input_->points[samples[0]].y; + model_coefficients[2] = input_->points[samples[0]].z; + + model_coefficients[3] = input_->points[samples[1]].x; + model_coefficients[4] = input_->points[samples[1]].y; + model_coefficients[5] = input_->points[samples[1]].z; + +// model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0]; +// model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1]; +// model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2]; + +// model_coefficients.template segment<3> (3).normalize (); + // We don't care about model_coefficients[6] which is the width (radius) of the stick + + return (true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelStick::getDistancesToModel ( + const Eigen::VectorXf &model_coefficients, std::vector &distances) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return; + + float sqr_threshold = static_cast (radius_max_ * radius_max_); + distances.resize (indices_->size ()); + + // Obtain the line point and direction + 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); + line_dir.normalize (); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + float sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); + + if (sqr_distance < sqr_threshold) + // Need to estimate sqrt here to keep MSAC and friends general + distances[i] = sqrt (sqr_distance); + else + // Penalize outliers by doubling the distance + distances[i] = 2 * sqrt (sqr_distance); + } +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelStick::selectWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return; + + float sqr_threshold = static_cast (threshold * threshold); + + int nr_p = 0; + inliers.resize (indices_->size ()); + error_sqr_dists_.resize (indices_->size ()); + + // Obtain the line point and direction + Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_dir = line_pt2 - line_pt1; + //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); + line_dir.normalize (); + //float norm = line_dir.squaredNorm (); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1; + //float u = dir.dot (line_dir); + + // If the point falls outside of the segment, ignore it + //if (u < 0.0f || u > 1.0f) + // continue; + + float sqr_distance = dir.cross3 (line_dir).squaredNorm (); + if (sqr_distance < sqr_threshold) + { + // Returns the indices of the points whose squared distances are smaller than the threshold + inliers[nr_p] = (*indices_)[i]; + error_sqr_dists_[nr_p] = static_cast (sqr_distance); + ++nr_p; + } + } + inliers.resize (nr_p); + error_sqr_dists_.resize (nr_p); +} + +/////////////////////////////////////////////////////////////////////////// +template std::size_t +pcl::SampleConsensusModelStick::countWithinDistance ( + const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return (0); + + float sqr_threshold = static_cast (threshold * threshold); + + std::size_t nr_i = 0, nr_o = 0; + + // Obtain the line point and direction + Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_dir = line_pt2 - line_pt1; + line_dir.normalize (); + + //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); + //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < indices_->size (); ++i) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1; + //float u = dir.dot (line_dir); + + // If the point falls outside of the segment, ignore it + //if (u < 0.0f || u > 1.0f) + // continue; + + float sqr_distance = dir.cross3 (line_dir).squaredNorm (); + // Use a larger threshold (4 times the radius) to get more points in + if (sqr_distance < sqr_threshold) + nr_i++; + else if (sqr_distance < 4 * sqr_threshold) + nr_o++; + } + + return (nr_i <= nr_o ? 0 : nr_i - nr_o); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelStick::optimizeModelCoefficients ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + { + optimized_coefficients = model_coefficients; + return; + } + + // Need at least 2 points to estimate a line + if (inliers.size () <= 2) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + optimized_coefficients = model_coefficients; + return; + } + + optimized_coefficients.resize (7); + + // Compute the 3x3 covariance matrix + Eigen::Vector4f centroid; + Eigen::Matrix3f covariance_matrix; + + computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, centroid); + + optimized_coefficients[0] = centroid[0]; + optimized_coefficients[1] = centroid[1]; + optimized_coefficients[2] = centroid[2]; + + // Extract the eigenvalues and eigenvectors + Eigen::Vector3f eigen_values; + Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_values); + pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector); + + optimized_coefficients.template segment<3> (3).matrix () = eigen_vector; +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::SampleConsensusModelStick::projectPoints ( + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const +{ + // Needs a valid model coefficients + if (!isModelValid (model_coefficients)) + return; + + // Obtain the line point and direction + 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); + + projected_points.header = input_->header; + projected_points.is_dense = input_->is_dense; + + // 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; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < projected_points.points.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the line + for (const int &inlier : inliers) + { + Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0); + // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); + + Eigen::Vector4f pp = line_pt + k * line_dir; + // Calculate the projection of the point on the line (pointProj = A + k * B) + projected_points.points[inlier].x = pp[0]; + projected_points.points[inlier].y = pp[1]; + projected_points.points[inlier].z = pp[2]; + } + } + else + { + // Allocate enough space and copy the basics + projected_points.points.resize (inliers.size ()); + projected_points.width = static_cast (inliers.size ()); + projected_points.height = 1; + + using FieldList = typename pcl::traits::fieldList::type; + // Iterate over each point + for (std::size_t i = 0; i < inliers.size (); ++i) + // Iterate over each dimension + pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + + // Iterate through the 3d points and calculate the distances from them to the line + for (std::size_t i = 0; i < inliers.size (); ++i) + { + Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); + // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); + + Eigen::Vector4f pp = line_pt + k * line_dir; + // Calculate the projection of the point on the line (pointProj = A + k * B) + projected_points.points[i].x = pp[0]; + projected_points.points[i].y = pp[1]; + projected_points.points[i].z = pp[2]; + } + } +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::SampleConsensusModelStick::doSamplesVerifyModel ( + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const +{ + // Needs a valid set of model coefficients + if (!isModelValid (model_coefficients)) + return (false); + + // Obtain the line point and direction + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); + //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + line_dir.normalize (); + + float sqr_threshold = static_cast (threshold * threshold); + // Iterate through the 3d points and calculate the distances from them to the line + for (const int &index : indices) + { + // Calculate the distance from the point to the line + // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) + if ((line_pt - input_->points[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold) + return (false); + } + + return (true); +} + +#define PCL_INSTANTIATE_SampleConsensusModelStick(T) template class PCL_EXPORTS pcl::SampleConsensusModelStick; + +#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/lmeds.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/lmeds.h new file mode 100755 index 0000000..b83a026 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/lmeds.h @@ -0,0 +1,103 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b LeastMedianSquares represents an implementation of the LMedS (Least Median of Squares) algorithm. LMedS + * is a RANSAC-like model-fitting algorithm that can tolerate up to 50% outliers without requiring thresholds to be + * set. See Andrea Fusiello's "Elements of Geometric Computer Vision" + * (http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/FUSIELLO4/tutorial.html#x1-520007) for more details. + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class LeastMedianSquares : public SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SampleConsensus::max_iterations_; + using SampleConsensus::threshold_; + using SampleConsensus::iterations_; + using SampleConsensus::sac_model_; + using SampleConsensus::model_; + using SampleConsensus::model_coefficients_; + using SampleConsensus::inliers_; + + /** \brief LMedS (Least Median of Squares) main constructor + * \param[in] model a Sample Consensus model + */ + LeastMedianSquares (const SampleConsensusModelPtr &model) + : SampleConsensus (model) + { + // Maximum number of trials before we give up. + max_iterations_ = 50; + } + + /** \brief LMedS (Least Median of Squares) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + */ + LeastMedianSquares (const SampleConsensusModelPtr &model, double threshold) + : SampleConsensus (model, threshold) + { + // Maximum number of trials before we give up. + max_iterations_ = 50; + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/method_types.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/method_types.h new file mode 100755 index 0000000..0eb0e47 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/method_types.h @@ -0,0 +1,52 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +namespace pcl +{ + const static int SAC_RANSAC = 0; + const static int SAC_LMEDS = 1; + const static int SAC_MSAC = 2; + const static int SAC_RRANSAC = 3; + const static int SAC_RMSAC = 4; + const static int SAC_MLESAC = 5; + const static int SAC_PROSAC = 6; +} diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/mlesac.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/mlesac.h new file mode 100755 index 0000000..cfecb9b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/mlesac.h @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b MaximumLikelihoodSampleConsensus represents an implementation of the MLESAC (Maximum Likelihood + * Estimator SAmple Consensus) algorithm, as described in: "MLESAC: A new robust estimator with application to + * estimating image geometry", P.H.S. Torr and A. Zisserman, Computer Vision and Image Understanding, vol 78, 2000. + * \note MLESAC is useful in situations where most of the data samples belong to the model, and a fast outlier rejection algorithm is needed. + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class MaximumLikelihoodSampleConsensus : public SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SampleConsensus::max_iterations_; + using SampleConsensus::threshold_; + using SampleConsensus::iterations_; + using SampleConsensus::sac_model_; + using SampleConsensus::model_; + using SampleConsensus::model_coefficients_; + using SampleConsensus::inliers_; + using SampleConsensus::probability_; + + /** \brief MLESAC (Maximum Likelihood Estimator SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + */ + MaximumLikelihoodSampleConsensus (const SampleConsensusModelPtr &model) : + SampleConsensus (model), + iterations_EM_ (3), // Max number of EM (Expectation Maximization) iterations + sigma_ (0) + { + max_iterations_ = 10000; // Maximum number of trials before we give up. + } + + /** \brief MLESAC (Maximum Likelihood Estimator SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + */ + MaximumLikelihoodSampleConsensus (const SampleConsensusModelPtr &model, double threshold) : + SampleConsensus (model, threshold), + iterations_EM_ (3), // Max number of EM (Expectation Maximization) iterations + sigma_ (0) + { + max_iterations_ = 10000; // Maximum number of trials before we give up. + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + + /** \brief Set the number of EM iterations. + * \param[in] iterations the number of EM iterations + */ + inline void + setEMIterations (int iterations) { iterations_EM_ = iterations; } + + /** \brief Get the number of EM iterations. */ + inline int + getEMIterations () const { return (iterations_EM_); } + + + protected: + /** \brief Compute the median absolute deviation: + * \f[ + * MAD = \sigma * median_i (| Xi - median_j(Xj) |) + * \f] + * \note Sigma needs to be chosen carefully (a good starting sigma value is 1.4826) + * \param[in] cloud the point cloud data message + * \param[in] indices the set of point indices to use + * \param[in] sigma the sigma value + */ + double + computeMedianAbsoluteDeviation (const PointCloudConstPtr &cloud, + const IndicesPtr &indices, + double sigma) const; + + /** \brief Determine the minimum and maximum 3D bounding box coordinates for a given set of points + * \param[in] cloud the point cloud message + * \param[in] indices the set of point indices to use + * \param[out] min_p the resultant minimum bounding box coordinates + * \param[out] max_p the resultant maximum bounding box coordinates + */ + void + getMinMax (const PointCloudConstPtr &cloud, + const IndicesPtr &indices, + Eigen::Vector4f &min_p, + Eigen::Vector4f &max_p) const; + + /** \brief Compute the median value of a 3D point cloud using a given set point indices and return it as a Point32. + * \param[in] cloud the point cloud data message + * \param[in] indices the point indices + * \param[out] median the resultant median value + */ + void + computeMedian (const PointCloudConstPtr &cloud, + const IndicesPtr &indices, + Eigen::Vector4f &median) const; + + private: + /** \brief Maximum number of EM (Expectation Maximization) iterations. */ + int iterations_EM_; + /** \brief The MLESAC sigma parameter. */ + double sigma_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/model_types.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/model_types.h new file mode 100755 index 0000000..27670b6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/model_types.h @@ -0,0 +1,66 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +namespace pcl +{ + enum SacModel + { + SACMODEL_PLANE, + SACMODEL_LINE, + SACMODEL_CIRCLE2D, + SACMODEL_CIRCLE3D, + SACMODEL_SPHERE, + SACMODEL_CYLINDER, + SACMODEL_CONE, + SACMODEL_TORUS, + SACMODEL_PARALLEL_LINE, + SACMODEL_PERPENDICULAR_PLANE, + SACMODEL_PARALLEL_LINES, + SACMODEL_NORMAL_PLANE, + SACMODEL_NORMAL_SPHERE, + SACMODEL_REGISTRATION, + SACMODEL_REGISTRATION_2D, + SACMODEL_PARALLEL_PLANE, + SACMODEL_NORMAL_PARALLEL_PLANE, + SACMODEL_STICK + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/msac.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/msac.h new file mode 100755 index 0000000..0fa427c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/msac.h @@ -0,0 +1,104 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b MEstimatorSampleConsensus represents an implementation of the MSAC (M-estimator SAmple Consensus) + * algorithm, as described in: "MLESAC: A new robust estimator with application to estimating image geometry", P.H.S. + * Torr and A. Zisserman, Computer Vision and Image Understanding, vol 78, 2000. + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class MEstimatorSampleConsensus : public SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SampleConsensus::max_iterations_; + using SampleConsensus::threshold_; + using SampleConsensus::iterations_; + using SampleConsensus::sac_model_; + using SampleConsensus::model_; + using SampleConsensus::model_coefficients_; + using SampleConsensus::inliers_; + using SampleConsensus::probability_; + + /** \brief MSAC (M-estimator SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + */ + MEstimatorSampleConsensus (const SampleConsensusModelPtr &model) + : SampleConsensus (model) + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief MSAC (M-estimator SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + */ + MEstimatorSampleConsensus (const SampleConsensusModelPtr &model, double threshold) + : SampleConsensus (model, threshold) + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/prosac.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/prosac.h new file mode 100755 index 0000000..2929d98 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/prosac.h @@ -0,0 +1,103 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm, as + * described in: "Matching with PROSAC – Progressive Sample Consensus", Chum, O. and Matas, J.G., CVPR, I: 220-226 + * 2005. + * \author Vincent Rabaud + * \ingroup sample_consensus + */ + template + class ProgressiveSampleConsensus : public SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using SampleConsensus::max_iterations_; + using SampleConsensus::threshold_; + using SampleConsensus::iterations_; + using SampleConsensus::sac_model_; + using SampleConsensus::model_; + using SampleConsensus::model_coefficients_; + using SampleConsensus::inliers_; + using SampleConsensus::probability_; + + /** \brief PROSAC (Progressive SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + */ + ProgressiveSampleConsensus (const SampleConsensusModelPtr &model) : + SampleConsensus (model) + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief PROSAC (Progressive SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + */ + ProgressiveSampleConsensus (const SampleConsensusModelPtr &model, double threshold) : + SampleConsensus (model, threshold) + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/ransac.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/ransac.h new file mode 100755 index 0000000..23c0302 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/ransac.h @@ -0,0 +1,105 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b RandomSampleConsensus represents an implementation of the RANSAC (RANdom SAmple Consensus) algorithm, as + * described in: "Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and + * Automated Cartography", Martin A. Fischler and Robert C. Bolles, Comm. Of the ACM 24: 381–395, June 1981. + * A parallel variant is available, enable with setNumberOfThreads. Default is non-parallel. + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class RandomSampleConsensus : public SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SampleConsensus::max_iterations_; + using SampleConsensus::threshold_; + using SampleConsensus::iterations_; + using SampleConsensus::sac_model_; + using SampleConsensus::model_; + using SampleConsensus::model_coefficients_; + using SampleConsensus::inliers_; + using SampleConsensus::probability_; + using SampleConsensus::threads_; + + /** \brief RANSAC (RAndom SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + */ + RandomSampleConsensus (const SampleConsensusModelPtr &model) + : SampleConsensus (model) + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief RANSAC (RAndom SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + */ + RandomSampleConsensus (const SampleConsensusModelPtr &model, double threshold) + : SampleConsensus (model, threshold) + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/rmsac.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/rmsac.h new file mode 100755 index 0000000..5f81e7e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/rmsac.h @@ -0,0 +1,121 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator + * SAmple Consensus) algorithm, which basically adds a Td,d test (see \a RandomizedRandomSampleConsensus) to an MSAC + * estimator (see \a MEstimatorSampleConsensus). + * \note RMSAC is useful in situations where most of the data samples belong to the model, and a fast outlier rejection algorithm is needed. + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class RandomizedMEstimatorSampleConsensus : public SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SampleConsensus::max_iterations_; + using SampleConsensus::threshold_; + using SampleConsensus::iterations_; + using SampleConsensus::sac_model_; + using SampleConsensus::model_; + using SampleConsensus::model_coefficients_; + using SampleConsensus::inliers_; + using SampleConsensus::probability_; + + /** \brief RMSAC (Randomized M-estimator SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + */ + RandomizedMEstimatorSampleConsensus (const SampleConsensusModelPtr &model) + : SampleConsensus (model) + , fraction_nr_pretest_ (10.0) // Number of samples to try randomly in percents + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief RMSAC (Randomized M-estimator SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + */ + RandomizedMEstimatorSampleConsensus (const SampleConsensusModelPtr &model, double threshold) + : SampleConsensus (model, threshold) + , fraction_nr_pretest_ (10.0) // Number of samples to try randomly in percents + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + + /** \brief Set the percentage of points to pre-test. + * \param[in] nr_pretest percentage of points to pre-test + */ + inline void + setFractionNrPretest (double nr_pretest) { fraction_nr_pretest_ = nr_pretest; } + + /** \brief Get the percentage of points to pre-test. */ + inline double + getFractionNrPretest () const { return (fraction_nr_pretest_); } + + private: + /** \brief Number of samples to randomly pre-test, in percents. */ + double fraction_nr_pretest_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/rransac.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/rransac.h new file mode 100755 index 0000000..d2fadbc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/rransac.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RAndom SAmple + * Consensus), as described in "Randomized RANSAC with Td,d test", O. Chum and J. Matas, Proc. British Machine Vision + * Conf. (BMVC '02), vol. 2, BMVA, pp. 448-457, 2002. + * \note RRANSAC is useful in situations where most of the data samples belong to the model, and a fast outlier rejection algorithm is needed. + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class RandomizedRandomSampleConsensus : public SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SampleConsensus::max_iterations_; + using SampleConsensus::threshold_; + using SampleConsensus::iterations_; + using SampleConsensus::sac_model_; + using SampleConsensus::model_; + using SampleConsensus::model_coefficients_; + using SampleConsensus::inliers_; + using SampleConsensus::probability_; + + /** \brief RANSAC (Randomized RAndom SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + */ + RandomizedRandomSampleConsensus (const SampleConsensusModelPtr &model) + : SampleConsensus (model) + , fraction_nr_pretest_ (10.0) // Number of samples to try randomly in percents + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief RRANSAC (RAndom SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + */ + RandomizedRandomSampleConsensus (const SampleConsensusModelPtr &model, double threshold) + : SampleConsensus (model, threshold) + , fraction_nr_pretest_ (10.0) // Number of samples to try randomly in percents + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + + /** \brief Set the percentage of points to pre-test. + * \param[in] nr_pretest percentage of points to pre-test + */ + inline void + setFractionNrPretest (double nr_pretest) { fraction_nr_pretest_ = nr_pretest; } + + /** \brief Get the percentage of points to pre-test. */ + inline double + getFractionNrPretest () const { return (fraction_nr_pretest_); } + + private: + /** \brief Number of samples to randomly pre-test, in percents. */ + double fraction_nr_pretest_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac.h new file mode 100755 index 0000000..4489a88 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac.h @@ -0,0 +1,357 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace pcl +{ + /** \brief SampleConsensus represents the base class. All sample consensus methods must inherit from this class. + * \author Radu Bogdan Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensus + { + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + private: + /** \brief Constructor for base SAC. */ + SampleConsensus () {}; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + + /** \brief Constructor for base SAC. + * \param[in] model a Sample Consensus model + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensus (const SampleConsensusModelPtr &model, bool random = false) + : sac_model_ (model) + , probability_ (0.99) + , iterations_ (0) + , threshold_ (std::numeric_limits::max ()) + , max_iterations_ (1000) + , threads_ (-1) + , rng_ (new boost::uniform_01 (rng_alg_)) + { + // Create a random number generator object + if (random) + rng_->base ().seed (static_cast (std::time (nullptr))); + else + rng_->base ().seed (12345u); + }; + + /** \brief Constructor for base SAC. + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensus (const SampleConsensusModelPtr &model, + double threshold, + bool random = false) + : sac_model_ (model) + , probability_ (0.99) + , iterations_ (0) + , threshold_ (threshold) + , max_iterations_ (1000) + , threads_ (-1) + , rng_ (new boost::uniform_01 (rng_alg_)) + { + // Create a random number generator object + if (random) + rng_->base ().seed (static_cast (std::time (nullptr))); + else + rng_->base ().seed (12345u); + }; + + /** \brief Set the Sample Consensus model to use. + * \param[in] model a Sample Consensus model + */ + void + setSampleConsensusModel (const SampleConsensusModelPtr &model) + { + sac_model_ = model; + } + + /** \brief Get the Sample Consensus model used. */ + SampleConsensusModelPtr + getSampleConsensusModel () const + { + return (sac_model_); + } + + /** \brief Destructor for base SAC. */ + virtual ~SampleConsensus () {}; + + /** \brief Set the distance to model threshold. + * \param[in] threshold distance to model threshold + */ + inline void + setDistanceThreshold (double threshold) { threshold_ = threshold; } + + /** \brief Get the distance to model threshold, as set by the user. */ + inline double + getDistanceThreshold () const { return (threshold_); } + + /** \brief Set the maximum number of iterations. + * \param[in] max_iterations maximum number of iterations + */ + inline void + setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; } + + /** \brief Get the maximum number of iterations, as set by the user. */ + inline int + getMaxIterations () const { return (max_iterations_); } + + /** \brief Set the desired probability of choosing at least one sample free from outliers. + * \param[in] probability the desired probability of choosing at least one sample free from outliers + * \note internally, the probability is set to 99% (0.99) by default. + */ + inline void + setProbability (double probability) { probability_ = probability; } + + /** \brief Obtain the probability of choosing at least one sample free from outliers, as set by the user. */ + inline double + getProbability () const { return (probability_); } + + /** \brief Set the number of threads to use or turn off parallelization. + * \param[in] nr_threads the number of hardware threads to use (0 sets the value automatically, a negative number turns parallelization off) + * \note Not all SAC methods have a parallel implementation. Some will ignore this setting. + */ + inline void + setNumberOfThreads (const int nr_threads = -1) { threads_ = nr_threads; } + + /** \brief Get the number of threads, as set by the user. */ + inline int + getNumberOfThreads () const { return (threads_); } + + /** \brief Compute the actual model. Pure virtual. */ + virtual bool + computeModel (int debug_verbosity_level = 0) = 0; + + /** \brief Refine the model found. + * This loops over the model coefficients and optimizes them together + * with the set of inliers, until the change in the set of inliers is + * minimal. + * \param[in] sigma standard deviation multiplier for considering a sample as inlier (Mahalanobis distance) + * \param[in] max_iterations the maxim number of iterations to try to refine in case the inliers keep on changing + */ + virtual bool + refineModel (const double sigma = 3.0, const unsigned int max_iterations = 1000) + { + if (!sac_model_) + { + PCL_ERROR ("[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n"); + return (false); + } + + double inlier_distance_threshold_sqr = threshold_ * threshold_, + error_threshold = threshold_; + double sigma_sqr = sigma * sigma; + unsigned int refine_iterations = 0; + bool inlier_changed = false, oscillating = false; + std::vector new_inliers, prev_inliers = inliers_; + std::vector inliers_sizes; + Eigen::VectorXf new_model_coefficients = model_coefficients_; + do + { + // Optimize the model coefficients + sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients); + inliers_sizes.push_back (prev_inliers.size ()); + + // Select the new inliers based on the optimized coefficients and new threshold + sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers); + PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %lu/%lu, with an error threshold of %g.\n", prev_inliers.size (), new_inliers.size (), error_threshold); + + if (new_inliers.empty ()) + { + refine_iterations++; + if (refine_iterations >= max_iterations) + break; + continue; + //return (false); + } + + // Estimate the variance and the new threshold + double variance = sac_model_->computeVariance (); + error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance)); + + PCL_DEBUG ("[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations); + inlier_changed = false; + std::swap (prev_inliers, new_inliers); + // If the number of inliers changed, then we are still optimizing + if (new_inliers.size () != prev_inliers.size ()) + { + // Check if the number of inliers is oscillating in between two values + if (inliers_sizes.size () >= 4) + { + if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] && + inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4]) + { + oscillating = true; + break; + } + } + inlier_changed = true; + continue; + } + + // Check the values of the inlier set + for (std::size_t i = 0; i < prev_inliers.size (); ++i) + { + // If the value of the inliers changed, then we are still optimizing + if (prev_inliers[i] != new_inliers[i]) + { + inlier_changed = true; + break; + } + } + } + while (inlier_changed && ++refine_iterations < max_iterations); + + // If the new set of inliers is empty, we didn't do a good job refining + if (new_inliers.empty ()) + { + PCL_ERROR ("[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n"); + return (false); + } + + if (oscillating) + { + PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n"); + return (true); + } + + // If no inliers have been changed anymore, then the refinement was successful + if (!inlier_changed) + { + std::swap (inliers_, new_inliers); + model_coefficients_ = new_model_coefficients; + return (true); + } + return (false); + } + + /** \brief Get a set of randomly selected indices. + * \param[in] indices the input indices vector + * \param[in] nr_samples the desired number of point indices to randomly select + * \param[out] indices_subset the resultant output set of randomly selected indices + */ + inline void + getRandomSamples (const IndicesPtr &indices, + std::size_t nr_samples, + std::set &indices_subset) + { + indices_subset.clear (); + while (indices_subset.size () < nr_samples) + //indices_subset.insert ((*indices)[(int) (indices->size () * (rand () / (RAND_MAX + 1.0)))]); + indices_subset.insert ((*indices)[static_cast (static_cast(indices->size ()) * rnd ())]); + } + + /** \brief Return the best model found so far. + * \param[out] model the resultant model + */ + inline void + getModel (std::vector &model) const { model = model_; } + + /** \brief Return the best set of inliers found so far for this model. + * \param[out] inliers the resultant set of inliers + */ + inline void + getInliers (std::vector &inliers) const { inliers = inliers_; } + + /** \brief Return the model coefficients of the best model found so far. + * \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus + */ + inline void + getModelCoefficients (Eigen::VectorXf &model_coefficients) const { model_coefficients = model_coefficients_; } + + protected: + /** \brief The underlying data model used (i.e. what is it that we attempt to search for). */ + SampleConsensusModelPtr sac_model_; + + /** \brief The model found after the last computeModel () as point cloud indices. */ + std::vector model_; + + /** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */ + std::vector inliers_; + + /** \brief The coefficients of our model computed directly from the model found. */ + Eigen::VectorXf model_coefficients_; + + /** \brief Desired probability of choosing at least one sample free from outliers. */ + double probability_; + + /** \brief Total number of internal loop iterations that we've done so far. */ + int iterations_; + + /** \brief Distance to model threshold. */ + double threshold_; + + /** \brief Maximum number of iterations before giving up. */ + int max_iterations_; + + /** \brief The number of threads the scheduler should use, or a negative number if no parallelization is wanted. */ + int threads_; + + /** \brief Boost-based random number generator algorithm. */ + boost::mt19937 rng_alg_; + + /** \brief Boost-based random number generator distribution. */ + std::shared_ptr > rng_; + + /** \brief Boost-based random number generator. */ + inline double + rnd () + { + return ((*rng_) ()); + } + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model.h new file mode 100755 index 0000000..edd7523 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model.h @@ -0,0 +1,672 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace pcl +{ + template class ProgressiveSampleConsensus; + + /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit + * from this class. + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModel + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using PointCloudPtr = typename PointCloud::Ptr; + using SearchPtr = typename pcl::search::Search::Ptr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + protected: + /** \brief Empty constructor for base SampleConsensusModel. + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModel (bool random = false) + : input_ () + , radius_min_ (-std::numeric_limits::max ()) + , radius_max_ (std::numeric_limits::max ()) + , samples_radius_ (0.) + , samples_radius_search_ () + , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits::max ())) + { + // Create a random number generator object + if (random) + rng_alg_.seed (static_cast (std::time(nullptr))); + else + rng_alg_.seed (12345u); + + rng_gen_.reset (new boost::variate_generator > (rng_alg_, *rng_dist_)); + } + + public: + /** \brief Constructor for base SampleConsensusModel. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModel (const PointCloudConstPtr &cloud, bool random = false) + : input_ () + , radius_min_ (-std::numeric_limits::max ()) + , radius_max_ (std::numeric_limits::max ()) + , samples_radius_ (0.) + , samples_radius_search_ () + , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits::max ())) + { + if (random) + rng_alg_.seed (static_cast (std::time (nullptr))); + else + rng_alg_.seed (12345u); + + // Sets the input cloud and creates a vector of "fake" indices + setInputCloud (cloud); + + // Create a random number generator object + rng_gen_.reset (new boost::variate_generator > (rng_alg_, *rng_dist_)); + } + + /** \brief Constructor for base SampleConsensusModel. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModel (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : input_ (cloud) + , indices_ (new std::vector (indices)) + , radius_min_ (-std::numeric_limits::max ()) + , radius_max_ (std::numeric_limits::max ()) + , samples_radius_ (0.) + , samples_radius_search_ () + , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits::max ())) + { + if (random) + rng_alg_.seed (static_cast (std::time(nullptr))); + else + rng_alg_.seed (12345u); + + if (indices_->size () > input_->points.size ()) + { + PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n", indices_->size (), input_->points.size ()); + indices_->clear (); + } + shuffled_indices_ = *indices_; + + // Create a random number generator object + rng_gen_.reset (new boost::variate_generator > (rng_alg_, *rng_dist_)); + }; + + /** \brief Destructor for base SampleConsensusModel. */ + virtual ~SampleConsensusModel () {}; + + /** \brief Get a set of random data samples and return them as point + * indices. + * \param[out] iterations the internal number of iterations used by SAC methods + * \param[out] samples the resultant model samples + */ + virtual void + getSamples (int &iterations, std::vector &samples) + { + // We're assuming that indices_ have already been set in the constructor + if (indices_->size () < getSampleSize ()) + { + PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n", + samples.size (), indices_->size ()); + // one of these will make it stop :) + samples.clear (); + iterations = INT_MAX - 1; + return; + } + + // Get a second point which is different than the first + samples.resize (getSampleSize ()); + for (unsigned int iter = 0; iter < max_sample_checks_; ++iter) + { + // Choose the random indices + if (samples_radius_ < std::numeric_limits::epsilon ()) + SampleConsensusModel::drawIndexSample (samples); + else + SampleConsensusModel::drawIndexSampleRadius (samples); + + // If it's a good sample, stop here + if (isSampleGood (samples)) + { + PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ()); + return; + } + } + PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_); + samples.clear (); + } + + /** \brief Check whether the given index samples can form a valid model, + * compute the model coefficients from these samples and store them + * in model_coefficients. Pure virtual. + * Implementations of this function must be thread-safe. + * \param[in] samples the point indices found as possible good candidates + * for creating a valid model + * \param[out] model_coefficients the computed model coefficients + */ + virtual bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const = 0; + + /** \brief Recompute the model coefficients using the given inlier set + * and return them to the user. Pure virtual. + * + * @note: these are the coefficients of the model after refinement + * (e.g., after a least-squares optimization) + * + * \param[in] inliers the data inliers supporting the model + * \param[in] model_coefficients the initial guess for the model coefficients + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + virtual void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const = 0; + + /** \brief Compute all distances from the cloud data to a given model. Pure virtual. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + virtual void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const = 0; + + /** \brief Select all the points which respect the given model + * coefficients as inliers. Pure virtual. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from + * the outliers + * \param[out] inliers the resultant model inliers + */ + virtual void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) = 0; + + /** \brief Count all the points which respect the given model + * coefficients as inliers. Pure virtual. + * Implementations of this function must be thread-safe. + * \param[in] model_coefficients the coefficients of a model that we need to + * compute distances to + * \param[in] threshold a maximum admissible distance threshold for + * determining the inliers from the outliers + * \return the resultant number of inliers + */ + virtual std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const = 0; + + /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual. + * \param[in] inliers the data inliers that we want to project on the model + * \param[in] model_coefficients the coefficients of a model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true (default) if we want the \a + * projected_points cloud to be an exact copy of the input dataset minus + * the point projections on the plane model + */ + virtual void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const = 0; + + /** \brief Verify whether a subset of indices verifies a given set of + * model coefficients. Pure virtual. + * + * \param[in] indices the data indices that need to be tested against the model + * \param[in] model_coefficients the set of model coefficients + * \param[in] threshold a maximum admissible distance threshold for + * determining the inliers from the outliers + */ + virtual bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const = 0; + + /** \brief Provide a pointer to the input dataset + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + inline virtual void + setInputCloud (const PointCloudConstPtr &cloud) + { + input_ = cloud; + if (!indices_) + indices_.reset (new std::vector ()); + if (indices_->empty ()) + { + // Prepare a set of indices to be used (entire cloud) + indices_->resize (cloud->points.size ()); + for (std::size_t i = 0; i < cloud->points.size (); ++i) + (*indices_)[i] = static_cast (i); + } + shuffled_indices_ = *indices_; + } + + /** \brief Get a pointer to the input point cloud dataset. */ + inline PointCloudConstPtr + getInputCloud () const { return (input_); } + + /** \brief Provide a pointer to the vector of indices that represents the input data. + * \param[in] indices a pointer to the vector of indices that represents the input data. + */ + inline void + setIndices (const IndicesPtr &indices) + { + indices_ = indices; + shuffled_indices_ = *indices_; + } + + /** \brief Provide the vector of indices that represents the input data. + * \param[out] indices the vector of indices that represents the input data. + */ + inline void + setIndices (const std::vector &indices) + { + indices_.reset (new std::vector (indices)); + shuffled_indices_ = indices; + } + + /** \brief Get a pointer to the vector of indices used. */ + inline IndicesPtr + getIndices () const { return (indices_); } + + /** \brief Return a unique id for each type of model employed. */ + virtual SacModel + getModelType () const = 0; + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const + { + return (model_name_); + } + + /** \brief Return the size of a sample from which the model is computed. */ + inline unsigned int + getSampleSize () const + { + return sample_size_; + } + + /** \brief Return the number of coefficients in the model. */ + inline unsigned int + getModelSize () const + { + return model_size_; + } + + /** \brief Set the minimum and maximum allowable radius limits for the + * model (applicable to models that estimate a radius) + * \param[in] min_radius the minimum radius model + * \param[in] max_radius the maximum radius model + * \todo change this to set limits on the entire model + */ + inline void + setRadiusLimits (const double &min_radius, const double &max_radius) + { + radius_min_ = min_radius; + radius_max_ = max_radius; + } + + /** \brief Get the minimum and maximum allowable radius limits for the + * model as set by the user. + * + * \param[out] min_radius the resultant minimum radius model + * \param[out] max_radius the resultant maximum radius model + */ + inline void + getRadiusLimits (double &min_radius, double &max_radius) const + { + min_radius = radius_min_; + max_radius = radius_max_; + } + + /** \brief Set the maximum distance allowed when drawing random samples + * \param[in] radius the maximum distance (L2 norm) + * \param search + */ + inline void + setSamplesMaxDist (const double &radius, SearchPtr search) + { + samples_radius_ = radius; + samples_radius_search_ = search; + } + + /** \brief Get maximum distance allowed when drawing random samples + * + * \param[out] radius the maximum distance (L2 norm) + */ + inline void + getSamplesMaxDist (double &radius) const + { + radius = samples_radius_; + } + + friend class ProgressiveSampleConsensus; + + /** \brief Compute the variance of the errors to the model. + * \param[in] error_sqr_dists a vector holding the distances + */ + inline double + computeVariance (const std::vector &error_sqr_dists) const + { + std::vector dists (error_sqr_dists); + const std::size_t medIdx = dists.size () >> 1; + std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ()); + double median_error_sqr = dists[medIdx]; + return (2.1981 * median_error_sqr); + } + + /** \brief Compute the variance of the errors to the model from the internally + * estimated vector of distances. The model must be computed first (or at least + * selectWithinDistance must be called). + */ + inline double + computeVariance () const + { + if (error_sqr_dists_.empty ()) + { + PCL_ERROR ("[pcl::SampleConsensusModel::computeVariance] The variance of the Sample Consensus model distances cannot be estimated, as the model has not been computed yet. Please compute the model first or at least run selectWithinDistance before continuing. Returning NAN!\n"); + return (std::numeric_limits::quiet_NaN ()); + } + return (computeVariance (error_sqr_dists_)); + } + + protected: + + /** \brief Fills a sample array with random samples from the indices_ vector + * \param[out] sample the set of indices of target_ to analyze + */ + inline void + drawIndexSample (std::vector &sample) + { + std::size_t sample_size = sample.size (); + std::size_t index_size = shuffled_indices_.size (); + for (std::size_t i = 0; i < sample_size; ++i) + // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo + // elements, that does not matter (and nowadays, random number generators are good) + //std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]); + std::swap (shuffled_indices_[i], shuffled_indices_[i + (rnd () % (index_size - i))]); + std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ()); + } + + /** \brief Fills a sample array with one random sample from the indices_ vector + * and other random samples that are closer than samples_radius_ + * \param[out] sample the set of indices of target_ to analyze + */ + inline void + drawIndexSampleRadius (std::vector &sample) + { + std::size_t sample_size = sample.size (); + std::size_t index_size = shuffled_indices_.size (); + + std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]); + //const PointT& pt0 = (*input_)[shuffled_indices_[0]]; + + std::vector indices; + std::vector sqr_dists; + + // If indices have been set when the search object was constructed, + // radiusSearch() expects an index into the indices vector as its + // first parameter. This can't be determined efficiently, so we use + // the point instead of the index. + // Returned indices are converted automatically. + samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]), + samples_radius_, indices, sqr_dists ); + + if (indices.size () < sample_size - 1) + { + // radius search failed, make an invalid model + for(std::size_t i = 1; i < sample_size; ++i) + shuffled_indices_[i] = shuffled_indices_[0]; + } + else + { + for (std::size_t i = 0; i < sample_size-1; ++i) + std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]); + for (std::size_t i = 1; i < sample_size; ++i) + shuffled_indices_[i] = indices[i-1]; + } + + std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ()); + } + + /** \brief Check whether a model is valid given the user constraints. + * + * Default implementation verifies that the number of coefficients in the supplied model is as expected for this + * SAC model type. Specific SAC models should extend this function by checking the user constraints (if any). + * + * \param[in] model_coefficients the set of model coefficients + */ + virtual bool + isModelValid (const Eigen::VectorXf &model_coefficients) const + { + if (model_coefficients.size () != model_size_) + { + PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (%lu)!\n", getClassName ().c_str (), model_coefficients.size ()); + return (false); + } + return (true); + } + + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param[in] samples the resultant index samples + */ + virtual bool + isSampleGood (const std::vector &samples) const = 0; + + /** \brief The model name. */ + std::string model_name_; + + /** \brief A boost shared pointer to the point cloud data array. */ + PointCloudConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + IndicesPtr indices_; + + /** The maximum number of samples to try until we get a good one */ + static const unsigned int max_sample_checks_ = 1000; + + /** \brief The minimum and maximum radius limits for the model. + * Applicable to all models that estimate a radius. + */ + double radius_min_, radius_max_; + + /** \brief The maximum distance of subsequent samples from the first (radius search) */ + double samples_radius_; + + /** \brief The search object for picking subsequent samples using radius search */ + SearchPtr samples_radius_search_; + + /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */ + std::vector shuffled_indices_; + + /** \brief Boost-based random number generator algorithm. */ + boost::mt19937 rng_alg_; + + /** \brief Boost-based random number generator distribution. */ + std::shared_ptr > rng_dist_; + + /** \brief Boost-based random number generator. */ + std::shared_ptr > > rng_gen_; + + /** \brief A vector holding the distances to the computed model. Used internally. */ + std::vector error_sqr_dists_; + + /** \brief The size of a sample from which the model is computed. Every subclass should initialize this appropriately. */ + unsigned int sample_size_; + + /** \brief The number of coefficients in the model. Every subclass should initialize this appropriately. */ + unsigned int model_size_; + + /** \brief Boost-based random number generator. */ + inline int + rnd () + { + return ((*rng_gen_) ()); + } + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief @b SampleConsensusModelFromNormals represents the base model class + * for models that require the use of surface normals for estimation. + */ + template + class SampleConsensusModelFromNormals //: public SampleConsensusModel + { + public: + using PointCloudNConstPtr = typename pcl::PointCloud::ConstPtr; + using PointCloudNPtr = typename pcl::PointCloud::Ptr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor for base SampleConsensusModelFromNormals. */ + SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0), normals_ () {}; + + /** \brief Destructor. */ + virtual ~SampleConsensusModelFromNormals () {} + + /** \brief Set the normal angular distance weight. + * \param[in] w the relative weight (between 0 and 1) to give to the angular + * distance (0 to pi/2) between point normals and the plane normal. + * (The Euclidean distance will have weight 1-w.) + */ + inline void + setNormalDistanceWeight (const double w) + { + normal_distance_weight_ = w; + } + + /** \brief Get the normal angular distance weight. */ + inline double + getNormalDistanceWeight () const { return (normal_distance_weight_); } + + /** \brief Provide a pointer to the input dataset that contains the point + * normals of the XYZ dataset. + * + * \param[in] normals the const boost shared pointer to a PointCloud message + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) + { + normals_ = normals; + } + + /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */ + inline PointCloudNConstPtr + getInputNormals () const { return (normals_); } + + protected: + /** \brief The relative weight (between 0 and 1) to give to the angular + * distance (0 to pi/2) between point normals and the plane normal. + */ + double normal_distance_weight_; + + /** \brief A pointer to the input dataset that contains the point normals + * of the XYZ dataset. + */ + PointCloudNConstPtr normals_; + }; + + /** Base functor all the models that need non linear optimization must + * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec) + * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar + */ + template + struct Functor + { + using Scalar = _Scalar; + enum + { + InputsAtCompileTime = NX, + ValuesAtCompileTime = NY + }; + + using ValueType = Eigen::Matrix; + using InputType = Eigen::Matrix; + using JacobianType = Eigen::Matrix; + + /** \brief Empty Constructor. */ + Functor () : m_data_points_ (ValuesAtCompileTime) {} + + /** \brief Constructor + * \param[in] m_data_points number of data points to evaluate. + */ + Functor (int m_data_points) : m_data_points_ (m_data_points) {} + + virtual ~Functor () {} + + /** \brief Get the number of values. */ + int + values () const { return (m_data_points_); } + + private: + const int m_data_points_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_circle.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_circle.h new file mode 100755 index 0000000..b1ef12b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_circle.h @@ -0,0 +1,255 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane. + * + * The model coefficients are defined as: + * - \b center.x : the X coordinate of the circle's center + * - \b center.y : the Y coordinate of the circle's center + * - \b radius : the circle's radius + * + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelCircle2D : public SampleConsensusModel + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelCircle2D. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelCircle2D"; + sample_size_ = 3; + model_size_ = 3; + } + + /** \brief Constructor for base SampleConsensusModelCircle2D. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelCircle2D"; + sample_size_ = 3; + model_size_ = 3; + } + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + SampleConsensusModelCircle2D (const SampleConsensusModelCircle2D &source) : + SampleConsensusModel () + { + *this = source; + model_name_ = "SampleConsensusModelCircle2D"; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelCircle2D () {} + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + inline SampleConsensusModelCircle2D& + operator = (const SampleConsensusModelCircle2D &source) + { + SampleConsensusModel::operator=(source); + return (*this); + } + + /** \brief Check whether the given index samples can form a valid 2D circle model, compute the model coefficients + * from these samples and store them in model_coefficients. The circle coefficients are: x, y, R. + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given 2D circle model. + * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Compute all distances from the cloud data to a given 2D circle model. + * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the 2d circle coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the 2d circle model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + /** \brief Create a new point cloud with inliers projected onto the 2d circle model. + * \param[in] inliers the data inliers that we want to project on the 2d circle model + * \param[in] model_coefficients the coefficients of a 2d circle model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given 2d circle model coefficients. + * \param[in] indices the data indices that need to be tested against the 2d circle model + * \param[in] model_coefficients the 2d circle model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_CIRCLE2D). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_CIRCLE2D); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief Check if a sample of indices results in a good sample of points indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const override; + + private: + /** \brief Functor for the optimization function */ + struct OptimizationFunctor : pcl::Functor + { + /** \brief Functor constructor + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelCircle2D *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} + + /** Cost function to be minimized + * \param[in] x the variables array + * \param[out] fvec the resultant functions evaluations + * \return 0 + */ + int + operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const + { + for (int i = 0; i < values (); ++i) + { + // Compute the difference between the center of the circle and the datapoint X_i + float xt = model_->input_->points[indices_[i]].x - x[0]; + float yt = model_->input_->points[indices_[i]].y - x[1]; + + // g = sqrt ((x-a)^2 + (y-b)^2) - R + fvec[i] = std::sqrt (xt * xt + yt * yt) - x[2]; + } + return (0); + } + + const pcl::SampleConsensusModelCircle2D *model_; + const std::vector &indices_; + }; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_circle3d.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_circle3d.h new file mode 100755 index 0000000..28d8a33 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_circle3d.h @@ -0,0 +1,272 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelCircle3D defines a model for 3D circle segmentation. + * + * The model coefficients are defined as: + * - \b center.x : the X coordinate of the circle's center + * - \b center.y : the Y coordinate of the circle's center + * - \b center.z : the Z coordinate of the circle's center + * - \b radius : the circle's radius + * - \b normal.x : the X coordinate of the normal's direction + * - \b normal.y : the Y coordinate of the normal's direction + * - \b normal.z : the Z coordinate of the normal's direction + * + * \author Raoul Hoffmann, Karol Hausman, Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelCircle3D : public SampleConsensusModel + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor for base SampleConsensusModelCircle3D. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelCircle3D"; + sample_size_ = 3; + model_size_ = 7; + } + + /** \brief Constructor for base SampleConsensusModelCircle3D. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelCircle3D"; + sample_size_ = 3; + model_size_ = 7; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelCircle3D () {} + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + SampleConsensusModelCircle3D (const SampleConsensusModelCircle3D &source) : + SampleConsensusModel () + { + *this = source; + model_name_ = "SampleConsensusModelCircle3D"; + } + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + inline SampleConsensusModelCircle3D& + operator = (const SampleConsensusModelCircle3D &source) + { + SampleConsensusModel::operator=(source); + return (*this); + } + + /** \brief Check whether the given index samples can form a valid 2D circle model, compute the model coefficients + * from these samples and store them in model_coefficients. The circle coefficients are: x, y, R. + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given 3D circle model. + * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Compute all distances from the cloud data to a given 3D circle model. + * \param[in] model_coefficients the coefficients of a 3D circle model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the 3d circle coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the 3d circle model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + /** \brief Create a new point cloud with inliers projected onto the 3d circle model. + * \param[in] inliers the data inliers that we want to project on the 3d circle model + * \param[in] model_coefficients the coefficients of a 3d circle model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given 3d circle model coefficients. + * \param[in] indices the data indices that need to be tested against the 3d circle model + * \param[in] model_coefficients the 3d circle model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_CIRCLE3D). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_CIRCLE3D); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief Check if a sample of indices results in a good sample of points indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const override; + + private: + /** \brief Functor for the optimization function */ + struct OptimizationFunctor : pcl::Functor + { + /** Functor constructor + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelCircle3D *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} + + /** Cost function to be minimized + * \param[in] x the variables array + * \param[out] fvec the resultant functions evaluations + * \return 0 + */ + int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const + { + for (int i = 0; i < values (); ++i) + { + // what i have: + // P : Sample Point + Eigen::Vector3d P (model_->input_->points[indices_[i]].x, model_->input_->points[indices_[i]].y, model_->input_->points[indices_[i]].z); + // C : Circle Center + Eigen::Vector3d C (x[0], x[1], x[2]); + // N : Circle (Plane) Normal + Eigen::Vector3d N (x[4], x[5], x[6]); + // r : Radius + double r = x[3]; + + Eigen::Vector3d helperVectorPC = P - C; + // 1.1. get line parameter + //float lambda = (helperVectorPC.dot(N)) / N.squaredNorm() ; + double lambda = (-(helperVectorPC.dot (N))) / N.dot (N); + // Projected Point on plane + Eigen::Vector3d P_proj = P + lambda * N; + Eigen::Vector3d helperVectorP_projC = P_proj - C; + + // K : Point on Circle + Eigen::Vector3d K = C + r * helperVectorP_projC.normalized (); + Eigen::Vector3d distanceVector = P - K; + + fvec[i] = distanceVector.norm (); + } + return (0); + } + + const pcl::SampleConsensusModelCircle3D *model_; + const std::vector &indices_; + }; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_cone.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_cone.h new file mode 100755 index 0000000..a607e0d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_cone.h @@ -0,0 +1,358 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation. + * The model coefficients are defined as: + *
    + *
  • apex.x : the X coordinate of cone's apex + *
  • apex.y : the Y coordinate of cone's apex + *
  • apex.z : the Z coordinate of cone's apex + *
  • axis_direction.x : the X coordinate of the cone's axis direction + *
  • axis_direction.y : the Y coordinate of the cone's axis direction + *
  • axis_direction.z : the Z coordinate of the cone's axis direction + *
  • opening_angle : the cone's opening angle + *
+ * \author Stefan Schrandt + * \ingroup sample_consensus + */ + template + class SampleConsensusModelCone : public SampleConsensusModel, public SampleConsensusModelFromNormals + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModelFromNormals::normals_; + using SampleConsensusModelFromNormals::normal_distance_weight_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelCone. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false) + : SampleConsensusModel (cloud, random) + , SampleConsensusModelFromNormals () + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0) + , min_angle_ (-std::numeric_limits::max ()) + , max_angle_ (std::numeric_limits::max ()) + { + model_name_ = "SampleConsensusModelCone"; + sample_size_ = 3; + model_size_ = 7; + } + + /** \brief Constructor for base SampleConsensusModelCone. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCone (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + , SampleConsensusModelFromNormals () + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0) + , min_angle_ (-std::numeric_limits::max ()) + , max_angle_ (std::numeric_limits::max ()) + { + model_name_ = "SampleConsensusModelCone"; + sample_size_ = 3; + model_size_ = 7; + } + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + SampleConsensusModelCone (const SampleConsensusModelCone &source) : + SampleConsensusModel (), + SampleConsensusModelFromNormals (), + eps_angle_ (), min_angle_ (), max_angle_ () + { + *this = source; + model_name_ = "SampleConsensusModelCone"; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelCone () {} + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + inline SampleConsensusModelCone& + operator = (const SampleConsensusModelCone &source) + { + SampleConsensusModel::operator=(source); + SampleConsensusModelFromNormals::operator=(source); + axis_ = source.axis_; + eps_angle_ = source.eps_angle_; + min_angle_ = source.min_angle_; + max_angle_ = source.max_angle_; + return (*this); + } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed difference between the cone's axis and the given axis. + */ + inline void + setEpsAngle (double ea) { eps_angle_ = ea; } + + /** \brief Get the angle epsilon (delta) threshold. */ + inline double + getEpsAngle () const { return (eps_angle_); } + + /** \brief Set the axis along which we need to search for a cone direction. + * \param[in] ax the axis along which we need to search for a cone direction + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } + + /** \brief Get the axis along which we need to search for a cone direction. */ + inline Eigen::Vector3f + getAxis () const { return (axis_); } + + /** \brief Set the minimum and maximum allowable opening angle for a cone model + * given from a user. + * \param[in] min_angle the minimum allowable opening angle of a cone model + * \param[in] max_angle the maximum allowable opening angle of a cone model + */ + inline void + setMinMaxOpeningAngle (const double &min_angle, const double &max_angle) + { + min_angle_ = min_angle; + max_angle_ = max_angle; + } + + /** \brief Get the opening angle which we need minimum to validate a cone model. + * \param[out] min_angle the minimum allowable opening angle of a cone model + * \param[out] max_angle the maximum allowable opening angle of a cone model + */ + inline void + getMinMaxOpeningAngle (double &min_angle, double &max_angle) const + { + min_angle = min_angle_; + max_angle = max_angle_; + } + + /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients + * from these samples and store them in model_coefficients. The cone coefficients are: apex, + * axis_direction, opening_angle. + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given cone model. + * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + + /** \brief Recompute the cone coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the cone model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + + /** \brief Create a new point cloud with inliers projected onto the cone model. + * \param[in] inliers the data inliers that we want to project on the cone model + * \param[in] model_coefficients the coefficients of a cone model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given cone model coefficients. + * \param[in] indices the data indices that need to be tested against the cone model + * \param[in] model_coefficients the cone model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_CONE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_CONE); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Get the distance from a point to a line (represented by a point and a direction) + * \param[in] pt a point + * \param[in] model_coefficients the line coefficients (a point on the line, line direction) + */ + double + pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const std::vector &samples) const override; + + private: + /** \brief The axis along which we need to search for a cone direction. */ + Eigen::Vector3f axis_; + + /** \brief The maximum allowed difference between the cone direction and the given axis. */ + double eps_angle_; + + /** \brief The minimum and maximum allowed opening angles of valid cone model. */ + double min_angle_; + double max_angle_; + + /** \brief Functor for the optimization function */ + struct OptimizationFunctor : pcl::Functor + { + /** Functor constructor + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelCone *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} + + /** Cost function to be minimized + * \param[in] x variables array + * \param[out] fvec resultant functions evaluations + * \return 0 + */ + int + operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const + { + Eigen::Vector4f apex (x[0], x[1], x[2], 0); + Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0); + float opening_angle = x[6]; + + float apexdotdir = apex.dot (axis_dir); + float dirdotdir = 1.0f / axis_dir.dot (axis_dir); + + for (int i = 0; i < values (); ++i) + { + // dist = f - r + Eigen::Vector4f pt (model_->input_->points[indices_[i]].x, + model_->input_->points[indices_[i]].y, + model_->input_->points[indices_[i]].z, 0); + + // Calculate the point's projection on the cone axis + float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; + Eigen::Vector4f pt_proj = apex + k * axis_dir; + + // Calculate the actual radius of the cone at the level of the projected point + Eigen::Vector4f height = apex-pt_proj; + float actual_cone_radius = tanf (opening_angle) * height.norm (); + + fvec[i] = static_cast (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius); + } + return (0); + } + + const pcl::SampleConsensusModelCone *model_; + const std::vector &indices_; + }; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_cylinder.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_cylinder.h new file mode 100755 index 0000000..eda0f89 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_cylinder.h @@ -0,0 +1,341 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief @b SampleConsensusModelCylinder defines a model for 3D cylinder segmentation. + * The model coefficients are defined as: + * - \b point_on_axis.x : the X coordinate of a point located on the cylinder axis + * - \b point_on_axis.y : the Y coordinate of a point located on the cylinder axis + * - \b point_on_axis.z : the Z coordinate of a point located on the cylinder axis + * - \b axis_direction.x : the X coordinate of the cylinder's axis direction + * - \b axis_direction.y : the Y coordinate of the cylinder's axis direction + * - \b axis_direction.z : the Z coordinate of the cylinder's axis direction + * - \b radius : the cylinder's radius + * + * \author Radu Bogdan Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelCylinder : public SampleConsensusModel, public SampleConsensusModelFromNormals + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModelFromNormals::normals_; + using SampleConsensusModelFromNormals::normal_distance_weight_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelCylinder. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCylinder (const PointCloudConstPtr &cloud, bool random = false) + : SampleConsensusModel (cloud, random) + , SampleConsensusModelFromNormals () + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0) + { + model_name_ = "SampleConsensusModelCylinder"; + sample_size_ = 2; + model_size_ = 7; + } + + /** \brief Constructor for base SampleConsensusModelCylinder. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelCylinder (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + , SampleConsensusModelFromNormals () + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0) + { + model_name_ = "SampleConsensusModelCylinder"; + sample_size_ = 2; + model_size_ = 7; + } + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + SampleConsensusModelCylinder (const SampleConsensusModelCylinder &source) : + SampleConsensusModel (), + SampleConsensusModelFromNormals (), + axis_ (Eigen::Vector3f::Zero ()), + eps_angle_ (0) + { + *this = source; + model_name_ = "SampleConsensusModelCylinder"; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelCylinder () {} + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + inline SampleConsensusModelCylinder& + operator = (const SampleConsensusModelCylinder &source) + { + SampleConsensusModel::operator=(source); + SampleConsensusModelFromNormals::operator=(source); + axis_ = source.axis_; + eps_angle_ = source.eps_angle_; + return (*this); + } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed difference between the cylinder axis and the given axis. + */ + inline void + setEpsAngle (const double ea) { eps_angle_ = ea; } + + /** \brief Get the angle epsilon (delta) threshold. */ + inline double + getEpsAngle () const { return (eps_angle_); } + + /** \brief Set the axis along which we need to search for a cylinder direction. + * \param[in] ax the axis along which we need to search for a cylinder direction + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } + + /** \brief Get the axis along which we need to search for a cylinder direction. */ + inline Eigen::Vector3f + getAxis () const { return (axis_); } + + /** \brief Check whether the given index samples can form a valid cylinder model, compute the model coefficients + * from these samples and store them in model_coefficients. The cylinder coefficients are: point_on_axis, + * axis_direction, cylinder_radius_R + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given cylinder model. + * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the cylinder model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + + /** \brief Create a new point cloud with inliers projected onto the cylinder model. + * \param[in] inliers the data inliers that we want to project on the cylinder model + * \param[in] model_coefficients the coefficients of a cylinder model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients. + * \param[in] indices the data indices that need to be tested against the cylinder model + * \param[in] model_coefficients the cylinder model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_CYLINDER). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_CYLINDER); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Get the distance from a point to a line (represented by a point and a direction) + * \param[in] pt a point + * \param[in] model_coefficients the line coefficients (a point on the line, line direction) + */ + double + pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const; + + /** \brief Project a point onto a line given by a point and a direction vector + * \param[in] pt the input point to project + * \param[in] line_pt the point on the line (make sure that line_pt[3] = 0 as there are no internal checks!) + * \param[in] line_dir the direction of the line (make sure that line_dir[3] = 0 as there are no internal checks!) + * \param[out] pt_proj the resultant projected point + */ + inline void + projectPointToLine (const Eigen::Vector4f &pt, + const Eigen::Vector4f &line_pt, + const Eigen::Vector4f &line_dir, + Eigen::Vector4f &pt_proj) const + { + float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); + // Calculate the projection of the point on the line + pt_proj = line_pt + k * line_dir; + } + + /** \brief Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction, + * cylinder_radius_R) + * \param[in] pt the input point to project + * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R) + * \param[out] pt_proj the resultant projected point + */ + void + projectPointToCylinder (const Eigen::Vector4f &pt, + const Eigen::VectorXf &model_coefficients, + Eigen::Vector4f &pt_proj) const; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const std::vector &samples) const override; + + private: + /** \brief The axis along which we need to search for a cylinder direction. */ + Eigen::Vector3f axis_; + + /** \brief The maximum allowed difference between the cylinder direction and the given axis. */ + double eps_angle_; + + /** \brief Functor for the optimization function */ + struct OptimizationFunctor : pcl::Functor + { + /** Functor constructor + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelCylinder *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} + + /** Cost function to be minimized + * \param[in] x variables array + * \param[out] fvec resultant functions evaluations + * \return 0 + */ + int + operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const + { + Eigen::Vector4f line_pt (x[0], x[1], x[2], 0); + Eigen::Vector4f line_dir (x[3], x[4], x[5], 0); + + for (int i = 0; i < values (); ++i) + { + // dist = f - r + Eigen::Vector4f pt (model_->input_->points[indices_[i]].x, + model_->input_->points[indices_[i]].y, + model_->input_->points[indices_[i]].z, 0); + + fvec[i] = static_cast (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]); + } + return (0); + } + + const pcl::SampleConsensusModelCylinder *model_; + const std::vector &indices_; + }; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_line.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_line.h new file mode 100755 index 0000000..db66a67 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_line.h @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelLine defines a model for 3D line segmentation. + * The model coefficients are defined as: + * - \b point_on_line.x : the X coordinate of a point on the line + * - \b point_on_line.y : the Y coordinate of a point on the line + * - \b point_on_line.z : the Z coordinate of a point on the line + * - \b line_direction.x : the X coordinate of a line's direction + * - \b line_direction.y : the Y coordinate of a line's direction + * - \b line_direction.z : the Z coordinate of a line's direction + * + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelLine : public SampleConsensusModel + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::error_sqr_dists_; + using SampleConsensusModel::isModelValid; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelLine. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelLine (const PointCloudConstPtr &cloud, bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelLine"; + sample_size_ = 2; + model_size_ = 6; + } + + /** \brief Constructor for base SampleConsensusModelLine. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelLine (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelLine"; + sample_size_ = 2; + model_size_ = 6; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelLine () {} + + /** \brief Check whether the given index samples can form a valid line model, compute the model coefficients from + * these samples and store them internally in model_coefficients_. The line coefficients are represented by a + * point and a line direction + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all squared distances from the cloud data to a given line model. + * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to + * \param[out] distances the resultant estimated squared distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the line coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the line model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the model coefficients + * \param[out] optimized_coefficients the resultant recomputed coefficients after optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + /** \brief Create a new point cloud with inliers projected onto the line model. + * \param[in] inliers the data inliers that we want to project on the line model + * \param[in] model_coefficients the *normalized* coefficients of a line model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given line model coefficients. + * \param[in] indices the data indices that need to be tested against the line model + * \param[in] model_coefficients the line model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_LINE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_LINE); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const std::vector &samples) const override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_parallel_plane.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_parallel_plane.h new file mode 100755 index 0000000..e6944d9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_parallel_plane.h @@ -0,0 +1,222 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelNormalParallelPlane defines a model for 3D + * plane segmentation using additional surface normal constraints. Basically + * this means that checking for inliers will not only involve a "distance to + * model" criterion, but also an additional "maximum angular deviation" + * between the plane's normal and the inlier points normals. In addition, + * the plane normal must lie parallel to a user-specified axis. + * This means that the plane itself will lie perpendicular to that axis, similar to \link pcl::SampleConsensusModelPerpendicularPlane SACMODEL_PERPENDICULAR_PLANE \endlink. + * + * The model coefficients are defined as: + * - \b a : the X coordinate of the plane's normal (normalized) + * - \b b : the Y coordinate of the plane's normal (normalized) + * - \b c : the Z coordinate of the plane's normal (normalized) + * - \b d : the fourth Hessian component of the plane's equation + * + * To set the influence of the surface normals in the inlier estimation + * process, set the normal weight (0.0-1.0), e.g.: + * \code + * SampleConsensusModelNormalPlane sac_model; + * ... + * sac_model.setNormalDistanceWeight (0.1); + * ... + * \endcode + * + * In addition, the user can specify more constraints, such as: + * + * - an axis along which we need to search for a plane perpendicular to (\ref setAxis); + * - an angle \a tolerance threshold between the plane's normal and the above given axis (\ref setEpsAngle); + * - a distance we expect the plane to be from the origin (\ref setDistanceFromOrigin); + * - a distance \a tolerance as the maximum allowed deviation from the above given distance from the origin (\ref setEpsDist). + * + * \note Please remember that you need to specify an angle > 0 in order to activate the axis-angle constraint! + * + * \author Radu B. Rusu and Jared Glover and Nico Blodow + * \ingroup sample_consensus + */ + template + class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelNormalPlane + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModelFromNormals::normals_; + using SampleConsensusModelFromNormals::normal_distance_weight_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using PointCloudNPtr = typename SampleConsensusModelFromNormals::PointCloudNPtr; + using PointCloudNConstPtr = typename SampleConsensusModelFromNormals::PointCloudNConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelNormalParallelPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModelNormalPlane (cloud, random) + , axis_ (Eigen::Vector4f::Zero ()) + , distance_from_origin_ (0) + , eps_angle_ (-1.0) + , cos_angle_ (-1.0) + , eps_dist_ (0.0) + { + model_name_ = "SampleConsensusModelNormalParallelPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelNormalParallelPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModelNormalPlane (cloud, indices, random) + , axis_ (Eigen::Vector4f::Zero ()) + , distance_from_origin_ (0) + , eps_angle_ (-1.0) + , cos_angle_ (-1.0) + , eps_dist_ (0.0) + { + model_name_ = "SampleConsensusModelNormalParallelPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelNormalParallelPlane () {} + + /** \brief Set the axis along which we need to search for a plane perpendicular to. + * \param[in] ax the axis along which we need to search for a plane perpendicular to + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_.head<3> () = ax; axis_.normalize ();} + + /** \brief Get the axis along which we need to search for a plane perpendicular to. */ + inline Eigen::Vector3f + getAxis () const { return (axis_.head<3> ()); } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed deviation from 90 degrees between the plane normal and the given axis. + * \note You need to specify an angle > 0 in order to activate the axis-angle constraint! + */ + inline void + setEpsAngle (const double ea) { eps_angle_ = ea; cos_angle_ = std::abs (std::cos (ea));} + + /** \brief Get the angle epsilon (delta) threshold. */ + inline double + getEpsAngle () const { return (eps_angle_); } + + /** \brief Set the distance we expect the plane to be from the origin + * \param[in] d distance from the template plane to the origin + */ + inline void + setDistanceFromOrigin (const double d) { distance_from_origin_ = d; } + + /** \brief Get the distance of the plane from the origin. */ + inline double + getDistanceFromOrigin () const { return (distance_from_origin_); } + + /** \brief Set the distance epsilon (delta) threshold. + * \param[in] delta the maximum allowed deviation from the template distance from the origin + */ + inline void + setEpsDist (const double delta) { eps_dist_ = delta; } + + /** \brief Get the distance epsilon (delta) threshold. */ + inline double + getEpsDist () const { return (eps_dist_); } + + /** \brief Return a unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_NORMAL_PARALLEL_PLANE); } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + private: + /** \brief The axis along which we need to search for a plane perpendicular to. */ + Eigen::Vector4f axis_; + + /** \brief The distance from the template plane to the origin. */ + double distance_from_origin_; + + /** \brief The maximum allowed difference between the plane normal and the given axis. */ + double eps_angle_; + + /** \brief The cosine of the angle*/ + double cos_angle_; + /** \brief The maximum allowed deviation from the template distance from the origin. */ + double eps_dist_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_plane.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_plane.h new file mode 100755 index 0000000..6ae57f9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_plane.h @@ -0,0 +1,172 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelNormalPlane defines a model for 3D plane + * segmentation using additional surface normal constraints. Basically this + * means that checking for inliers will not only involve a "distance to + * model" criterion, but also an additional "maximum angular deviation" + * between the plane's normal and the inlier points normals. + * + * The model coefficients are defined as: + * - \b a : the X coordinate of the plane's normal (normalized) + * - \b b : the Y coordinate of the plane's normal (normalized) + * - \b c : the Z coordinate of the plane's normal (normalized) + * - \b d : the fourth Hessian component of the plane's equation + * + * To set the influence of the surface normals in the inlier estimation + * process, set the normal weight (0.0-1.0), e.g.: + * \code + * SampleConsensusModelNormalPlane sac_model; + * ... + * sac_model.setNormalDistanceWeight (0.1); + * ... + * \endcode + * + * \author Radu B. Rusu and Jared Glover + * \ingroup sample_consensus + */ + template + class SampleConsensusModelNormalPlane : public SampleConsensusModelPlane, public SampleConsensusModelFromNormals + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModelFromNormals::normals_; + using SampleConsensusModelFromNormals::normal_distance_weight_; + using SampleConsensusModel::error_sqr_dists_; + using SampleConsensusModel::isModelValid; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using PointCloudNPtr = typename SampleConsensusModelFromNormals::PointCloudNPtr; + using PointCloudNConstPtr = typename SampleConsensusModelFromNormals::PointCloudNConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelNormalPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelNormalPlane (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModelPlane (cloud, random) + , SampleConsensusModelFromNormals () + { + model_name_ = "SampleConsensusModelNormalPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelNormalPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelNormalPlane (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModelPlane (cloud, indices, random) + , SampleConsensusModelFromNormals () + { + model_name_ = "SampleConsensusModelNormalPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelNormalPlane () {} + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Compute all distances from the cloud data to a given plane model. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Return a unique id for this model (SACMODEL_NORMAL_PLANE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_NORMAL_PLANE); } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_sphere.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_sphere.h new file mode 100755 index 0000000..91bb0a1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_normal_sphere.h @@ -0,0 +1,172 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: sac_model_normal_sphere.h schrandt $ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief @b SampleConsensusModelNormalSphere defines a model for 3D sphere + * segmentation using additional surface normal constraints. Basically this + * means that checking for inliers will not only involve a "distance to + * model" criterion, but also an additional "maximum angular deviation" + * between the sphere's normal and the inlier points normals. + * + * The model coefficients are defined as: + *
    + *
  • center.x : the X coordinate of the sphere's center + *
  • center.y : the Y coordinate of the sphere's center + *
  • center.z : the Z coordinate of the sphere's center + *
  • radius : radius of the sphere + *
+ * + * \author Stefan Schrandt + * \ingroup sample_consensus + */ + template + class SampleConsensusModelNormalSphere : public SampleConsensusModelSphere, public SampleConsensusModelFromNormals + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModelFromNormals::normals_; + using SampleConsensusModelFromNormals::normal_distance_weight_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using PointCloudNPtr = typename SampleConsensusModelFromNormals::PointCloudNPtr; + using PointCloudNConstPtr = typename SampleConsensusModelFromNormals::PointCloudNConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelNormalSphere. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelNormalSphere (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModelSphere (cloud, random) + , SampleConsensusModelFromNormals () + { + model_name_ = "SampleConsensusModelNormalSphere"; + sample_size_ = 4; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelNormalSphere. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelNormalSphere (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModelSphere (cloud, indices, random) + , SampleConsensusModelFromNormals () + { + model_name_ = "SampleConsensusModelNormalSphere"; + sample_size_ = 4; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelNormalSphere () {} + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Compute all distances from the cloud data to a given sphere model. + * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Return a unique id for this model (SACMODEL_NORMAL_SPHERE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_NORMAL_SPHERE); } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_parallel_line.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_parallel_line.h new file mode 100755 index 0000000..6e0d18c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_parallel_line.h @@ -0,0 +1,183 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +namespace pcl +{ + /** \brief SampleConsensusModelParallelLine defines a model for 3D line segmentation using additional angular + * constraints. + * + * Checking for inliers will not only involve a "distance to model" criterion, but also an additional "maximum + * angular deviation" between the line's direction and a user-specified axis. + * + * The model coefficients are defined as: + * - \b point_on_line.x : the X coordinate of a point on the line + * - \b point_on_line.y : the Y coordinate of a point on the line + * - \b point_on_line.z : the Z coordinate of a point on the line + * - \b line_direction.x : the X coordinate of a line's direction + * - \b line_direction.y : the Y coordinate of a line's direction + * - \b line_direction.z : the Z coordinate of a line's direction + * + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelParallelLine : public SampleConsensusModelLine + { + public: + using SampleConsensusModel::model_name_; + + using PointCloud = typename SampleConsensusModelLine::PointCloud; + using PointCloudPtr = typename SampleConsensusModelLine::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModelLine::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelParallelLine. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModelLine (cloud, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + { + model_name_ = "SampleConsensusModelParallelLine"; + sample_size_ = 2; + model_size_ = 6; + } + + /** \brief Constructor for base SampleConsensusModelParallelLine. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModelLine (cloud, indices, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + { + model_name_ = "SampleConsensusModelParallelLine"; + sample_size_ = 2; + model_size_ = 6; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelParallelLine () {} + + /** \brief Set the axis along which we need to search for a line. + * \param[in] ax the axis along which we need to search for a line + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_ = ax; axis_.normalize (); } + + /** \brief Get the axis along which we need to search for a line. */ + inline Eigen::Vector3f + getAxis () const { return (axis_); } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed difference between the line direction and the given axis (in radians). + */ + inline void + setEpsAngle (const double ea) { eps_angle_ = ea; } + + /** \brief Get the angle epsilon (delta) threshold (in radians). */ + inline double getEpsAngle () const { return (eps_angle_); } + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Compute all squared distances from the cloud data to a given line model. + * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to + * \param[out] distances the resultant estimated squared distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Return a unique id for this model (SACMODEL_PARALLEL_LINE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_PARALLEL_LINE); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief The axis along which we need to search for a line. */ + Eigen::Vector3f axis_; + + /** \brief The maximum allowed difference between the line direction and the given axis. */ + double eps_angle_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_parallel_plane.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_parallel_plane.h new file mode 100755 index 0000000..3697adf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_parallel_plane.h @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief @b SampleConsensusModelParallelPlane defines a model for 3D plane segmentation using additional + * angular constraints. The plane must be parallel to a user-specified axis + * (\ref setAxis) within a user-specified angle threshold (\ref setEpsAngle). + * In other words, the plane normal must be (nearly) perpendicular to the specified axis. + * + * Code example for a plane model, parallel (within a 15 degrees tolerance) with the Z axis: + * \code + * SampleConsensusModelParallelPlane model (cloud); + * model.setAxis (Eigen::Vector3f (0.0, 0.0, 1.0)); + * model.setEpsAngle (pcl::deg2rad (15)); + * \endcode + * + * \note Please remember that you need to specify an angle > 0 in order to activate the axis-angle constraint! + * + * \author Radu B. Rusu, Nico Blodow + * \ingroup sample_consensus + */ + template + class SampleConsensusModelParallelPlane : public SampleConsensusModelPlane + { + public: + using SampleConsensusModel::model_name_; + + using PointCloud = typename SampleConsensusModelPlane::PointCloud; + using PointCloudPtr = typename SampleConsensusModelPlane::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModelPlane::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelParallelPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelParallelPlane (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModelPlane (cloud, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + , sin_angle_ (-1.0) + { + model_name_ = "SampleConsensusModelParallelPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelParallelPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelParallelPlane (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModelPlane (cloud, indices, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + , sin_angle_ (-1.0) + { + model_name_ = "SampleConsensusModelParallelPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelParallelPlane () {} + + /** \brief Set the axis along which we need to search for a plane perpendicular to. + * \param[in] ax the axis along which we need to search for a plane perpendicular to + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } + + /** \brief Get the axis along which we need to search for a plane perpendicular to. */ + inline Eigen::Vector3f + getAxis () const { return (axis_); } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed difference between the plane normal and the given axis. + * \note You need to specify an angle > 0 in order to activate the axis-angle constraint! + */ + inline void + setEpsAngle (const double ea) { eps_angle_ = ea; sin_angle_ = std::abs (sin (ea));} + + /** \brief Get the angle epsilon (delta) threshold. */ + inline double + getEpsAngle () const { return (eps_angle_); } + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Compute all distances from the cloud data to a given plane model. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Return a unique id for this model (SACMODEL_PARALLEL_PLANE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_PARALLEL_PLANE); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief The axis along which we need to search for a plane perpendicular to. */ + Eigen::Vector3f axis_; + + /** \brief The maximum allowed difference between the plane and the given axis. */ + double eps_angle_; + + /** \brief The sine of the angle*/ + double sin_angle_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_perpendicular_plane.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_perpendicular_plane.h new file mode 100755 index 0000000..a449707 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_perpendicular_plane.h @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelPerpendicularPlane defines a model for 3D plane segmentation using additional + * angular constraints. The plane must be perpendicular to a user-specified axis (\ref setAxis), up to a user-specified angle threshold (\ref setEpsAngle). + * In other words, the plane normal must be (nearly) parallel to the specified axis. + * The model coefficients are defined as: + * - \b a : the X coordinate of the plane's normal (normalized) + * - \b b : the Y coordinate of the plane's normal (normalized) + * - \b c : the Z coordinate of the plane's normal (normalized) + * - \b d : the fourth Hessian component of the plane's equation + * + * + * Code example for a plane model, perpendicular (within a 15 degrees tolerance) with the Z axis: + * \code + * SampleConsensusModelPerpendicularPlane model (cloud); + * model.setAxis (Eigen::Vector3f (0.0, 0.0, 1.0)); + * model.setEpsAngle (pcl::deg2rad (15)); + * \endcode + * + * \note Please remember that you need to specify an angle > 0 in order to activate the axis-angle constraint! + * + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelPerpendicularPlane : public SampleConsensusModelPlane + { + public: + using SampleConsensusModel::model_name_; + + using PointCloud = typename SampleConsensusModelPlane::PointCloud; + using PointCloudPtr = typename SampleConsensusModelPlane::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModelPlane::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelPerpendicularPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelPerpendicularPlane (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModelPlane (cloud, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + { + model_name_ = "SampleConsensusModelPerpendicularPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelPerpendicularPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelPerpendicularPlane (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModelPlane (cloud, indices, random) + , axis_ (Eigen::Vector3f::Zero ()) + , eps_angle_ (0.0) + { + model_name_ = "SampleConsensusModelPerpendicularPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelPerpendicularPlane () {} + + /** \brief Set the axis along which we need to search for a plane perpendicular to. + * \param[in] ax the axis along which we need to search for a plane perpendicular to + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } + + /** \brief Get the axis along which we need to search for a plane perpendicular to. */ + inline Eigen::Vector3f + getAxis () const { return (axis_); } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed difference between the plane normal and the given axis. + * \note You need to specify an angle > 0 in order to activate the axis-angle constraint! + */ + inline void + setEpsAngle (const double ea) { eps_angle_ = ea; } + + /** \brief Get the angle epsilon (delta) threshold. */ + inline double + getEpsAngle () const { return (eps_angle_); } + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Compute all distances from the cloud data to a given plane model. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Return a unique id for this model (SACMODEL_PERPENDICULAR_PLANE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_PERPENDICULAR_PLANE); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override; + + /** \brief The axis along which we need to search for a plane perpendicular to. */ + Eigen::Vector3f axis_; + + /** \brief The maximum allowed difference between the plane normal and the given axis. */ + double eps_angle_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_plane.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_plane.h new file mode 100755 index 0000000..bd447b8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_plane.h @@ -0,0 +1,272 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + + /** \brief Project a point on a planar model given by a set of normalized coefficients + * \param[in] p the input point to project + * \param[in] model_coefficients the coefficients of the plane (a, b, c, d) that satisfy ax+by+cz+d=0 + * \param[out] q the resultant projected point + */ + template inline void + projectPoint (const Point &p, const Eigen::Vector4f &model_coefficients, Point &q) + { + // Calculate the distance from the point to the plane + Eigen::Vector4f pp (p.x, p.y, p.z, 1); + // use normalized coefficients to calculate the scalar projection + float distance_to_plane = pp.dot(model_coefficients); + + + //TODO: Why doesn't getVector4Map work here? + //Eigen::Vector4f q_e = q.getVector4fMap (); + //q_e = pp - model_coefficients * distance_to_plane; + + Eigen::Vector4f q_e = pp - distance_to_plane * model_coefficients; + q.x = q_e[0]; + q.y = q_e[1]; + q.z = q_e[2]; + } + + /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0 + * \param p a point + * \param a the normalized a coefficient of a plane + * \param b the normalized b coefficient of a plane + * \param c the normalized c coefficient of a plane + * \param d the normalized d coefficient of a plane + * \ingroup sample_consensus + */ + template inline double + pointToPlaneDistanceSigned (const Point &p, double a, double b, double c, double d) + { + return (a * p.x + b * p.y + c * p.z + d); + } + + /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0 + * \param p a point + * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane + * \ingroup sample_consensus + */ + template inline double + pointToPlaneDistanceSigned (const Point &p, const Eigen::Vector4f &plane_coefficients) + { + return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] ); + } + + /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0 + * \param p a point + * \param a the normalized a coefficient of a plane + * \param b the normalized b coefficient of a plane + * \param c the normalized c coefficient of a plane + * \param d the normalized d coefficient of a plane + * \ingroup sample_consensus + */ + template inline double + pointToPlaneDistance (const Point &p, double a, double b, double c, double d) + { + return (std::abs (pointToPlaneDistanceSigned (p, a, b, c, d)) ); + } + + /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0 + * \param p a point + * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane + * \ingroup sample_consensus + */ + template inline double + pointToPlaneDistance (const Point &p, const Eigen::Vector4f &plane_coefficients) + { + return ( std::abs (pointToPlaneDistanceSigned (p, plane_coefficients)) ); + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief SampleConsensusModelPlane defines a model for 3D plane segmentation. + * The model coefficients are defined as: + * - \b a : the X coordinate of the plane's normal (normalized) + * - \b b : the Y coordinate of the plane's normal (normalized) + * - \b c : the Z coordinate of the plane's normal (normalized) + * - \b d : the fourth Hessian component of the plane's equation + * + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelPlane : public SampleConsensusModel + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::error_sqr_dists_; + using SampleConsensusModel::isModelValid; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelPlane (const PointCloudConstPtr &cloud, bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelPlane. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelPlane (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelPlane"; + sample_size_ = 3; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelPlane () {} + + /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from + * these samples and store them internally in model_coefficients_. The plane coefficients are: + * a, b, c, d (ax+by+cz+d=0) + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given plane model. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the plane coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the plane model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the model coefficients + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + /** \brief Create a new point cloud with inliers projected onto the plane model. + * \param[in] inliers the data inliers that we want to project on the plane model + * \param[in] model_coefficients the *normalized* coefficients of a plane model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given plane model coefficients. + * \param[in] indices the data indices that need to be tested against the plane model + * \param[in] model_coefficients the plane model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_PLANE). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_PLANE); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + private: + /** \brief Check if a sample of indices results in a good sample of points + * indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const std::vector &samples) const override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_registration.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_registration.h new file mode 100755 index 0000000..04dae53 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_registration.h @@ -0,0 +1,331 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection. + * \author Radu Bogdan Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelRegistration : public SampleConsensusModel + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::error_sqr_dists_; + using SampleConsensusModel::isModelValid; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelRegistration. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModel (cloud, random) + , target_ () + , sample_dist_thresh_ (0) + { + // Call our own setInputCloud + setInputCloud (cloud); + model_name_ = "SampleConsensusModelRegistration"; + sample_size_ = 3; + model_size_ = 16; + } + + /** \brief Constructor for base SampleConsensusModelRegistration. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + , target_ () + , sample_dist_thresh_ (0) + { + computeOriginalIndexMapping (); + computeSampleDistanceThreshold (cloud, indices); + model_name_ = "SampleConsensusModelRegistration"; + sample_size_ = 3; + model_size_ = 16; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelRegistration () {} + + /** \brief Provide a pointer to the input dataset + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + inline void + setInputCloud (const PointCloudConstPtr &cloud) override + { + SampleConsensusModel::setInputCloud (cloud); + computeOriginalIndexMapping (); + computeSampleDistanceThreshold (cloud); + } + + /** \brief Set the input point cloud target. + * \param[in] target the input point cloud target + */ + inline void + setInputTarget (const PointCloudConstPtr &target) + { + target_ = target; + indices_tgt_.reset (new std::vector); + // Cache the size and fill the target indices + int target_size = static_cast (target->size ()); + indices_tgt_->resize (target_size); + + for (int i = 0; i < target_size; ++i) + (*indices_tgt_)[i] = i; + computeOriginalIndexMapping (); + } + + /** \brief Set the input point cloud target. + * \param[in] target the input point cloud target + * \param[in] indices_tgt a vector of point indices to be used from \a target + */ + inline void + setInputTarget (const PointCloudConstPtr &target, const std::vector &indices_tgt) + { + target_ = target; + indices_tgt_.reset (new std::vector (indices_tgt)); + computeOriginalIndexMapping (); + } + + /** \brief Compute a 4x4 rigid transformation matrix from the samples given + * \param[in] samples the indices found as good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the transformed points to their correspondences + * \param[in] model_coefficients the 4x4 transformation matrix + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the 4x4 transformation matrix + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the 4x4 transformation using the given inlier set + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed transformation + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + void + projectPoints (const std::vector &, + const Eigen::VectorXf &, + PointCloud &, bool = true) const override + { + }; + + bool + doSamplesVerifyModel (const std::set &, + const Eigen::VectorXf &, + const double) const override + { + return (false); + } + + /** \brief Return a unique id for this model (SACMODEL_REGISTRATION). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_REGISTRATION); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const std::vector &samples) const override; + + /** \brief Computes an "optimal" sample distance threshold based on the + * principal directions of the input cloud. + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + inline void + computeSampleDistanceThreshold (const PointCloudConstPtr &cloud) + { + // Compute the principal directions via PCA + Eigen::Vector4f xyz_centroid; + Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero (); + + computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid); + + // Check if the covariance matrix is finite or not. + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + if (!std::isfinite (covariance_matrix.coeffRef (i, j))) + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); + + Eigen::Vector3f eigen_values; + pcl::eigen33 (covariance_matrix, eigen_values); + + // Compute the distance threshold for sample selection + sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; + sample_dist_thresh_ *= sample_dist_thresh_; + PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); + } + + /** \brief Computes an "optimal" sample distance threshold based on the + * principal directions of the input cloud. + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param indices + */ + inline void + computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, + const std::vector &indices) + { + // Compute the principal directions via PCA + Eigen::Vector4f xyz_centroid; + Eigen::Matrix3f covariance_matrix; + computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid); + + // Check if the covariance matrix is finite or not. + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + if (!std::isfinite (covariance_matrix.coeffRef (i, j))) + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); + + Eigen::Vector3f eigen_values; + pcl::eigen33 (covariance_matrix, eigen_values); + + // Compute the distance threshold for sample selection + sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; + sample_dist_thresh_ *= sample_dist_thresh_; + PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); + } + + /** \brief Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form + * solution of absolute orientation using unit quaternions + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from + * indices_src + * \param[out] transform the resultant transformation matrix (as model coefficients) + * + * This method is an implementation of: Horn, B. “Closed-Form Solution of Absolute Orientation Using Unit Quaternions,” JOSA A, Vol. 4, No. 4, 1987 + */ + void + estimateRigidTransformationSVD (const pcl::PointCloud &cloud_src, + const std::vector &indices_src, + const pcl::PointCloud &cloud_tgt, + const std::vector &indices_tgt, + Eigen::VectorXf &transform) const; + + /** \brief Compute mappings between original indices of the input_/target_ clouds. */ + void + computeOriginalIndexMapping () + { + if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ()) + return; + for (std::size_t i = 0; i < indices_->size (); ++i) + correspondences_[(*indices_)[i]] = (*indices_tgt_)[i]; + } + + /** \brief A boost shared pointer to the target point cloud data array. */ + PointCloudConstPtr target_; + + /** \brief A pointer to the vector of target point indices to use. */ + IndicesPtr indices_tgt_; + + /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */ + std::map correspondences_; + + /** \brief Internal distance threshold used for the sample selection step. */ + double sample_dist_thresh_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_registration_2d.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_registration_2d.h new file mode 100755 index 0000000..2f508d0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_registration_2d.h @@ -0,0 +1,223 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection using distances between 2D pixels + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelRegistration2D : public pcl::SampleConsensusModelRegistration + { + public: + using pcl::SampleConsensusModelRegistration::model_name_; + using pcl::SampleConsensusModelRegistration::input_; + using pcl::SampleConsensusModelRegistration::target_; + using pcl::SampleConsensusModelRegistration::indices_; + using pcl::SampleConsensusModelRegistration::indices_tgt_; + using pcl::SampleConsensusModelRegistration::error_sqr_dists_; + using pcl::SampleConsensusModelRegistration::correspondences_; + using pcl::SampleConsensusModelRegistration::sample_dist_thresh_; + using pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping; + using pcl::SampleConsensusModel::isModelValid; + + using PointCloud = typename pcl::SampleConsensusModel::PointCloud; + using PointCloudPtr = typename pcl::SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename pcl::SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Constructor for base SampleConsensusModelRegistration2D. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud, + bool random = false) + : pcl::SampleConsensusModelRegistration (cloud, random) + , projection_matrix_ (Eigen::Matrix3f::Identity ()) + { + // Call our own setInputCloud + setInputCloud (cloud); + model_name_ = "SampleConsensusModelRegistration2D"; + sample_size_ = 3; + model_size_ = 16; + } + + /** \brief Constructor for base SampleConsensusModelRegistration2D. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : pcl::SampleConsensusModelRegistration (cloud, indices, random) + , projection_matrix_ (Eigen::Matrix3f::Identity ()) + { + computeOriginalIndexMapping (); + computeSampleDistanceThreshold (cloud, indices); + model_name_ = "SampleConsensusModelRegistration2D"; + sample_size_ = 3; + model_size_ = 16; + } + + /** \brief Empty destructor */ + virtual ~SampleConsensusModelRegistration2D () {} + + /** \brief Compute all distances from the transformed points to their correspondences + * \param[in] model_coefficients the 4x4 transformation matrix + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the 4x4 transformation matrix + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers); + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + virtual std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; + + /** \brief Set the camera projection matrix. + * \param[in] projection_matrix the camera projection matrix + */ + inline void + setProjectionMatrix (const Eigen::Matrix3f &projection_matrix) + { projection_matrix_ = projection_matrix; } + + /** \brief Get the camera projection matrix. */ + inline Eigen::Matrix3f + getProjectionMatrix () const + { return (projection_matrix_); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const std::vector &samples) const; + + /** \brief Computes an "optimal" sample distance threshold based on the + * principal directions of the input cloud. + */ + inline void + computeSampleDistanceThreshold (const PointCloudConstPtr&) + { + //// Compute the principal directions via PCA + //Eigen::Vector4f xyz_centroid; + //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero (); + + //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid); + + //// Check if the covariance matrix is finite or not. + //for (int i = 0; i < 3; ++i) + // for (int j = 0; j < 3; ++j) + // if (!std::isfinite (covariance_matrix.coeffRef (i, j))) + // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); + + //Eigen::Vector3f eigen_values; + //pcl::eigen33 (covariance_matrix, eigen_values); + + //// Compute the distance threshold for sample selection + //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; + //sample_dist_thresh_ *= sample_dist_thresh_; + //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); + } + + /** \brief Computes an "optimal" sample distance threshold based on the + * principal directions of the input cloud. + */ + inline void + computeSampleDistanceThreshold (const PointCloudConstPtr&, + const std::vector&) + { + //// Compute the principal directions via PCA + //Eigen::Vector4f xyz_centroid; + //Eigen::Matrix3f covariance_matrix; + //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid); + + //// Check if the covariance matrix is finite or not. + //for (int i = 0; i < 3; ++i) + // for (int j = 0; j < 3; ++j) + // if (!std::isfinite (covariance_matrix.coeffRef (i, j))) + // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n"); + + //Eigen::Vector3f eigen_values; + //pcl::eigen33 (covariance_matrix, eigen_values); + + //// Compute the distance threshold for sample selection + //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0; + //sample_dist_thresh_ *= sample_dist_thresh_; + //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_); + } + + private: + /** \brief Camera projection matrix. */ + Eigen::Matrix3f projection_matrix_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_sphere.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_sphere.h new file mode 100755 index 0000000..ae00f81 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_sphere.h @@ -0,0 +1,271 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelSphere defines a model for 3D sphere segmentation. + * The model coefficients are defined as: + * - \b center.x : the X coordinate of the sphere's center + * - \b center.y : the Y coordinate of the sphere's center + * - \b center.z : the Z coordinate of the sphere's center + * - \b radius : the sphere's radius + * + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelSphere : public SampleConsensusModel + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModel::error_sqr_dists_; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelSphere. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelSphere (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelSphere"; + sample_size_ = 4; + model_size_ = 4; + } + + /** \brief Constructor for base SampleConsensusModelSphere. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelSphere (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelSphere"; + sample_size_ = 4; + model_size_ = 4; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelSphere () {} + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + SampleConsensusModelSphere (const SampleConsensusModelSphere &source) : + SampleConsensusModel () + { + *this = source; + model_name_ = "SampleConsensusModelSphere"; + } + + /** \brief Copy constructor. + * \param[in] source the model to copy into this + */ + inline SampleConsensusModelSphere& + operator = (const SampleConsensusModelSphere &source) + { + SampleConsensusModel::operator=(source); + return (*this); + } + + /** \brief Check whether the given index samples can form a valid sphere model, compute the model + * coefficients from these samples and store them internally in model_coefficients. + * The sphere coefficients are: x, y, z, R. + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all distances from the cloud data to a given sphere model. + * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to + * \param[out] distances the resultant estimated distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the sphere model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the optimization + * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + /** \brief Create a new point cloud with inliers projected onto the sphere model. + * \param[in] inliers the data inliers that we want to project on the sphere model + * \param[in] model_coefficients the coefficients of a sphere model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + * \todo implement this. + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given sphere model coefficients. + * \param[in] indices the data indices that need to be tested against the sphere model + * \param[in] model_coefficients the sphere model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_SPHERE). */ + inline pcl::SacModel getModelType () const override { return (SACMODEL_SPHERE); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check whether a model is valid given the user constraints. + * \param[in] model_coefficients the set of model coefficients + */ + bool + isModelValid (const Eigen::VectorXf &model_coefficients) const override + { + if (!SampleConsensusModel::isModelValid (model_coefficients)) + return (false); + + if (radius_min_ != -std::numeric_limits::max() && model_coefficients[3] < radius_min_) + return (false); + if (radius_max_ != std::numeric_limits::max() && model_coefficients[3] > radius_max_) + return (false); + + return (true); + } + + /** \brief Check if a sample of indices results in a good sample of points + * indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const override; + + private: + struct OptimizationFunctor : pcl::Functor + { + /** Functor constructor + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelSphere *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} + + /** Cost function to be minimized + * \param[in] x the variables array + * \param[out] fvec the resultant functions evaluations + * \return 0 + */ + int + operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const + { + Eigen::Vector4f cen_t; + cen_t[3] = 0; + for (int i = 0; i < values (); ++i) + { + // Compute the difference between the center of the sphere and the datapoint X_i + cen_t[0] = model_->input_->points[indices_[i]].x - x[0]; + cen_t[1] = model_->input_->points[indices_[i]].y - x[1]; + cen_t[2] = model_->input_->points[indices_[i]].z - x[2]; + + // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R + fvec[i] = std::sqrt (cen_t.dot (cen_t)) - x[3]; + } + return (0); + } + + const pcl::SampleConsensusModelSphere *model_; + const std::vector &indices_; + }; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_stick.h b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_stick.h new file mode 100755 index 0000000..865997f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sample_consensus/sac_model_stick.h @@ -0,0 +1,203 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: sac_model_line.h 2326 2011-08-31 07:48:25Z rusu $ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief SampleConsensusModelStick defines a model for 3D stick segmentation. + * A stick is a line with a user given minimum/maximum width. + * The model coefficients are defined as: + * - \b point_on_line.x : the X coordinate of a point on the line + * - \b point_on_line.y : the Y coordinate of a point on the line + * - \b point_on_line.z : the Z coordinate of a point on the line + * - \b line_direction.x : the X coordinate of a line's direction + * - \b line_direction.y : the Y coordinate of a line's direction + * - \b line_direction.z : the Z coordinate of a line's direction + * - \b line_width : the width of the line + * + * \author Radu B. Rusu + * \ingroup sample_consensus + */ + template + class SampleConsensusModelStick : public SampleConsensusModel + { + public: + using SampleConsensusModel::model_name_; + using SampleConsensusModel::input_; + using SampleConsensusModel::indices_; + using SampleConsensusModel::radius_min_; + using SampleConsensusModel::radius_max_; + using SampleConsensusModel::error_sqr_dists_; + using SampleConsensusModel::isModelValid; + + using PointCloud = typename SampleConsensusModel::PointCloud; + using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; + using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief Constructor for base SampleConsensusModelStick. + * \param[in] cloud the input point cloud dataset + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelStick (const PointCloudConstPtr &cloud, + bool random = false) + : SampleConsensusModel (cloud, random) + { + model_name_ = "SampleConsensusModelStick"; + sample_size_ = 2; + model_size_ = 7; + } + + /** \brief Constructor for base SampleConsensusModelStick. + * \param[in] cloud the input point cloud dataset + * \param[in] indices a vector of point indices to be used from \a cloud + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SampleConsensusModelStick (const PointCloudConstPtr &cloud, + const std::vector &indices, + bool random = false) + : SampleConsensusModel (cloud, indices, random) + { + model_name_ = "SampleConsensusModelStick"; + sample_size_ = 2; + model_size_ = 7; + } + + /** \brief Empty destructor */ + ~SampleConsensusModelStick () {} + + /** \brief Check whether the given index samples can form a valid stick model, compute the model coefficients from + * these samples and store them internally in model_coefficients_. The stick coefficients are represented by a + * point and a line direction + * \param[in] samples the point indices found as possible good candidates for creating a valid model + * \param[out] model_coefficients the resultant model coefficients + */ + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const override; + + /** \brief Compute all squared distances from the cloud data to a given stick model. + * \param[in] model_coefficients the coefficients of a stick model that we need to compute distances to + * \param[out] distances the resultant estimated squared distances + */ + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const override; + + /** \brief Select all the points which respect the given model coefficients as inliers. + * \param[in] model_coefficients the coefficients of a stick model that we need to compute distances to + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + * \param[out] inliers the resultant model inliers + */ + void + selectWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold, + std::vector &inliers) override; + + /** \brief Count all the points which respect the given model coefficients as inliers. + * + * \param[in] model_coefficients the coefficients of a model that we need to compute distances to + * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers + * \return the resultant number of inliers + */ + std::size_t + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Recompute the stick coefficients using the given inlier set and return them to the user. + * @note: these are the coefficients of the stick model after refinement (e.g. after SVD) + * \param[in] inliers the data inliers found as supporting the model + * \param[in] model_coefficients the initial guess for the model coefficients + * \param[out] optimized_coefficients the resultant recomputed coefficients after optimization + */ + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const override; + + /** \brief Create a new point cloud with inliers projected onto the stick model. + * \param[in] inliers the data inliers that we want to project on the stick model + * \param[in] model_coefficients the *normalized* coefficients of a stick model + * \param[out] projected_points the resultant projected points + * \param[in] copy_data_fields set to true if we need to copy the other data fields + */ + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const override; + + /** \brief Verify whether a subset of indices verifies the given stick model coefficients. + * \param[in] indices the data indices that need to be tested against the plane model + * \param[in] model_coefficients the plane model coefficients + * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers + */ + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const override; + + /** \brief Return a unique id for this model (SACMODEL_STICK). */ + inline pcl::SacModel + getModelType () const override { return (SACMODEL_STICK); } + + protected: + using SampleConsensusModel::sample_size_; + using SampleConsensusModel::model_size_; + + /** \brief Check if a sample of indices results in a good sample of points + * indices. + * \param[in] samples the resultant index samples + */ + bool + isSampleGood (const std::vector &samples) const override; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/search/brute_force.h b/deps_install/include/pcl-1.10/pcl/search/brute_force.h new file mode 100755 index 0000000..ec97f4a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/brute_force.h @@ -0,0 +1,146 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +namespace pcl +{ + namespace search + { + /** \brief Implementation of a simple brute force search algorithm. + * \author Suat Gedikli + * \ingroup search + */ + template + class BruteForce: public Search + { + using PointCloud = typename Search::PointCloud; + using PointCloudConstPtr = typename Search::PointCloudConstPtr; + + using IndicesPtr = shared_ptr >; + using IndicesConstPtr = shared_ptr >; + + using pcl::search::Search::input_; + using pcl::search::Search::indices_; + using pcl::search::Search::sorted_results_; + + struct Entry + { + Entry (int idx, float dist) : index (idx), distance (dist) {} + + Entry () : index (0), distance (0) {} + unsigned index; + float distance; + + inline bool + operator < (const Entry& other) const + { + return (distance < other.distance); + } + + inline bool + operator > (const Entry& other) const + { + return (distance > other.distance); + } + }; + + // replace by some metric functor + float getDistSqr (const PointT& point1, const PointT& point2) const; + public: + BruteForce (bool sorted_results = false) + : Search ("BruteForce", sorted_results) + { + } + + /** \brief Destructor for KdTree. */ + + ~BruteForce () + { + } + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + int + nearestKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const override; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + int + radiusSearch (const PointT& point, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn = 0) const override; + + private: + int + denseKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const; + + int + sparseKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const; + + int + denseRadiusSearch (const PointT& point, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn = 0) const; + + int + sparseRadiusSearch (const PointT& point, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn = 0) const; + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/search/flann_search.h b/deps_install/include/pcl-1.10/pcl/search/flann_search.h new file mode 100755 index 0000000..95f6105 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/flann_search.h @@ -0,0 +1,371 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include +#include + +namespace flann +{ + template class NNIndex; + template struct L2; + template struct L2_Simple; + template class Matrix; +} + +namespace pcl +{ + namespace search + { + + /** \brief @b search::FlannSearch is a generic FLANN wrapper class for the new search interface. + * It is able to wrap any FLANN index type, e.g. the kd tree as well as indices for high-dimensional + * searches and intended as a more powerful and cleaner successor to KdTreeFlann. + * + * By default, this class creates a single kd tree for indexing the input data. However, for high dimensions + * (> 10), it is often better to use the multiple randomized kd tree index provided by FLANN in combination with + * the \ref flann::L2 distance functor. During search in this type of index, the number of checks to perform before + * terminating the search can be controlled. Here is a code example if a high-dimensional 2-NN search: + * + * \code + * // Feature and distance type + * using FeatureT = SHOT352; + * using DistanceT = flann::L2; + * + * // Search and index types + * using SearchT = search::FlannSearch; + * using CreatorPtrT = typename SearchT::FlannIndexCreatorPtr; + * using IndexT = typename SearchT::KdTreeMultiIndexCreator; + * using RepresentationPtrT = typename SearchT::PointRepresentationPtr; + * + * // Features + * PointCloud::Ptr query, target; + * + * // Fill query and target with calculated features... + * + * // Instantiate search object with 4 randomized trees and 256 checks + * SearchT search (true, CreatorPtrT (new IndexT (4))); + * search.setPointRepresentation (RepresentationPtrT (new DefaultFeatureRepresentation)); + * search.setChecks (256); + * search.setInputCloud (target); + * + * // Do search + * std::vector > k_indices; + * std::vector > k_sqr_distances; + * search.nearestKSearch (*query, std::vector (), 2, k_indices, k_sqr_distances); + * \endcode + * + * \author Andreas Muetzel + * \author Anders Glent Buch (multiple randomized kd tree interface) + * \ingroup search + */ + template > + class FlannSearch: public Search + { + using Search::input_; + using Search::indices_; + using Search::sorted_results_; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloud = typename Search::PointCloud; + using PointCloudConstPtr = typename Search::PointCloudConstPtr; + + using typename Search::IndicesPtr; + using typename Search::IndicesConstPtr; + + using MatrixPtr = shared_ptr >; + using MatrixConstPtr = shared_ptr >; + + using Index = flann::NNIndex; + using IndexPtr = shared_ptr >; + + using PointRepresentation = pcl::PointRepresentation; + using PointRepresentationPtr = typename PointRepresentation::Ptr; + using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; + + /** \brief Helper class that creates a FLANN index from a given FLANN matrix. To + * use a FLANN index type with FlannSearch, implement this interface and + * pass an object of the new type to the FlannSearch constructor. + * See the implementation of KdTreeIndexCreator for an example. + */ + class FlannIndexCreator + { + public: + /** \brief Create a FLANN Index from the input data. + * \param[in] data The FLANN matrix containing the input. + * \return The FLANN index. + */ + virtual IndexPtr createIndex (MatrixConstPtr data)=0; + + /** \brief destructor + */ + virtual ~FlannIndexCreator () {} + }; + using FlannIndexCreatorPtr = shared_ptr; + + /** \brief Creates a FLANN KdTreeSingleIndex from the given input data. + */ + class KdTreeIndexCreator: public FlannIndexCreator + { + public: + /** \param[in] max_leaf_size All FLANN kd trees created by this class will have + * a maximum of max_leaf_size points per leaf node. Higher values make index creation + * cheaper, but search more costly (and the other way around). + */ + KdTreeIndexCreator (unsigned int max_leaf_size=15) : max_leaf_size_ (max_leaf_size){} + + /** \brief Empty destructor */ + ~KdTreeIndexCreator () {} + + /** \brief Create a FLANN Index from the input data. + * \param[in] data The FLANN matrix containing the input. + * \return The FLANN index. + */ + IndexPtr createIndex (MatrixConstPtr data) override; + private: + unsigned int max_leaf_size_; + }; + + /** \brief Creates a FLANN KdTreeSingleIndex from the given input data. + */ + class KMeansIndexCreator: public FlannIndexCreator + { + public: + /** \brief All FLANN kd trees created by this class will have + * a maximum of max_leaf_size points per leaf node. Higher values make index creation + * cheaper, but search more costly (and the other way around). + */ + KMeansIndexCreator (){} + + /** \brief Empty destructor */ + virtual ~KMeansIndexCreator () {} + + /** \brief Create a FLANN Index from the input data. + * \param[in] data The FLANN matrix containing the input. + * \return The FLANN index. + */ + virtual IndexPtr createIndex (MatrixConstPtr data); + private: + }; + + /** \brief Creates a FLANN KdTreeIndex of multiple randomized trees from the given input data, + * suitable for feature matching. Note that in this case, it is often more efficient to use the + * \ref flann::L2 distance functor. + */ + class KdTreeMultiIndexCreator: public FlannIndexCreator + { + public: + /** \param[in] trees Number of randomized trees to create. + */ + KdTreeMultiIndexCreator (int trees = 4) : trees_ (trees) {} + + /** \brief Empty destructor */ + virtual ~KdTreeMultiIndexCreator () {} + + /** \brief Create a FLANN Index from the input data. + * \param[in] data The FLANN matrix containing the input. + * \return The FLANN index. + */ + virtual IndexPtr createIndex (MatrixConstPtr data); + private: + int trees_; + }; + + FlannSearch (bool sorted = true, FlannIndexCreatorPtr creator = FlannIndexCreatorPtr (new KdTreeIndexCreator ())); + + /** \brief Destructor for FlannSearch. */ + + ~FlannSearch (); + + + //void + //setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. + * \param[in] eps precision (error bound) for nearest neighbors searches + */ + inline void + setEpsilon (double eps) + { + eps_ = eps; + } + + /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ + inline double + getEpsilon () + { + return (eps_); + } + + /** \brief Set the number of checks to perform during approximate searches in multiple randomized trees. + * \param[in] checks number of checks to perform during approximate searches in multiple randomized trees. + */ + inline void + setChecks (int checks) + { + checks_ = checks; + } + + /** \brief Get the number of checks to perform during approximate searches in multiple randomized trees. */ + inline int + getChecks () + { + return (checks_); + } + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud + */ + void + setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override; + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + int + nearestKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_sqr_distances) const override; + + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] cloud the point cloud data + * \param[in] indices a vector of point cloud indices to query for nearest neighbors + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i + */ + void + nearestKSearch (const PointCloud& cloud, const std::vector& indices, int k, + std::vector< std::vector >& k_indices, std::vector< std::vector >& k_sqr_distances) const override; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + int + radiusSearch (const PointT& point, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn = 0) const override; + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] cloud the point cloud data + * \param[in] indices a vector of point cloud indices to query for nearest neighbors + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value + */ + void + radiusSearch (const PointCloud& cloud, const std::vector& indices, double radius, std::vector< std::vector >& k_indices, + std::vector< std::vector >& k_sqr_distances, unsigned int max_nn=0) const override; + + /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. + * \param[in] point_representation the const boost shared pointer to a PointRepresentation + */ + inline void + setPointRepresentation (const PointRepresentationConstPtr &point_representation) + { + point_representation_ = point_representation; + dim_ = point_representation->getNumberOfDimensions (); + if (input_) // re-create the tree, since point_representation might change things such as the scaling of the point clouds. + setInputCloud (input_, indices_); + } + + /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */ + inline PointRepresentationConstPtr const + getPointRepresentation () + { + return (point_representation_); + } + + protected: + + /** \brief converts the input data to a format usable by FLANN + */ + void convertInputToFlannMatrix(); + + /** The FLANN index. + */ + IndexPtr index_; + + /** The index creator, used to (re-) create the index when the search data is passed. + */ + FlannIndexCreatorPtr creator_; + + /** Input data in FLANN format. + */ + MatrixPtr input_flann_; + + /** Epsilon for approximate NN search. + */ + float eps_; + + /** Number of checks to perform for approximate NN search using the multiple randomized tree index + */ + int checks_; + + bool input_copied_for_flann_; + + PointRepresentationConstPtr point_representation_; + + int dim_; + + std::vector index_mapping_; + bool identity_mapping_; + + }; + } +} + +#define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch; diff --git a/deps_install/include/pcl-1.10/pcl/search/impl/brute_force.hpp b/deps_install/include/pcl-1.10/pcl/search/impl/brute_force.hpp new file mode 100755 index 0000000..118d238 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/impl/brute_force.hpp @@ -0,0 +1,352 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ +#define PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::search::BruteForce::getDistSqr ( + const PointT& point1, const PointT& point2) const +{ + return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::BruteForce::nearestKSearch ( + const PointT& point, int k, std::vector& k_indices, std::vector& k_distances) const +{ + assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + k_indices.clear (); + k_distances.clear (); + if (k < 1) + return 0; + + if (input_->is_dense) + return denseKSearch (point, k, k_indices, k_distances); + return sparseKSearch (point, k, k_indices, k_distances); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::BruteForce::denseKSearch ( + const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const +{ + // container for first k elements -> O(1) for insertion, since order not required here + std::vector result; + result.reserve (k); + std::priority_queue queue; + if (indices_) + { + std::vector::const_iterator iIt =indices_->begin (); + std::vector::const_iterator iEnd = indices_->begin () + std::min (static_cast (k), static_cast (indices_->size ())); + for (; iIt != iEnd; ++iIt) + result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point))); + + queue = std::priority_queue (result.begin (), result.end ()); + + // add the rest + Entry entry; + for (; iIt != indices_->end (); ++iIt) + { + entry.distance = getDistSqr (input_->points[*iIt], point); + if (queue.top ().distance > entry.distance) + { + entry.index = *iIt; + queue.pop (); + queue.push (entry); + } + } + } + else + { + Entry entry; + for (entry.index = 0; entry.index < std::min (static_cast (k), static_cast (input_->size ())); ++entry.index) + { + entry.distance = getDistSqr (input_->points[entry.index], point); + result.push_back (entry); + } + + queue = std::priority_queue (result.begin (), result.end ()); + + // add the rest + for (; entry.index < input_->size (); ++entry.index) + { + entry.distance = getDistSqr (input_->points[entry.index], point); + if (queue.top ().distance > entry.distance) + { + queue.pop (); + queue.push (entry); + } + } + } + + k_indices.resize (queue.size ()); + k_distances.resize (queue.size ()); + std::size_t idx = queue.size () - 1; + while (!queue.empty ()) + { + k_indices [idx] = queue.top ().index; + k_distances [idx] = queue.top ().distance; + queue.pop (); + --idx; + } + + return (static_cast (k_indices.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::BruteForce::sparseKSearch ( + const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const +{ + // result used to collect the first k neighbors -> unordered + std::vector result; + result.reserve (k); + + std::priority_queue queue; + if (indices_) + { + std::vector::const_iterator iIt =indices_->begin (); + for (; iIt != indices_->end () && result.size () < static_cast (k); ++iIt) + { + if (std::isfinite (input_->points[*iIt].x)) + result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point))); + } + + queue = std::priority_queue (result.begin (), result.end ()); + + // either we have k elements, or there are none left to iterate >in either case we're fine + // add the rest + Entry entry; + for (; iIt != indices_->end (); ++iIt) + { + if (!std::isfinite (input_->points[*iIt].x)) + continue; + + entry.distance = getDistSqr (input_->points[*iIt], point); + if (queue.top ().distance > entry.distance) + { + entry.index = *iIt; + queue.pop (); + queue.push (entry); + } + } + } + else + { + Entry entry; + for (entry.index = 0; entry.index < input_->size () && result.size () < static_cast (k); ++entry.index) + { + if (std::isfinite (input_->points[entry.index].x)) + { + entry.distance = getDistSqr (input_->points[entry.index], point); + result.push_back (entry); + } + } + queue = std::priority_queue (result.begin (), result.end ()); + + // add the rest + for (; entry.index < input_->size (); ++entry.index) + { + if (!std::isfinite (input_->points[entry.index].x)) + continue; + + entry.distance = getDistSqr (input_->points[entry.index], point); + if (queue.top ().distance > entry.distance) + { + queue.pop (); + queue.push (entry); + } + } + } + + k_indices.resize (queue.size ()); + k_distances.resize (queue.size ()); + std::size_t idx = queue.size () - 1; + while (!queue.empty ()) + { + k_indices [idx] = queue.top ().index; + k_distances [idx] = queue.top ().distance; + queue.pop (); + --idx; + } + return (static_cast (k_indices.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::BruteForce::denseRadiusSearch ( + const PointT& point, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn) const +{ + radius *= radius; + + std::size_t reserve = max_nn; + if (reserve == 0) + { + if (indices_) + reserve = std::min (indices_->size (), input_->size ()); + else + reserve = input_->size (); + } + k_indices.reserve (reserve); + k_sqr_distances.reserve (reserve); + float distance; + if (indices_) + { + for (std::vector::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt) + { + distance = getDistSqr (input_->points[*iIt], point); + if (distance <= radius) + { + k_indices.push_back (*iIt); + k_sqr_distances.push_back (distance); + if (k_indices.size () == max_nn) // max_nn = 0 -> never true + break; + } + } + } + else + { + for (std::size_t index = 0; index < input_->size (); ++index) + { + distance = getDistSqr (input_->points[index], point); + if (distance <= radius) + { + k_indices.push_back (index); + k_sqr_distances.push_back (distance); + if (k_indices.size () == max_nn) // never true if max_nn = 0 + break; + } + } + } + + if (sorted_results_) + this->sortResults (k_indices, k_sqr_distances); + + return (static_cast (k_indices.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::BruteForce::sparseRadiusSearch ( + const PointT& point, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn) const +{ + radius *= radius; + + std::size_t reserve = max_nn; + if (reserve == 0) + { + if (indices_) + reserve = std::min (indices_->size (), input_->size ()); + else + reserve = input_->size (); + } + k_indices.reserve (reserve); + k_sqr_distances.reserve (reserve); + + float distance; + if (indices_) + { + for (std::vector::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt) + { + if (!std::isfinite (input_->points[*iIt].x)) + continue; + + distance = getDistSqr (input_->points[*iIt], point); + if (distance <= radius) + { + k_indices.push_back (*iIt); + k_sqr_distances.push_back (distance); + if (k_indices.size () == max_nn) // never true if max_nn = 0 + break; + } + } + } + else + { + for (std::size_t index = 0; index < input_->size (); ++index) + { + if (!std::isfinite (input_->points[index].x)) + continue; + distance = getDistSqr (input_->points[index], point); + if (distance <= radius) + { + k_indices.push_back (index); + k_sqr_distances.push_back (distance); + if (k_indices.size () == max_nn) // never true if max_nn = 0 + break; + } + } + } + + if (sorted_results_) + this->sortResults (k_indices, k_sqr_distances); + + return (static_cast (k_indices.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::BruteForce::radiusSearch ( + const PointT& point, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn) const +{ + assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + k_indices.clear (); + k_sqr_distances.clear (); + if (radius <= 0) + return 0; + + if (input_->is_dense) + return denseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn); + return sparseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn); +} + +#define PCL_INSTANTIATE_BruteForce(T) template class PCL_EXPORTS pcl::search::BruteForce; + +#endif //PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ diff --git a/deps_install/include/pcl-1.10/pcl/search/impl/flann_search.hpp b/deps_install/include/pcl-1.10/pcl/search/impl/flann_search.hpp new file mode 100755 index 0000000..baf01e1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/impl/flann_search.hpp @@ -0,0 +1,434 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_ +#define PCL_SEARCH_IMPL_FLANN_SEARCH_H_ + +#include +#include +#include + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +typename pcl::search::FlannSearch::IndexPtr +pcl::search::FlannSearch::KdTreeIndexCreator::createIndex (MatrixConstPtr data) +{ + return (IndexPtr (new flann::KDTreeSingleIndex (*data,flann::KDTreeSingleIndexParams (max_leaf_size_)))); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +typename pcl::search::FlannSearch::IndexPtr +pcl::search::FlannSearch::KMeansIndexCreator::createIndex (MatrixConstPtr data) +{ + return (IndexPtr (new flann::KMeansIndex (*data,flann::KMeansIndexParams ()))); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +typename pcl::search::FlannSearch::IndexPtr +pcl::search::FlannSearch::KdTreeMultiIndexCreator::createIndex (MatrixConstPtr data) +{ + return (IndexPtr (new flann::KDTreeIndex (*data, flann::KDTreeIndexParams (trees_)))); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::search::FlannSearch::FlannSearch(bool sorted, FlannIndexCreatorPtr creator) : pcl::search::Search ("FlannSearch",sorted), + index_(), creator_ (creator), eps_ (0), checks_ (32), input_copied_for_flann_ (false), point_representation_ (new DefaultPointRepresentation), + dim_ (0), identity_mapping_() +{ + dim_ = point_representation_->getNumberOfDimensions (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::search::FlannSearch::~FlannSearch() +{ + if (input_copied_for_flann_) + delete [] input_flann_->ptr(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::FlannSearch::setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices) +{ + input_ = cloud; + indices_ = indices; + convertInputToFlannMatrix (); + index_ = creator_->createIndex (input_flann_); + index_->buildIndex (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::FlannSearch::nearestKSearch (const PointT &point, int k, std::vector &indices, std::vector &dists) const +{ + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally + bool can_cast = point_representation_->isTrivial (); + + float* data = nullptr; + if (!can_cast) + { + data = new float [point_representation_->getNumberOfDimensions ()]; + point_representation_->vectorize (point,data); + } + + float* cdata = can_cast ? const_cast (reinterpret_cast (&point)): data; + const flann::Matrix m (cdata ,1, point_representation_->getNumberOfDimensions ()); + + flann::SearchParams p; + p.eps = eps_; + p.sorted = sorted_results_; + p.checks = checks_; + if (indices.size() != static_cast (k)) + indices.resize (k,-1); + if (dists.size() != static_cast (k)) + dists.resize (k); + flann::Matrix i (&indices[0],1,k); + flann::Matrix d (&dists[0],1,k); + int result = index_->knnSearch (m,i,d,k, p); + + delete [] data; + + if (!identity_mapping_) + { + for (std::size_t i = 0; i < static_cast (k); ++i) + { + int& neighbor_index = indices[i]; + neighbor_index = index_mapping_[neighbor_index]; + } + } + return result; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::FlannSearch::nearestKSearch ( + const PointCloud& cloud, const std::vector& indices, int k, std::vector< std::vector >& k_indices, + std::vector< std::vector >& k_sqr_distances) const +{ + if (indices.empty ()) + { + k_indices.resize (cloud.size ()); + k_sqr_distances.resize (cloud.size ()); + + if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally + { + for (std::size_t i = 0; i < cloud.size(); i++) + { + assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + } + } + + bool can_cast = point_representation_->isTrivial (); + + // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix! + float* data=nullptr; + if (!can_cast) + { + data = new float[dim_*cloud.size ()]; + for (std::size_t i = 0; i < cloud.size (); ++i) + { + float* out = data+i*dim_; + point_representation_->vectorize (cloud[i],out); + } + } + + // const cast is evil, but the matrix constructor won't change the data, and the + // search won't change the matrix + float* cdata = can_cast ? const_cast (reinterpret_cast (&cloud[0])): data; + const flann::Matrix m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) ); + + flann::SearchParams p; + p.sorted = sorted_results_; + p.eps = eps_; + p.checks = checks_; + index_->knnSearch (m,k_indices,k_sqr_distances,k, p); + + delete [] data; + } + else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here. + { + k_indices.resize (indices.size ()); + k_sqr_distances.resize (indices.size ()); + + if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally + { + for (std::size_t i = 0; i < indices.size(); i++) + { + assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + } + } + + float* data=new float [dim_*indices.size ()]; + for (std::size_t i = 0; i < indices.size (); ++i) + { + float* out = data+i*dim_; + point_representation_->vectorize (cloud[indices[i]],out); + } + const flann::Matrix m (data ,indices.size (), point_representation_->getNumberOfDimensions ()); + + flann::SearchParams p; + p.sorted = sorted_results_; + p.eps = eps_; + p.checks = checks_; + index_->knnSearch (m,k_indices,k_sqr_distances,k, p); + + delete[] data; + } + if (!identity_mapping_) + { + for (auto &k_index : k_indices) + { + for (int &neighbor_index : k_index) + { + neighbor_index = index_mapping_[neighbor_index]; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::FlannSearch::radiusSearch (const PointT& point, double radius, + std::vector &indices, std::vector &distances, + unsigned int max_nn) const +{ + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally + bool can_cast = point_representation_->isTrivial (); + + float* data = nullptr; + if (!can_cast) + { + data = new float [point_representation_->getNumberOfDimensions ()]; + point_representation_->vectorize (point,data); + } + + float* cdata = can_cast ? const_cast (reinterpret_cast (&point)) : data; + const flann::Matrix m (cdata ,1, point_representation_->getNumberOfDimensions ()); + + flann::SearchParams p; + p.sorted = sorted_results_; + p.eps = eps_; + p.max_neighbors = max_nn > 0 ? max_nn : -1; + p.checks = checks_; + std::vector > i (1); + std::vector > d (1); + int result = index_->radiusSearch (m,i,d,static_cast (radius * radius), p); + + delete [] data; + indices = i [0]; + distances = d [0]; + + if (!identity_mapping_) + { + for (int &neighbor_index : indices) + { + neighbor_index = index_mapping_ [neighbor_index]; + } + } + return result; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::FlannSearch::radiusSearch ( + const PointCloud& cloud, const std::vector& indices, double radius, std::vector< std::vector >& k_indices, + std::vector< std::vector >& k_sqr_distances, unsigned int max_nn) const +{ + if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix! + { + k_indices.resize (cloud.size ()); + k_sqr_distances.resize (cloud.size ()); + + if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally + { + for (std::size_t i = 0; i < cloud.size(); i++) + { + assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); + } + } + + bool can_cast = point_representation_->isTrivial (); + + float* data = nullptr; + if (!can_cast) + { + data = new float[dim_*cloud.size ()]; + for (std::size_t i = 0; i < cloud.size (); ++i) + { + float* out = data+i*dim_; + point_representation_->vectorize (cloud[i],out); + } + } + + float* cdata = can_cast ? const_cast (reinterpret_cast (&cloud[0])) : data; + const flann::Matrix m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float)); + + flann::SearchParams p; + p.sorted = sorted_results_; + p.eps = eps_; + p.checks = checks_; + // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors + p.max_neighbors = max_nn != 0 ? max_nn : -1; + index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast (radius * radius), p); + + delete [] data; + } + else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here. + { + k_indices.resize (indices.size ()); + k_sqr_distances.resize (indices.size ()); + + if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally + { + for (std::size_t i = 0; i < indices.size(); i++) + { + assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); + } + } + + float* data = new float [dim_ * indices.size ()]; + for (std::size_t i = 0; i < indices.size (); ++i) + { + float* out = data+i*dim_; + point_representation_->vectorize (cloud[indices[i]], out); + } + const flann::Matrix m (data, cloud.size (), point_representation_->getNumberOfDimensions ()); + + flann::SearchParams p; + p.sorted = sorted_results_; + p.eps = eps_; + p.checks = checks_; + // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors + p.max_neighbors = max_nn != 0 ? max_nn : -1; + index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast (radius * radius), p); + + delete[] data; + } + if (!identity_mapping_) + { + for (auto &k_index : k_indices) + { + for (int &neighbor_index : k_index) + { + neighbor_index = index_mapping_[neighbor_index]; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::FlannSearch::convertInputToFlannMatrix () +{ + std::size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size (); + + if (input_copied_for_flann_) + delete input_flann_->ptr(); + input_copied_for_flann_ = true; + index_mapping_.clear(); + identity_mapping_ = true; + + //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float)); + //index_mapping_.reserve(original_no_of_points); + //identity_mapping_ = true; + + if (!indices_ || indices_->empty ()) + { + // best case: all points can be passed to flann without any conversions + if (input_->is_dense && point_representation_->isTrivial ()) + { + // const cast is evil, but flann won't change the data + input_flann_ = MatrixPtr (new flann::Matrix (const_cast(reinterpret_cast(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (PointT))); + input_copied_for_flann_ = false; + } + else + { + input_flann_ = MatrixPtr (new flann::Matrix (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ())); + float* cloud_ptr = input_flann_->ptr(); + for (std::size_t i = 0; i < original_no_of_points; ++i) + { + const PointT& point = (*input_)[i]; + // Check if the point is invalid + if (!point_representation_->isValid (point)) + { + identity_mapping_ = false; + continue; + } + + index_mapping_.push_back (static_cast (i)); // If the returned index should be for the indices vector + + point_representation_->vectorize (point, cloud_ptr); + cloud_ptr += dim_; + } + } + + } + else + { + input_flann_ = MatrixPtr (new flann::Matrix (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ())); + float* cloud_ptr = input_flann_->ptr(); + for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index) + { + int cloud_index = (*indices_)[indices_index]; + const PointT& point = (*input_)[cloud_index]; + // Check if the point is invalid + if (!point_representation_->isValid (point)) + { + identity_mapping_ = false; + continue; + } + + index_mapping_.push_back (static_cast (indices_index)); // If the returned index should be for the indices vector + + point_representation_->vectorize (point, cloud_ptr); + cloud_ptr += dim_; + } + } + if (input_copied_for_flann_) + input_flann_->rows = index_mapping_.size (); +} + +#define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/search/impl/kdtree.hpp b/deps_install/include/pcl-1.10/pcl/search/impl/kdtree.hpp new file mode 100755 index 0000000..a6d7fb2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/impl/kdtree.hpp @@ -0,0 +1,109 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEARCH_KDTREE_IMPL_HPP_ +#define PCL_SEARCH_KDTREE_IMPL_HPP_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::search::KdTree::KdTree (bool sorted) + : pcl::search::Search ("KdTree", sorted) + , tree_ (new Tree (sorted)) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::KdTree::setPointRepresentation ( + const PointRepresentationConstPtr &point_representation) +{ + tree_->setPointRepresentation (point_representation); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::KdTree::setSortedResults (bool sorted_results) +{ + sorted_results_ = sorted_results; + tree_->setSortedResults (sorted_results); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::KdTree::setEpsilon (float eps) +{ + tree_->setEpsilon (eps); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::KdTree::setInputCloud ( + const PointCloudConstPtr& cloud, + const IndicesConstPtr& indices) +{ + tree_->setInputCloud (cloud, indices); + input_ = cloud; + indices_ = indices; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::KdTree::nearestKSearch ( + const PointT &point, int k, std::vector &k_indices, + std::vector &k_sqr_distances) const +{ + return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances)); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::KdTree::radiusSearch ( + const PointT& point, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn) const +{ + return (tree_->radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn)); +} + +#define PCL_INSTANTIATE_KdTree(T) template class PCL_EXPORTS pcl::search::KdTree; + +#endif //#ifndef _PCL_SEARCH_KDTREE_IMPL_HPP_ + + diff --git a/deps_install/include/pcl-1.10/pcl/search/impl/organized.hpp b/deps_install/include/pcl-1.10/pcl/search/impl/organized.hpp new file mode 100755 index 0000000..85438a3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/impl/organized.hpp @@ -0,0 +1,391 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_ +#define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::OrganizedNeighbor::radiusSearch (const PointT &query, + const double radius, + std::vector &k_indices, + std::vector &k_sqr_distances, + unsigned int max_nn) const +{ + // NAN test + assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + // search window + unsigned left, right, top, bottom; + //unsigned x, y, idx; + float squared_distance; + double squared_radius; + + k_indices.clear (); + k_sqr_distances.clear (); + + squared_radius = radius * radius; + + this->getProjectedRadiusSearchBox (query, static_cast (squared_radius), left, right, top, bottom); + + // iterate over search box + if (max_nn == 0 || max_nn >= static_cast (input_->points.size ())) + max_nn = static_cast (input_->points.size ()); + + k_indices.reserve (max_nn); + k_sqr_distances.reserve (max_nn); + + unsigned yEnd = (bottom + 1) * input_->width + right + 1; + unsigned idx = top * input_->width + left; + unsigned skip = input_->width - right + left - 1; + unsigned xEnd = idx - left + right + 1; + + for (; xEnd != yEnd; idx += skip, xEnd += input_->width) + { + for (; idx < xEnd; ++idx) + { + if (!mask_[idx] || !isFinite (input_->points[idx])) + continue; + + float dist_x = input_->points[idx].x - query.x; + float dist_y = input_->points[idx].y - query.y; + float dist_z = input_->points[idx].z - query.z; + squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; + //squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm (); + if (squared_distance <= squared_radius) + { + k_indices.push_back (idx); + k_sqr_distances.push_back (squared_distance); + // already done ? + if (k_indices.size () == max_nn) + { + if (sorted_results_) + this->sortResults (k_indices, k_sqr_distances); + return (max_nn); + } + } + } + } + if (sorted_results_) + this->sortResults (k_indices, k_sqr_distances); + return (static_cast (k_indices.size ())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::OrganizedNeighbor::nearestKSearch (const PointT &query, + int k, + std::vector &k_indices, + std::vector &k_sqr_distances) const +{ + assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + if (k < 1) + { + k_indices.clear (); + k_sqr_distances.clear (); + return (0); + } + + Eigen::Vector3f queryvec (query.x, query.y, query.z); + // project query point on the image plane + //Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3); + Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3)); + int xBegin = int(q [0] / q [2] + 0.5f); + int yBegin = int(q [1] / q [2] + 0.5f); + int xEnd = xBegin + 1; // end is the pixel that is not used anymore, like in iterators + int yEnd = yBegin + 1; + + // the search window. This is supposed to shrink within the iterations + unsigned left = 0; + unsigned right = input_->width - 1; + unsigned top = 0; + unsigned bottom = input_->height - 1; + + std::priority_queue results; + //std::vector k_results; + //k_results.reserve (k); + // add point laying on the projection of the query point. + if (xBegin >= 0 && + xBegin < static_cast (input_->width) && + yBegin >= 0 && + yBegin < static_cast (input_->height)) + testPoint (query, k, results, yBegin * input_->width + xBegin); + else // point lys + { + // find the box that touches the image border -> don't waste time evaluating boxes that are completely outside the image! + int dist = std::numeric_limits::max (); + + if (xBegin < 0) + dist = -xBegin; + else if (xBegin >= static_cast (input_->width)) + dist = xBegin - static_cast (input_->width); + + if (yBegin < 0) + dist = std::min (dist, -yBegin); + else if (yBegin >= static_cast (input_->height)) + dist = std::min (dist, yBegin - static_cast (input_->height)); + + xBegin -= dist; + xEnd += dist; + + yBegin -= dist; + yEnd += dist; + } + + + // stop used as isChanged as well as stop. + bool stop = false; + do + { + // increment box size + --xBegin; + ++xEnd; + --yBegin; + ++yEnd; + + // the range in x-direction which intersects with the image width + int xFrom = xBegin; + int xTo = xEnd; + clipRange (xFrom, xTo, 0, input_->width); + + // if x-extend is not 0 + if (xTo > xFrom) + { + // if upper line of the rectangle is visible and x-extend is not 0 + if (yBegin >= 0 && yBegin < static_cast (input_->height)) + { + int idx = yBegin * input_->width + xFrom; + int idxTo = idx + xTo - xFrom; + for (; idx < idxTo; ++idx) + stop = testPoint (query, k, results, idx) || stop; + } + + + // the row yEnd does NOT belong to the box -> last row = yEnd - 1 + // if lower line of the rectangle is visible + if (yEnd > 0 && yEnd <= static_cast (input_->height)) + { + int idx = (yEnd - 1) * input_->width + xFrom; + int idxTo = idx + xTo - xFrom; + + for (; idx < idxTo; ++idx) + stop = testPoint (query, k, results, idx) || stop; + } + + // skip first row and last row (already handled above) + int yFrom = yBegin + 1; + int yTo = yEnd - 1; + clipRange (yFrom, yTo, 0, input_->height); + + // if we have lines in between that are also visible + if (yFrom < yTo) + { + if (xBegin >= 0 && xBegin < static_cast (input_->width)) + { + int idx = yFrom * input_->width + xBegin; + int idxTo = yTo * input_->width + xBegin; + + for (; idx < idxTo; idx += input_->width) + stop = testPoint (query, k, results, idx) || stop; + } + + if (xEnd > 0 && xEnd <= static_cast (input_->width)) + { + int idx = yFrom * input_->width + xEnd - 1; + int idxTo = yTo * input_->width + xEnd - 1; + + for (; idx < idxTo; idx += input_->width) + stop = testPoint (query, k, results, idx) || stop; + } + + } + // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse. + if (stop) + getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom); + + } + // now we use it as stop flag -> if bounding box is completely within the already examined search box were done! + stop = (static_cast (left) >= xBegin && static_cast (left) < xEnd && + static_cast (right) >= xBegin && static_cast (right) < xEnd && + static_cast (top) >= yBegin && static_cast (top) < yEnd && + static_cast (bottom) >= yBegin && static_cast (bottom) < yEnd); + + } while (!stop); + + + k_indices.resize (results.size ()); + k_sqr_distances.resize (results.size ()); + std::size_t idx = results.size () - 1; + while (!results.empty ()) + { + k_indices [idx] = results.top ().index; + k_sqr_distances [idx] = results.top ().distance; + results.pop (); + --idx; + } + + return (static_cast (k_indices.size ())); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::OrganizedNeighbor::getProjectedRadiusSearchBox (const PointT& point, + float squared_radius, + unsigned &minX, + unsigned &maxX, + unsigned &minY, + unsigned &maxY) const +{ + Eigen::Vector3f queryvec (point.x, point.y, point.z); + //Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3); + Eigen::Vector3f q (KR_ * queryvec + projection_matrix_.block <3, 1> (0, 3)); + + float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2]; + float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2]; + float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1]; + int min, max; + // a and c are multiplied by two already => - 4ac -> - ac + float det = b * b - a * c; + if (det < 0) + { + minY = 0; + maxY = input_->height - 1; + } + else + { + float y1 = static_cast ((b - std::sqrt (det)) / a); + float y2 = static_cast ((b + std::sqrt (det)) / a); + + min = std::min (static_cast (std::floor (y1)), static_cast (std::floor (y2))); + max = std::max (static_cast (std::ceil (y1)), static_cast (std::ceil (y2))); + minY = static_cast (std::min (static_cast (input_->height) - 1, std::max (0, min))); + maxY = static_cast (std::max (std::min (static_cast (input_->height) - 1, max), 0)); + } + + b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2]; + c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0]; + + det = b * b - a * c; + if (det < 0) + { + minX = 0; + maxX = input_->width - 1; + } + else + { + float x1 = static_cast ((b - std::sqrt (det)) / a); + float x2 = static_cast ((b + std::sqrt (det)) / a); + + min = std::min (static_cast (std::floor (x1)), static_cast (std::floor (x2))); + max = std::max (static_cast (std::ceil (x1)), static_cast (std::ceil (x2))); + minX = static_cast (std::min (static_cast (input_->width)- 1, std::max (0, min))); + maxX = static_cast (std::max (std::min (static_cast (input_->width) - 1, max), 0)); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::OrganizedNeighbor::computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const +{ + pcl::getCameraMatrixFromProjectionMatrix (projection_matrix_, camera_matrix); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::OrganizedNeighbor::estimateProjectionMatrix () +{ + // internally we calculate with double but store the result into float matrices. + projection_matrix_.setZero (); + if (input_->height == 1 || input_->width == 1) + { + PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ()); + return; + } + + const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1)); + const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1)); + + std::vector indices; + indices.reserve (input_->size () >> (pyramid_level_ << 1)); + + for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip) + { + for (unsigned xIdx = 0, idx2 = idx; xIdx < input_->width; xIdx += xSkip, idx2 += xSkip) + { + if (!mask_ [idx2]) + continue; + + indices.push_back (idx2); + } + } + + double residual_sqr = pcl::estimateProjectionMatrix (input_, projection_matrix_, indices); + + if (std::abs (residual_sqr) > eps_ * float (indices.size ())) + { + PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ()); + return; + } + + // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]] + // and R being the rotation matrix + KR_ = projection_matrix_.topLeftCorner <3, 3> (); + + // precalculate KR * KR^T needed by calculations during nn-search + KR_KRT_ = KR_ * KR_.transpose (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::search::OrganizedNeighbor::projectPoint (const PointT& point, pcl::PointXY& q) const +{ + Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3); + q.x = projected [0] / projected [2]; + q.y = projected [1] / projected [2]; + return (projected[2] != 0); +} +#define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/search/impl/search.hpp b/deps_install/include/pcl-1.10/pcl/search/impl/search.hpp new file mode 100755 index 0000000..dbb6880 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/impl/search.hpp @@ -0,0 +1,212 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEARCH_SEARCH_IMPL_HPP_ +#define PCL_SEARCH_SEARCH_IMPL_HPP_ + +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::search::Search::Search (const std::string& name, bool sorted) + : input_ () + , sorted_results_ (sorted) + , name_ (name) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template const std::string& +pcl::search::Search::getName () const +{ + return (name_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::Search::setSortedResults (bool sorted) +{ + sorted_results_ = sorted; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::search::Search::getSortedResults () +{ + return (sorted_results_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::Search::setInputCloud ( + const PointCloudConstPtr& cloud, const IndicesConstPtr &indices) +{ + input_ = cloud; + indices_ = indices; +} + + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::Search::nearestKSearch ( + const PointCloud &cloud, int index, int k, + std::vector &k_indices, std::vector &k_sqr_distances) const +{ + assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!"); + return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances)); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::Search::nearestKSearch ( + int index, int k, + std::vector &k_indices, + std::vector &k_sqr_distances) const +{ + if (!indices_) + { + assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!"); + return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances)); + } + assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in nearestKSearch!"); + if (index >= static_cast (indices_->size ()) || index < 0) + return (0); + return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances)); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::Search::nearestKSearch ( + const PointCloud& cloud, const std::vector& indices, + int k, std::vector< std::vector >& k_indices, + std::vector< std::vector >& k_sqr_distances) const +{ + if (indices.empty ()) + { + k_indices.resize (cloud.size ()); + k_sqr_distances.resize (cloud.size ()); + for (std::size_t i = 0; i < cloud.size (); i++) + nearestKSearch (cloud, static_cast (i), k, k_indices[i], k_sqr_distances[i]); + } + else + { + k_indices.resize (indices.size ()); + k_sqr_distances.resize (indices.size ()); + for (std::size_t i = 0; i < indices.size (); i++) + nearestKSearch (cloud, indices[i], k, k_indices[i], k_sqr_distances[i]); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::Search::radiusSearch ( + const PointCloud &cloud, int index, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn) const +{ + assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!"); + return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::search::Search::radiusSearch ( + int index, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn ) const +{ + if (!indices_) + { + assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in radiusSearch!"); + return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn)); + } + assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in radiusSearch!"); + return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn)); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::Search::radiusSearch ( + const PointCloud& cloud, + const std::vector& indices, + double radius, + std::vector< std::vector >& k_indices, + std::vector< std::vector > &k_sqr_distances, + unsigned int max_nn) const +{ + if (indices.empty ()) + { + k_indices.resize (cloud.size ()); + k_sqr_distances.resize (cloud.size ()); + for (std::size_t i = 0; i < cloud.size (); i++) + radiusSearch (cloud, static_cast (i), radius,k_indices[i], k_sqr_distances[i], max_nn); + } + else + { + k_indices.resize (indices.size ()); + k_sqr_distances.resize (indices.size ()); + for (std::size_t i = 0; i < indices.size (); i++) + radiusSearch (cloud,indices[i],radius,k_indices[i],k_sqr_distances[i], max_nn); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::search::Search::sortResults ( + std::vector& indices, std::vector& distances) const +{ + std::vector order (indices.size ()); + for (std::size_t idx = 0; idx < order.size (); ++idx) + order [idx] = static_cast (idx); + + Compare compare (distances); + sort (order.begin (), order.end (), compare); + + std::vector sorted (indices.size ()); + for (std::size_t idx = 0; idx < order.size (); ++idx) + sorted [idx] = indices[order [idx]]; + + indices = sorted; + + // sort the according distances. + sort (distances.begin (), distances.end ()); +} + +#define PCL_INSTANTIATE_Search(T) template class PCL_EXPORTS pcl::search::Search; + +#endif //#ifndef _PCL_SEARCH_SEARCH_IMPL_HPP_ + + diff --git a/deps_install/include/pcl-1.10/pcl/search/kdtree.h b/deps_install/include/pcl-1.10/pcl/search/kdtree.h new file mode 100755 index 0000000..25d4e86 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/kdtree.h @@ -0,0 +1,179 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + // Forward declarations + template class PointRepresentation; + + namespace search + { + /** \brief @b search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search + * functions using KdTree structure. KdTree is a generic type of 3D spatial locator using kD-tree structures. + * The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor) project + * by Marius Muja and David Lowe. + * + * \author Radu B. Rusu + * \ingroup search + */ + template > + class KdTree: public Search + { + public: + using PointCloud = typename Search::PointCloud; + using PointCloudConstPtr = typename Search::PointCloudConstPtr; + + using typename Search::IndicesPtr; + using typename Search::IndicesConstPtr; + + using pcl::search::Search::indices_; + using pcl::search::Search::input_; + using pcl::search::Search::getIndices; + using pcl::search::Search::getInputCloud; + using pcl::search::Search::nearestKSearch; + using pcl::search::Search::radiusSearch; + using pcl::search::Search::sorted_results_; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using KdTreePtr = typename Tree::Ptr; + using KdTreeConstPtr = typename Tree::ConstPtr; + using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; + + /** \brief Constructor for KdTree. + * + * \param[in] sorted set to true if the nearest neighbor search results + * need to be sorted in ascending order based on their distance to the + * query point + * + */ + KdTree (bool sorted = true); + + /** \brief Destructor for KdTree. */ + + ~KdTree () + { + } + + /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. + * \param[in] point_representation the const boost shared pointer to a PointRepresentation + */ + void + setPointRepresentation (const PointRepresentationConstPtr &point_representation); + + /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */ + inline PointRepresentationConstPtr + getPointRepresentation () const + { + return (tree_->getPointRepresentation ()); + } + + /** \brief Sets whether the results have to be sorted or not. + * \param[in] sorted_results set to true if the radius search results should be sorted + */ + void + setSortedResults (bool sorted_results) override; + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. + * \param[in] eps precision (error bound) for nearest neighbors searches + */ + void + setEpsilon (float eps); + + /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ + inline float + getEpsilon () const + { + return (tree_->getEpsilon ()); + } + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud + */ + void + setInputCloud (const PointCloudConstPtr& cloud, + const IndicesConstPtr& indices = IndicesConstPtr ()) override; + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + int + nearestKSearch (const PointT &point, int k, + std::vector &k_indices, + std::vector &k_sqr_distances) const override; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + int + radiusSearch (const PointT& point, double radius, + std::vector &k_indices, + std::vector &k_sqr_distances, + unsigned int max_nn = 0) const override; + protected: + /** \brief A pointer to the internal KdTree object. */ + KdTreePtr tree_; + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#else +#define PCL_INSTANTIATE_KdTree(T) template class PCL_EXPORTS pcl::search::KdTree; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/search/octree.h b/deps_install/include/pcl-1.10/pcl/search/octree.h new file mode 100755 index 0000000..0b0a312 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/octree.h @@ -0,0 +1,290 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace search + { + /** \brief @b search::Octree is a wrapper class which implements nearest neighbor search operations based on the + * pcl::octree::Octree structure. + * + * The octree pointcloud class needs to be initialized with its voxel + * resolution. Its bounding box is automatically adjusted according to the + * pointcloud dimension or it can be predefined. Note: The tree depth + * equates to the resolution and the bounding box dimensions of the + * octree. + * + * \note typename: PointT: type of point used in pointcloud + * \note typename: LeafT: leaf node class (usuallt templated with integer indices values) + * \note typename: OctreeT: octree implementation () + * + * \author Julius Kammerl + * \ingroup search + */ + template > + class Octree: public Search + { + public: + // public typedefs + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using typename Search::IndicesPtr; + using typename Search::IndicesConstPtr; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + // Boost shared pointers + using OctreePointCloudSearchPtr = typename pcl::octree::OctreePointCloudSearch::Ptr; + using OctreePointCloudSearchConstPtr = typename pcl::octree::OctreePointCloudSearch::ConstPtr; + OctreePointCloudSearchPtr tree_; + + using pcl::search::Search::input_; + using pcl::search::Search::indices_; + using pcl::search::Search::sorted_results_; + + /** \brief Octree constructor. + * \param[in] resolution octree resolution at lowest octree level + */ + Octree (const double resolution) + : Search ("Octree") + , tree_ (new pcl::octree::OctreePointCloudSearch (resolution)) + { + } + + /** \brief Empty Destructor. */ + + ~Octree () + { + } + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const boost shared pointer to a PointCloud message + */ + inline void + setInputCloud (const PointCloudConstPtr &cloud) + { + tree_->deleteTree (); + tree_->setInputCloud (cloud); + tree_->addPointsFromInputCloud (); + input_ = cloud; + } + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud + */ + inline void + setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr& indices) override + { + tree_->deleteTree (); + tree_->setInputCloud (cloud, indices); + tree_->addPointsFromInputCloud (); + input_ = cloud; + indices_ = indices; + } + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] cloud the point cloud data + * \param[in] index the index in \a cloud representing the query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + inline int + nearestKSearch (const PointCloud &cloud, int index, int k, std::vector &k_indices, + std::vector &k_sqr_distances) const override + { + return (tree_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances)); + } + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + inline int + nearestKSearch (const PointT &point, int k, std::vector &k_indices, + std::vector &k_sqr_distances) const override + { + return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances)); + } + + /** \brief Search for the k-nearest neighbors for the given query point (zero-copy). + * + * \param[in] index the index representing the query point in the + * dataset given by \a setInputCloud if indices were given in + * setInputCloud, index will be the position in the indices vector + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + inline int + nearestKSearch (int index, int k, std::vector &k_indices, std::vector &k_sqr_distances) const override + { + return (tree_->nearestKSearch (index, k, k_indices, k_sqr_distances)); + } + + /** \brief search for all neighbors of query point that are within a given radius. + * \param cloud the point cloud data + * \param index the index in \a cloud representing the query point + * \param radius the radius of the sphere bounding all of p_q's neighbors + * \param k_indices the resultant indices of the neighboring points + * \param k_sqr_distances the resultant squared distances to the neighboring points + * \param max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + inline int + radiusSearch (const PointCloud &cloud, + int index, + double radius, + std::vector &k_indices, + std::vector &k_sqr_distances, + unsigned int max_nn = 0) const override + { + tree_->radiusSearch (cloud, index, radius, k_indices, k_sqr_distances, max_nn); + if (sorted_results_) + this->sortResults (k_indices, k_sqr_distances); + return (static_cast (k_indices.size ())); + } + + /** \brief search for all neighbors of query point that are within a given radius. + * \param p_q the given query point + * \param radius the radius of the sphere bounding all of p_q's neighbors + * \param k_indices the resultant indices of the neighboring points + * \param k_sqr_distances the resultant squared distances to the neighboring points + * \param max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + inline int + radiusSearch (const PointT &p_q, + double radius, + std::vector &k_indices, + std::vector &k_sqr_distances, + unsigned int max_nn = 0) const override + { + tree_->radiusSearch (p_q, radius, k_indices, k_sqr_distances, max_nn); + if (sorted_results_) + this->sortResults (k_indices, k_sqr_distances); + return (static_cast (k_indices.size ())); + } + + /** \brief search for all neighbors of query point that are within a given radius. + * \param index index representing the query point in the dataset given by \a setInputCloud. + * If indices were given in setInputCloud, index will be the position in the indices vector + * \param radius radius of the sphere bounding all of p_q's neighbors + * \param k_indices the resultant indices of the neighboring points + * \param k_sqr_distances the resultant squared distances to the neighboring points + * \param max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + inline int + radiusSearch (int index, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) const override + { + tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, max_nn); + if (sorted_results_) + this->sortResults (k_indices, k_sqr_distances); + return (static_cast (k_indices.size ())); + } + + + /** \brief Search for approximate nearest neighbor at the query point. + * \param[in] cloud the point cloud data + * \param[in] query_index the index in \a cloud representing the query point + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + * \return number of neighbors found + */ + inline void + approxNearestSearch (const PointCloudConstPtr &cloud, int query_index, int &result_index, + float &sqr_distance) + { + return (tree_->approxNearestSearch (cloud->points[query_index], result_index, sqr_distance)); + } + + /** \brief Search for approximate nearest neighbor at the query point. + * \param[in] p_q the given query point + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + */ + inline void + approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance) + { + return (tree_->approxNearestSearch (p_q, result_index, sqr_distance)); + } + + /** \brief Search for approximate nearest neighbor at the query point. + * \param query_index index representing the query point in the dataset given by \a setInputCloud. + * If indices were given in setInputCloud, index will be the position in the indices vector. + * \param result_index the resultant index of the neighbor point + * \param sqr_distance the resultant squared distance to the neighboring point + * \return number of neighbors found + */ + inline void + approxNearestSearch (int query_index, int &result_index, float &sqr_distance) + { + return (tree_->approxNearestSearch (query_index, result_index, sqr_distance)); + } + + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#else +#define PCL_INSTANTIATE_Octree(T) template class PCL_EXPORTS pcl::search::Octree; +#endif diff --git a/deps_install/include/pcl-1.10/pcl/search/organized.h b/deps_install/include/pcl-1.10/pcl/search/organized.h new file mode 100755 index 0000000..d819a1a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/organized.h @@ -0,0 +1,285 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace pcl +{ + namespace search + { + /** \brief OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds. + * \author Radu B. Rusu, Julius Kammerl, Suat Gedikli, Koen Buys + * \ingroup search + */ + template + class OrganizedNeighbor : public pcl::search::Search + { + + public: + // public typedefs + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using typename Search::IndicesConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::search::Search::indices_; + using pcl::search::Search::sorted_results_; + using pcl::search::Search::input_; + + /** \brief Constructor + * \param[in] sorted_results whether the results should be return sorted in ascending order on the distances or not. + * This applies only for radius search, since knn always returns sorted resutls + * \param[in] eps the threshold for the mean-squared-error of the estimation of the projection matrix. + * if the MSE is above this value, the point cloud is considered as not from a projective device, + * thus organized neighbor search can not be applied on that cloud. + * \param[in] pyramid_level the level of the down sampled point cloud to be used for projection matrix estimation + */ + OrganizedNeighbor (bool sorted_results = false, float eps = 1e-4f, unsigned pyramid_level = 5) + : Search ("OrganizedNeighbor", sorted_results) + , projection_matrix_ (Eigen::Matrix::Zero ()) + , KR_ (Eigen::Matrix::Zero ()) + , KR_KRT_ (Eigen::Matrix::Zero ()) + , eps_ (eps) + , pyramid_level_ (pyramid_level) + { + } + + /** \brief Empty deconstructor. */ + ~OrganizedNeighbor () {} + + /** \brief Test whether this search-object is valid (input is organized AND from projective device) + * User should use this method after setting the input cloud, since setInput just prints an error + * if input is not organized or a projection matrix could not be determined. + * \return true if the input data is organized and from a projective device, false otherwise + */ + bool + isValid () const + { + // determinant (KR) = determinant (K) * determinant (R) = determinant (K) = f_x * f_y. + // If we expect at max an opening angle of 170degree in x-direction -> f_x = 2.0 * width / tan (85 degree); + // 2 * tan (85 degree) ~ 22.86 + float min_f = 0.043744332f * static_cast(input_->width); + //std::cout << "isValid: " << determinant3x3Matrix (KR_ / sqrt (KR_KRT_.coeff (8))) << " >= " << (min_f * min_f) << std::endl; + return (determinant3x3Matrix (KR_ / std::sqrt (KR_KRT_.coeff (8))) >= (min_f * min_f)); + } + + /** \brief Compute the camera matrix + * \param[out] camera_matrix the resultant computed camera matrix + */ + void + computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const; + + /** \brief Provide a pointer to the input data set, if user has focal length he must set it before calling this + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param[in] indices the const boost shared pointer to PointIndices + */ + void + setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) override + { + input_ = cloud; + + mask_.resize (input_->size ()); + input_ = cloud; + indices_ = indices; + + if (indices_ && !indices_->empty()) + { + mask_.assign (input_->size (), 0); + for (std::vector::const_iterator iIt = indices_->begin (); iIt != indices_->end (); ++iIt) + mask_[*iIt] = 1; + } + else + mask_.assign (input_->size (), 1); + + estimateProjectionMatrix (); + } + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] p_q the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + int + radiusSearch (const PointT &p_q, + double radius, + std::vector &k_indices, + std::vector &k_sqr_distances, + unsigned int max_nn = 0) const override; + + /** \brief estimated the projection matrix from the input cloud. */ + void + estimateProjectionMatrix (); + + /** \brief Search for the k-nearest neighbors for a given query point. + * \note limiting the maximum search radius (with setMaxDistance) can lead to a significant improvement in search speed + * \param[in] p_q the given query point (\ref setInputCloud must be given a-priori!) + * \param[in] k the number of neighbors to search for (used only if horizontal and vertical window not given already!) + * \param[out] k_indices the resultant point indices (must be resized to \a k beforehand!) + * \param[out] k_sqr_distances \note this function does not return distances + * \return number of neighbors found + * @todo still need to implements this functionality + */ + int + nearestKSearch (const PointT &p_q, + int k, + std::vector &k_indices, + std::vector &k_sqr_distances) const override; + + /** \brief projects a point into the image + * \param[in] p point in 3D World Coordinate Frame to be projected onto the image plane + * \param[out] q the 2D projected point in pixel coordinates (u,v) + * @return true if projection is valid, false otherwise + */ + bool projectPoint (const PointT& p, pcl::PointXY& q) const; + + protected: + + struct Entry + { + Entry (int idx, float dist) : index (idx), distance (dist) {} + Entry () : index (0), distance (0) {} + unsigned index; + float distance; + + inline bool + operator < (const Entry& other) const + { + return (distance < other.distance); + } + }; + + /** \brief test if point given by index is among the k NN in results to the query point. + * \param[in] query query point + * \param[in] k number of maximum nn interested in + * \param[in,out] queue priority queue with k NN + * \param[in] index index on point to be tested + * \return whether the top element changed or not. + */ + inline bool + testPoint (const PointT& query, unsigned k, std::priority_queue& queue, unsigned index) const + { + const PointT& point = input_->points [index]; + if (mask_ [index] && std::isfinite (point.x)) + { + //float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm (); + float dist_x = point.x - query.x; + float dist_y = point.y - query.y; + float dist_z = point.z - query.z; + float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; + if (queue.size () < k) + { + queue.push (Entry (index, squared_distance)); + return queue.size () == k; + } + if (queue.top ().distance > squared_distance) + { + queue.pop (); + queue.push (Entry (index, squared_distance)); + return true; // top element has changed! + } + } + return false; + } + + inline void + clipRange (int& begin, int &end, int min, int max) const + { + begin = std::max (std::min (begin, max), min); + end = std::min (std::max (end, min), max); + } + + /** \brief Obtain a search box in 2D from a sphere with a radius in 3D + * \param[in] point the query point (sphere center) + * \param[in] squared_radius the squared sphere radius + * \param[out] minX the min X box coordinate + * \param[out] minY the min Y box coordinate + * \param[out] maxX the max X box coordinate + * \param[out] maxY the max Y box coordinate + */ + void + getProjectedRadiusSearchBox (const PointT& point, float squared_radius, unsigned& minX, unsigned& minY, + unsigned& maxX, unsigned& maxY) const; + + + /** \brief the projection matrix. Either set by user or calculated by the first / each input cloud */ + Eigen::Matrix projection_matrix_; + + /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/ + Eigen::Matrix KR_; + + /** \brief inveser of the left 3x3 projection matrix which is K * R (with K being the camera matrix and R the rotation matrix)*/ + Eigen::Matrix KR_KRT_; + + /** \brief epsilon value for the MSE of the projection matrix estimation*/ + const float eps_; + + /** \brief using only a subsample of points to calculate the projection matrix. pyramid_level_ = use down sampled cloud given by pyramid_level_*/ + const unsigned pyramid_level_; + + /** \brief mask, indicating whether the point was in the indices list or not.*/ + std::vector mask_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/search/pcl_search.h b/deps_install/include/pcl-1.10/pcl/search/pcl_search.h new file mode 100755 index 0000000..42bcb10 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/pcl_search.h @@ -0,0 +1,45 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/search/search.h b/deps_install/include/pcl-1.10/pcl/search/search.h new file mode 100755 index 0000000..f90c64a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/search/search.h @@ -0,0 +1,429 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace search + { + /** \brief Generic search class. All search wrappers must inherit from this. + * + * Each search method must implement 2 different types of search: + * - \b nearestKSearch - search for K-nearest neighbors. + * - \b radiusSearch - search for all nearest neighbors in a sphere of a given radius + * + * The input to each search method can be given in 3 different ways: + * - as a query point + * - as a (cloud, index) pair + * - as an index + * + * For the latter option, it is assumed that the user specified the input + * via a \ref setInputCloud () method first. + * + * \note In case of an error, all methods are supposed to return 0 as the number of neighbors found. + * + * \note libpcl_search deals with three-dimensional search problems. For higher + * level dimensional search, please refer to the libpcl_kdtree module. + * + * \author Radu B. Rusu + * \ingroup search + */ + template + class Search + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using IndicesPtr = shared_ptr >; + using IndicesConstPtr = shared_ptr >; + + /** Constructor. */ + Search (const std::string& name = "", bool sorted = false); + + /** Destructor. */ + virtual + ~Search () + { + } + + /** \brief Returns the search method name + */ + virtual const std::string& + getName () const; + + /** \brief sets whether the results should be sorted (ascending in the distance) or not + * \param[in] sorted should be true if the results should be sorted by the distance in ascending order. + * Otherwise the results may be returned in any order. + */ + virtual void + setSortedResults (bool sorted); + + /** \brief Gets whether the results should be sorted (ascending in the distance) or not + * Otherwise the results may be returned in any order. + */ + virtual bool + getSortedResults (); + + + /** \brief Pass the input dataset that the search will be performed on. + * \param[in] cloud a const pointer to the PointCloud data + * \param[in] indices the point indices subset that is to be used from the cloud + */ + virtual void + setInputCloud (const PointCloudConstPtr& cloud, + const IndicesConstPtr &indices = IndicesConstPtr ()); + + /** \brief Get a pointer to the input point cloud dataset. */ + virtual PointCloudConstPtr + getInputCloud () const + { + return (input_); + } + + /** \brief Get a pointer to the vector of indices used. */ + virtual IndicesConstPtr + getIndices () const + { + return (indices_); + } + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + virtual int + nearestKSearch (const PointT &point, int k, std::vector &k_indices, + std::vector &k_sqr_distances) const = 0; + + /** \brief Search for k-nearest neighbors for the given query point. + * This method accepts a different template parameter for the point type. + * \param[in] point the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + */ + template inline int + nearestKSearchT (const PointTDiff &point, int k, + std::vector &k_indices, std::vector &k_sqr_distances) const + { + PointT p; + copyPoint (point, p); + return (nearestKSearch (p, k, k_indices, k_sqr_distances)); + } + + /** \brief Search for k-nearest neighbors for the given query point. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] cloud the point cloud data + * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * + * \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + nearestKSearch (const PointCloud &cloud, int index, int k, + std::vector &k_indices, + std::vector &k_sqr_distances) const; + + /** \brief Search for k-nearest neighbors for the given query point (zero-copy). + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] index a \a valid index representing a \a valid query point in the dataset given + * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in + * the indices vector. + * + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k + * a priori!) + * \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + nearestKSearch (int index, int k, + std::vector &k_indices, + std::vector &k_sqr_distances) const; + + /** \brief Search for the k-nearest neighbors for the given query point. + * \param[in] cloud the point cloud data + * \param[in] indices a vector of point cloud indices to query for nearest neighbors + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i + */ + virtual void + nearestKSearch (const PointCloud& cloud, const std::vector& indices, + int k, std::vector< std::vector >& k_indices, + std::vector< std::vector >& k_sqr_distances) const; + + /** \brief Search for the k-nearest neighbors for the given query point. Use this method if the query points are of a different type than the points in the data set (e.g. PointXYZRGBA instead of PointXYZ). + * \param[in] cloud the point cloud data + * \param[in] indices a vector of point cloud indices to query for nearest neighbors + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i + * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search. + */ + template void + nearestKSearchT (const pcl::PointCloud &cloud, const std::vector& indices, int k, std::vector< std::vector > &k_indices, + std::vector< std::vector > &k_sqr_distances) const + { + // Copy all the data fields from the input cloud to the output one + using FieldListInT = typename pcl::traits::fieldList::type; + using FieldListOutT = typename pcl::traits::fieldList::type; + using FieldList = typename pcl::intersect::type; + + pcl::PointCloud pc; + if (indices.empty ()) + { + pc.resize (cloud.size()); + for (std::size_t i = 0; i < cloud.size(); i++) + { + pcl::for_each_type (pcl::NdConcatenateFunctor ( + cloud[i], pc[i])); + } + nearestKSearch (pc,std::vector(),k,k_indices,k_sqr_distances); + } + else + { + pc.resize (indices.size()); + for (std::size_t i = 0; i < indices.size(); i++) + { + pcl::for_each_type (pcl::NdConcatenateFunctor ( + cloud[indices[i]], pc[i])); + } + nearestKSearch (pc,std::vector(),k,k_indices,k_sqr_distances); + } + } + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + virtual int + radiusSearch (const PointT& point, double radius, std::vector& k_indices, + std::vector& k_sqr_distances, unsigned int max_nn = 0) const = 0; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] point the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + template inline int + radiusSearchT (const PointTDiff &point, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) const + { + PointT p; + copyPoint (point, p); + return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn)); + } + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] cloud the point cloud data + * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + radiusSearch (const PointCloud &cloud, int index, double radius, + std::vector &k_indices, std::vector &k_sqr_distances, + unsigned int max_nn = 0) const; + + /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy). + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] index a \a valid index representing a \a valid query point in the dataset given + * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in + * the indices vector. + * + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum number of points + */ + virtual int + radiusSearch (int index, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) const; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * \param[in] cloud the point cloud data + * \param[in] indices the indices in \a cloud. If indices is empty, neighbors will be searched for all points. + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + */ + virtual void + radiusSearch (const PointCloud& cloud, + const std::vector& indices, + double radius, + std::vector< std::vector >& k_indices, + std::vector< std::vector > &k_sqr_distances, + unsigned int max_nn = 0) const; + + /** \brief Search for all the nearest neighbors of the query points in a given radius. + * \param[in] cloud the point cloud data + * \param[in] indices a vector of point cloud indices to query for nearest neighbors + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search. + */ + template void + radiusSearchT (const pcl::PointCloud &cloud, + const std::vector& indices, + double radius, + std::vector< std::vector > &k_indices, + std::vector< std::vector > &k_sqr_distances, + unsigned int max_nn = 0) const + { + // Copy all the data fields from the input cloud to the output one + using FieldListInT = typename pcl::traits::fieldList::type; + using FieldListOutT = typename pcl::traits::fieldList::type; + using FieldList = typename pcl::intersect::type; + + pcl::PointCloud pc; + if (indices.empty ()) + { + pc.resize (cloud.size ()); + for (std::size_t i = 0; i < cloud.size (); ++i) + pcl::for_each_type (pcl::NdConcatenateFunctor (cloud[i], pc[i])); + radiusSearch (pc, std::vector (), radius, k_indices, k_sqr_distances, max_nn); + } + else + { + pc.resize (indices.size ()); + for (std::size_t i = 0; i < indices.size (); ++i) + pcl::for_each_type (pcl::NdConcatenateFunctor (cloud[indices[i]], pc[i])); + radiusSearch (pc, std::vector(), radius, k_indices, k_sqr_distances, max_nn); + } + } + + protected: + void + sortResults (std::vector& indices, std::vector& distances) const; + + PointCloudConstPtr input_; + IndicesConstPtr indices_; + bool sorted_results_; + std::string name_; + + private: + struct Compare + { + Compare (const std::vector& distances) + : distances_ (distances) + { + } + + bool + operator () (int first, int second) const + { + return (distances_ [first] < distances_[second]); + } + + const std::vector& distances_; + }; + }; // class Search + } // namespace search +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/approximate_progressive_morphological_filter.h b/deps_install/include/pcl-1.10/pcl/segmentation/approximate_progressive_morphological_filter.h new file mode 100755 index 0000000..401175e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/approximate_progressive_morphological_filter.h @@ -0,0 +1,174 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * Implements the Progressive Morphological Filter for segmentation of ground points. + * Description can be found in the article + * "A Progressive Morphological Filter for Removing Nonground Measurements from + * Airborne LIDAR Data" + * by K. Zhang, S. Chen, D. Whitman, M. Shyu, J. Yan, and C. Zhang. + */ + template + class PCL_EXPORTS ApproximateProgressiveMorphologicalFilter : public pcl::PCLBase + { + public: + + using PointCloud = pcl::PointCloud; + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + public: + + /** \brief Constructor that sets default values for member variables. */ + ApproximateProgressiveMorphologicalFilter (); + + + ~ApproximateProgressiveMorphologicalFilter (); + + /** \brief Get the maximum window size to be used in filtering ground returns. */ + inline int + getMaxWindowSize () const { return (max_window_size_); } + + /** \brief Set the maximum window size to be used in filtering ground returns. */ + inline void + setMaxWindowSize (int max_window_size) { max_window_size_ = max_window_size; } + + /** \brief Get the slope value to be used in computing the height threshold. */ + inline float + getSlope () const { return (slope_); } + + /** \brief Set the slope value to be used in computing the height threshold. */ + inline void + setSlope (float slope) { slope_ = slope; } + + /** \brief Get the maximum height above the parameterized ground surface to be considered a ground return. */ + inline float + getMaxDistance () const { return (max_distance_); } + + /** \brief Set the maximum height above the parameterized ground surface to be considered a ground return. */ + inline void + setMaxDistance (float max_distance) { max_distance_ = max_distance; } + + /** \brief Get the initial height above the parameterized ground surface to be considered a ground return. */ + inline float + getInitialDistance () const { return (initial_distance_); } + + /** \brief Set the initial height above the parameterized ground surface to be considered a ground return. */ + inline void + setInitialDistance (float initial_distance) { initial_distance_ = initial_distance; } + + /** \brief Get the cell size. */ + inline float + getCellSize () const { return (cell_size_); } + + /** \brief Set the cell size. */ + inline void + setCellSize (float cell_size) { cell_size_ = cell_size; } + + /** \brief Get the base to be used in computing progressive window sizes. */ + inline float + getBase () const { return (base_); } + + /** \brief Set the base to be used in computing progressive window sizes. */ + inline void + setBase (float base) { base_ = base; } + + /** \brief Get flag indicating whether or not to exponentially grow window sizes? */ + inline bool + getExponential () const { return (exponential_); } + + /** \brief Set flag indicating whether or not to exponentially grow window sizes? */ + inline void + setExponential (bool exponential) { exponential_ = exponential; } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /** \brief This method launches the segmentation algorithm and returns indices of + * points determined to be ground returns. + * \param[out] ground indices of points determined to be ground returns. + */ + virtual void + extract (std::vector& ground); + + protected: + + /** \brief Maximum window size to be used in filtering ground returns. */ + int max_window_size_; + + /** \brief Slope value to be used in computing the height threshold. */ + float slope_; + + /** \brief Maximum height above the parameterized ground surface to be considered a ground return. */ + float max_distance_; + + /** \brief Initial height above the parameterized ground surface to be considered a ground return. */ + float initial_distance_; + + /** \brief Cell size. */ + float cell_size_; + + /** \brief Base to be used in computing progressive window sizes. */ + float base_; + + /** \brief Exponentially grow window sizes? */ + bool exponential_; + + /** \brief Number of threads to be used. */ + unsigned int threads_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/boost.h b/deps_install/include/pcl-1.10/pcl/segmentation/boost.h new file mode 100755 index 0000000..19e7508 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/boost.h @@ -0,0 +1,56 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: io.h 5850 2012-06-06 14:04:59Z stfox88 $ + * + */ + +#pragma once + +#ifdef __GNUC__ +#pragma GCC system_header +#endif + +#ifndef Q_MOC_RUN +// Marking all Boost headers as system headers to remove warnings +#include +#include +#include +#include +#include + +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/comparator.h new file mode 100755 index 0000000..122745a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/comparator.h @@ -0,0 +1,103 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief Comparator is the base class for comparators that compare two points given some function. + * Currently intended for use with OrganizedConnectedComponentSegmentation + * + * \author Alex Trevor + */ + template + class Comparator + { + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor for comparator. */ + Comparator () : input_ () + { + } + + /** \brief Empty destructor for comparator. */ + virtual + ~Comparator () + { + } + + /** \brief Set the input cloud for the comparator. + * \param[in] cloud the point cloud this comparator will operate on + */ + virtual void + setInputCloud (const PointCloudConstPtr& cloud) + { + input_ = cloud; + } + + /** \brief Get the input cloud this comparator operates on. */ + virtual PointCloudConstPtr + getInputCloud () const + { + return (input_); + } + + /** \brief Compares the two points in the input cloud designated by these two indices. + * This is pure virtual and must be implemented by subclasses with some comparison function. + * \param[in] idx1 the index of the first point. + * \param[in] idx2 the index of the second point. + */ + virtual bool + compare (int idx1, int idx2) const = 0; + + protected: + PointCloudConstPtr input_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/conditional_euclidean_clustering.h b/deps_install/include/pcl-1.10/pcl/segmentation/conditional_euclidean_clustering.h new file mode 100755 index 0000000..387f123 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/conditional_euclidean_clustering.h @@ -0,0 +1,268 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +#include + +namespace pcl +{ + using IndicesClusters = std::vector; + using IndicesClustersPtr = shared_ptr >; + + /** \brief @b ConditionalEuclideanClustering performs segmentation based on Euclidean distance and a user-defined clustering condition. + * \details The condition that need to hold is currently passed using a function pointer. + * For more information check the documentation of setConditionFunction() or the usage example below: + * \code + * bool + * enforceIntensitySimilarity (const pcl::PointXYZI& point_a, const pcl::PointXYZI& point_b, float squared_distance) + * { + * if (std::abs (point_a.intensity - point_b.intensity) < 0.1f) + * return (true); + * else + * return (false); + * } + * // ... + * // Somewhere down to the main code + * // ... + * pcl::ConditionalEuclideanClustering cec (true); + * cec.setInputCloud (cloud_in); + * cec.setConditionFunction (&enforceIntensitySimilarity); + * // Points within this distance from one another are going to need to validate the enforceIntensitySimilarity function to be part of the same cluster: + * cec.setClusterTolerance (0.09f); + * // Size constraints for the clusters: + * cec.setMinClusterSize (5); + * cec.setMaxClusterSize (30); + * // The resulting clusters (an array of pointindices): + * cec.segment (*clusters); + * // The clusters that are too small or too large in size can also be extracted separately: + * cec.getRemovedClusters (small_clusters, large_clusters); + * \endcode + * \author Frits Florentinus + * \ingroup segmentation + */ + template + class ConditionalEuclideanClustering : public PCLBase + { + protected: + using SearcherPtr = typename pcl::search::Search::Ptr; + + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + /** \brief Constructor. + * \param[in] extract_removed_clusters Set to true if you want to be able to extract the clusters that are too large or too small (default = false) + */ + ConditionalEuclideanClustering (bool extract_removed_clusters = false) : + searcher_ (), + condition_function_ (), + cluster_tolerance_ (0.0f), + min_cluster_size_ (1), + max_cluster_size_ (std::numeric_limits::max ()), + extract_removed_clusters_ (extract_removed_clusters), + small_clusters_ (new pcl::IndicesClusters), + large_clusters_ (new pcl::IndicesClusters) + { + } + + /** \brief Provide a pointer to the search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const SearcherPtr &tree) + { + searcher_ = tree; + } + + /** \brief Get a pointer to the search method used. + */ + inline const SearcherPtr& + getSearchMethod () const + { + return searcher_; + } + + /** \brief Set the condition that needs to hold for neighboring points to be considered part of the same cluster. + * \details Any two points within a certain distance from one another will need to evaluate this condition in order to be made part of the same cluster. + * The distance can be set using setClusterTolerance(). + *
+ * Note that for a point to be part of a cluster, the condition only needs to hold for at least 1 point pair. + * To clarify, the following statement is false: + * Any two points within a cluster always evaluate this condition function to true. + *

+ * The input arguments of the condition function are: + *
    + *
  • PointT The first point of the point pair
    • + *
  • PointT The second point of the point pair
    • + *
  • float The squared distance between the points
    • + *
+ * The output argument is a boolean, returning true will merge the second point into the cluster of the first point. + * \param[in] condition_function The condition function that needs to hold for clustering + */ + inline void + setConditionFunction (bool (*condition_function) (const PointT&, const PointT&, float)) + { + condition_function_ = condition_function; + } + + /** \brief Set the condition that needs to hold for neighboring points to be considered part of the same cluster. + * This is an overloaded function provided for convenience. See the documentation for setConditionFunction(). */ + inline void + setConditionFunction (std::function condition_function) + { + condition_function_ = condition_function; + } + + /** \brief Set the spatial tolerance for new cluster candidates. + * \details Any two points within this distance from one another will need to evaluate a certain condition in order to be made part of the same cluster. + * The condition can be set using setConditionFunction(). + * \param[in] cluster_tolerance The distance to scan for cluster candidates (default = 0.0) + */ + inline void + setClusterTolerance (float cluster_tolerance) + { + cluster_tolerance_ = cluster_tolerance; + } + + /** \brief Get the spatial tolerance for new cluster candidates.*/ + inline float + getClusterTolerance () + { + return (cluster_tolerance_); + } + + /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. + * \param[in] min_cluster_size The minimum cluster size (default = 1) + */ + inline void + setMinClusterSize (int min_cluster_size) + { + min_cluster_size_ = min_cluster_size; + } + + /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid.*/ + inline int + getMinClusterSize () + { + return (min_cluster_size_); + } + + /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. + * \param[in] max_cluster_size The maximum cluster size (default = unlimited) + */ + inline void + setMaxClusterSize (int max_cluster_size) + { + max_cluster_size_ = max_cluster_size; + } + + /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid.*/ + inline int + getMaxClusterSize () + { + return (max_cluster_size_); + } + + /** \brief Segment the input into separate clusters. + * \details The input can be set using setInputCloud() and setIndices(). + *
+ * The size constraints for the resulting clusters can be set using setMinClusterSize() and setMaxClusterSize(). + *
+ * The region growing parameters can be set using setConditionFunction() and setClusterTolerance(). + *
+ * \param[out] clusters The resultant set of indices, indexing the points of the input cloud that correspond to the clusters + */ + void + segment (IndicesClusters &clusters); + + /** \brief Get the clusters that are invalidated due to size constraints. + * \note The constructor of this class needs to be initialized with true, and the segment method needs to have been called prior to using this method. + * \param[out] small_clusters The resultant clusters that contain less than min_cluster_size points + * \param[out] large_clusters The resultant clusters that contain more than max_cluster_size points + */ + inline void + getRemovedClusters (IndicesClustersPtr &small_clusters, IndicesClustersPtr &large_clusters) + { + if (!extract_removed_clusters_) + { + PCL_WARN("[pcl::ConditionalEuclideanClustering::getRemovedClusters] You need to set extract_removed_clusters to true (in this class' constructor) if you want to use this functionality.\n"); + return; + } + small_clusters = small_clusters_; + large_clusters = large_clusters_; + } + + private: + /** \brief A pointer to the spatial search object */ + SearcherPtr searcher_; + + /** \brief The condition function that needs to hold for clustering */ + std::function condition_function_; + + /** \brief The distance to scan for cluster candidates (default = 0.0) */ + float cluster_tolerance_; + + /** \brief The minimum cluster size (default = 1) */ + int min_cluster_size_; + + /** \brief The maximum cluster size (default = unlimited) */ + int max_cluster_size_; + + /** \brief Set to true if you want to be able to extract the clusters that are too large or too small (default = false) */ + bool extract_removed_clusters_; + + /** \brief The resultant clusters that contain less than min_cluster_size points */ + pcl::IndicesClustersPtr small_clusters_; + + /** \brief The resultant clusters that contain more than max_cluster_size points */ + pcl::IndicesClustersPtr large_clusters_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/cpc_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/cpc_segmentation.h new file mode 100755 index 0000000..e7f76ab --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/cpc_segmentation.h @@ -0,0 +1,285 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +// common includes +#include +#include +#include + +// segmentation and sample consensus includes +#include +#include +#include + +#include +#include + +#define PCL_INSTANTIATE_CPCSegmentation(T) template class PCL_EXPORTS pcl::CPCSegmentation; + +namespace pcl +{ + /** \brief A segmentation algorithm partitioning a supervoxel graph. It uses planar cuts induced by local concavities for the recursive segmentation. Cuts are estimated using locally constrained directed RANSAC. + * \note If you use this in a scientific work please cite the following paper: + * M. Schoeler, J. Papon, F. Woergoetter + * Constrained Planar Cuts - Object Partitioning for Point Clouds + * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2015 + * Inherits most of its functionality from \ref LCCPSegmentation + * \author Markus Schoeler (mschoeler@web.de) + * \ingroup segmentation + */ + template + class CPCSegmentation : public LCCPSegmentation + { + using WeightSACPointType = PointXYZINormal; + using LCCP = LCCPSegmentation; + // LCCP typedefs + using EdgeID = typename LCCP::EdgeID; + using EdgeIterator = typename LCCP::EdgeIterator; + // LCCP methods + using LCCP::calculateConvexConnections; + using LCCP::applyKconvexity; + using LCCP::doGrouping; + using LCCP::mergeSmallSegments; + // LCCP variables + using LCCP::sv_adjacency_list_; + using LCCP::k_factor_; + using LCCP::grouping_data_valid_; + using LCCP::sv_label_to_seg_label_map_; + using LCCP::sv_label_to_supervoxel_map_; + using LCCP::concavity_tolerance_threshold_; + using LCCP::seed_resolution_; + using LCCP::supervoxels_set_; + + public: + CPCSegmentation (); + + ~CPCSegmentation (); + + /** \brief Merge supervoxels using cuts through local convexities. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method. + * \note There are three ways to retrieve the segmentation afterwards (inherited from \ref LCCPSegmentation): \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/ + void + segment (); + + /** \brief Determines if we want to use cutting planes + * \param[in] max_cuts Maximum number of cuts + * \param[in] cutting_min_segments Minimum segment size for cutting + * \param[in] cutting_min_score Minimum score a proposed cut has to achieve for being performed + * \param[in] locally_constrained Decide if we constrain our cuts locally + * \param[in] directed_cutting Decide if we prefer cuts perpendicular to the edge-direction + * \param[in] clean_cutting Decide if we cut only edges with supervoxels on opposite sides of the plane (clean) or all edges within the seed_resolution_ distance to the plane (not clean). The later was used in the paper. + */ + inline void + setCutting (const std::uint32_t max_cuts = 20, + const std::uint32_t cutting_min_segments = 0, + const float cutting_min_score = 0.16, + const bool locally_constrained = true, + const bool directed_cutting = true, + const bool clean_cutting = false) + { + max_cuts_ = max_cuts; + min_segment_size_for_cutting_ = cutting_min_segments; + min_cut_score_ = cutting_min_score; + use_local_constrains_ = locally_constrained; + use_directed_weights_ = directed_cutting; + use_clean_cutting_ = clean_cutting; + } + + /** \brief Set the number of iterations for the weighted RANSAC step (best cut estimations) + * \param[in] ransac_iterations The number of iterations */ + inline void + setRANSACIterations (const std::uint32_t ransac_iterations) + { + ransac_itrs_ = ransac_iterations; + } + + private: + + /** \brief Used in for CPC to find and fit cutting planes to the pointcloud. + * \note Is used recursively + * \param[in] depth_levels_left When first calling the function set this parameter to the maximum levels you want to cut down */ + void + applyCuttingPlane (std::uint32_t depth_levels_left); + + /// *** Parameters *** /// + + /** \brief Maximum number of cuts */ + std::uint32_t max_cuts_; + + /** \brief Minimum segment size for cutting */ + std::uint32_t min_segment_size_for_cutting_; + + /** \brief Cut_score threshold */ + float min_cut_score_; + + /** \brief Use local constrains for cutting */ + bool use_local_constrains_; + + /** \brief Use directed weights for the cutting */ + bool use_directed_weights_; + + /** \brief Use clean cutting */ + bool use_clean_cutting_; + + /** \brief Iterations for RANSAC */ + std::uint32_t ransac_itrs_; + + +/******************************************* Directional weighted RANSAC declarations ******************************************************************/ + /** \brief @b WeightedRandomSampleConsensus represents an implementation of the Directionally Weighted RANSAC algorithm, as described in: "Constrained Planar Cuts - Part Segmentation for Point Clouds", CVPR 2015, M. Schoeler, J. Papon, F. Wörgötter. + * \note It only uses points with a weight > 0 for the model calculation, but uses all points for the evaluation (scoring of the model) + * Only use in conjunction with sac_model_plane + * If you use this in a scientific work please cite the following paper: + * M. Schoeler, J. Papon, F. Woergoetter + * Constrained Planar Cuts - Object Partitioning for Point Clouds + * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2015 + * \author Markus Schoeler (mschoeler@web.de) + * \ingroup segmentation + */ + + class WeightedRandomSampleConsensus : public SampleConsensus + { + using SampleConsensusModelPtr = SampleConsensusModel::Ptr; + + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief WeightedRandomSampleConsensus (Weighted RAndom SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model, + bool random = false) + : SampleConsensus (model, random) + { + initialize (); + } + + /** \brief WeightedRandomSampleConsensus (Weighted RAndom SAmple Consensus) main constructor + * \param[in] model a Sample Consensus model + * \param[in] threshold distance to model threshold + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model, + double threshold, + bool random = false) + : SampleConsensus (model, threshold, random) + { + initialize (); + } + + /** \brief Compute the actual model and find the inliers + * \param[in] debug_verbosity_level enable/disable on-screen debug information and set the verbosity level + */ + bool + computeModel (int debug_verbosity_level = 0) override; + + /** \brief Set the weights for the input points + * \param[in] weights Weights for input samples. Negative weights are counted as penalty. + */ + void + setWeights (const std::vector &weights, + const bool directed_weights = false) + { + if (weights.size () != full_cloud_pt_indices_->size ()) + { + PCL_ERROR ("[pcl::WeightedRandomSampleConsensus::setWeights] Cannot assign weights. Weight vector needs to have the same length as the input pointcloud\n"); + return; + } + weights_ = weights; + model_pt_indices_->clear (); + for (std::size_t i = 0; i < weights.size (); ++i) + { + if (weights[i] > std::numeric_limits::epsilon ()) + model_pt_indices_->push_back (i); + } + use_directed_weights_ = directed_weights; + } + + /** \brief Get the best score + * \returns The best score found. + */ + double + getBestScore () const + { + return (best_score_); + } + + protected: + /** \brief Initialize the model parameters. Called by the constructors. */ + void + initialize () + { + // Maximum number of trials before we give up. + max_iterations_ = 10000; + use_directed_weights_ = false; + model_pt_indices_.reset (new std::vector); + full_cloud_pt_indices_.reset (new std::vector (* (sac_model_->getIndices ()))); + point_cloud_ptr_ = sac_model_->getInputCloud (); + } + + /** \brief weight each positive weight point by the inner product between the normal and the plane normal */ + bool use_directed_weights_; + + /** \brief vector of weights assigned to points. Set by the setWeights-method */ + std::vector weights_; + + /** \brief The indices used for estimating the RANSAC model. Only those whose weight is > 0 */ + pcl::IndicesPtr model_pt_indices_; + + /** \brief The complete list of indices used for the model evaluation */ + pcl::IndicesPtr full_cloud_pt_indices_; + + /** \brief Pointer to the input PointCloud */ + pcl::PointCloud::ConstPtr point_cloud_ptr_; + + /** \brief Highest score found so far */ + double best_score_; + }; + + }; +} + +#ifdef PCL_NO_PRECOMPILE + #include +#elif defined(PCL_ONLY_CORE_POINT_TYPES) + //pcl::PointXYZINormal is not a core point type (so we cannot use the precompiled classes here) + #include + #include +#endif // PCL_NO_PRECOMPILE / PCL_ONLY_CORE_POINT_TYPES diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/crf_normal_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/crf_normal_segmentation.h new file mode 100755 index 0000000..71be7ce --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/crf_normal_segmentation.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** + * \brief + * \author Christian Potthast + */ + template + class PCL_EXPORTS CrfNormalSegmentation + { + public: + /** \brief Constructor that sets default values for member variables. */ + CrfNormalSegmentation (); + + /** \brief Destructor that frees memory. */ + ~CrfNormalSegmentation (); + + /** + * \brief This method sets the input cloud. + * \param[in] input_cloud input point cloud + */ + void + setCloud (typename pcl::PointCloud::Ptr input_cloud); + + /** \brief This method simply launches the segmentation algorithm */ + void + segmentPoints (); + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/crf_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/crf_segmentation.h new file mode 100755 index 0000000..f06434d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/crf_segmentation.h @@ -0,0 +1,213 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +//#include + +namespace pcl +{ + /** \brief + * + */ + template + class PCL_EXPORTS CrfSegmentation + { + public: + + /** \brief Constructor that sets default values for member variables. */ + CrfSegmentation (); + + /** \brief This destructor destroys the cloud... + * + */ + ~CrfSegmentation (); + + /** \brief This method sets the input cloud. + * \param[in] input_cloud input point cloud + */ + void + setInputCloud (typename pcl::PointCloud::Ptr input_cloud); + + void + setAnnotatedCloud (typename pcl::PointCloud::Ptr anno_cloud); + + void + setNormalCloud (typename pcl::PointCloud::Ptr normal_cloud); + + + /** \brief Set the leaf size for the voxel grid. + * \param[in] x leaf size x-axis + * \param[in] y leaf size y-axis + * \param[in] z leaf size z-axis + */ + void + setVoxelGridLeafSize (const float x, const float y, const float z); + + void + setNumberOfIterations (unsigned int n_iterations = 10) {n_iterations_ = n_iterations;}; + + /** \brief This method simply launches the segmentation algorithm */ + void + segmentPoints (pcl::PointCloud &output); + + /** \brief Create a voxel grid to discretize the scene */ + void + createVoxelGrid (); + + /** \brief Get the data from the voxel grid and convert it into a vector */ + void + createDataVectorFromVoxelGrid (); + + + void + createUnaryPotentials (std::vector &unary, + std::vector &colors, + unsigned int n_labels); + + + /** \brief Set the smoothness kernel parameters. + * \param[in] sx standard deviation x + * \param[in] sy standard deviation y + * \param[in] sz standard deviation z + * \param[in] w weight + */ + void + setSmoothnessKernelParameters (const float sx, const float sy, const float sz, const float w); + + /** \brief Set the appearanche kernel parameters. + * \param[in] sx standard deviation x + * \param[in] sy standard deviation y + * \param[in] sz standard deviation z + * \param[in] sr standard deviation red + * \param[in] sg standard deviation green + * \param[in] sb standard deviation blue + * \param[in] w weight + */ + void + setAppearanceKernelParameters (float sx, float sy, float sz, + float sr, float sg, float sb, + float w); + + + void + setSurfaceKernelParameters (float sx, float sy, float sz, + float snx, float sny, float snz, + float w); + + + protected: + /** \brief Voxel grid to discretize the scene */ + typename pcl::VoxelGrid voxel_grid_; + + /** \brief input cloud that will be segmented. */ + typename pcl::PointCloud::Ptr input_cloud_; + typename pcl::PointCloud::Ptr anno_cloud_; + typename pcl::PointCloud::Ptr normal_cloud_; + + /** \brief voxel grid filtered cloud. */ + typename pcl::PointCloud::Ptr filtered_cloud_; + typename pcl::PointCloud::Ptr filtered_anno_; + typename pcl::PointCloud::Ptr filtered_normal_; + + /** \brief indices of the filtered cloud. */ + //typename pcl::VoxelGrid::IndicesPtr cloud_indices_; + + /** \brief Voxel grid leaf size */ + Eigen::Vector4f voxel_grid_leaf_size_; + + /** \brief Voxel grid dimensions */ + Eigen::Vector3i dim_; + + /** \brief voxel grid data points + packing order [x0y0z0, x1y0z0,x2y0z0,...,x0y1z0,x1y1z0,...,x0y0z1,x1y0z1,...] + */ + std::vector > data_; + + std::vector > color_; + + std::vector > normal_; + + /** \brief smoothness kernel parameters + * [0] = standard deviation x + * [1] = standard deviation y + * [2] = standard deviation z + * [3] = weight + */ + float smoothness_kernel_param_[4]; + + /** \brief appearance kernel parameters + * [0] = standard deviation x + * [1] = standard deviation y + * [2] = standard deviation z + * [3] = standard deviation red + * [4] = standard deviation green + * [5] = standard deviation blue + * [6] = weight + */ + float appearance_kernel_param_[7]; + + float surface_kernel_param_[7]; + + + unsigned int n_iterations_; + + + /** \brief Contains normals of the points that will be segmented. */ + //typename pcl::PointCloud::Ptr normals_; + + /** \brief Stores the cloud that will be segmented. */ + //typename pcl::PointCloud::Ptr cloud_for_segmentation_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/edge_aware_plane_comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/edge_aware_plane_comparator.h new file mode 100755 index 0000000..acb483c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/edge_aware_plane_comparator.h @@ -0,0 +1,211 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: extract_clusters.h 5027 2012-03-12 03:10:45Z rusu $ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients, + * for use in planar segmentation. + * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. + * + * \author Stefan Holzer, Alex Trevor + */ + template + class EdgeAwarePlaneComparator: public PlaneCoefficientComparator + { + public: + using PointCloud = typename Comparator::PointCloud; + using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::PlaneCoefficientComparator::input_; + using pcl::PlaneCoefficientComparator::normals_; + using pcl::PlaneCoefficientComparator::plane_coeff_d_; + using pcl::PlaneCoefficientComparator::angular_threshold_; + using pcl::PlaneCoefficientComparator::distance_threshold_; + using pcl::PlaneCoefficientComparator::depth_dependent_; + using pcl::PlaneCoefficientComparator::z_axis_; + + /** \brief Empty constructor for PlaneCoefficientComparator. */ + EdgeAwarePlaneComparator () : + distance_map_threshold_ (5), + curvature_threshold_ (0.04f), + euclidean_distance_threshold_ (0.04f) + { + } + + /** \brief Empty constructor for PlaneCoefficientComparator. + * \param[in] distance_map the distance map to use + */ + EdgeAwarePlaneComparator (const float *distance_map) : + distance_map_ (distance_map), + distance_map_threshold_ (5), + curvature_threshold_ (0.04f), + euclidean_distance_threshold_ (0.04f) + { + } + + /** \brief Destructor for PlaneCoefficientComparator. */ + + ~EdgeAwarePlaneComparator () + { + } + + /** \brief Set a distance map to use. For an example of a valid distance map see + * \ref OrganizedIntegralImageNormalEstimation + * \param[in] distance_map the distance map to use + */ + inline void + setDistanceMap (const float *distance_map) + { + distance_map_ = distance_map; + } + + /** \brief Return the distance map used. */ + const float* + getDistanceMap () const + { + return (distance_map_); + } + + /** \brief Set the curvature threshold for creating a new segment + * \param[in] curvature_threshold a threshold for the curvature + */ + void + setCurvatureThreshold (float curvature_threshold) + { + curvature_threshold_ = curvature_threshold; + } + + /** \brief Get the curvature threshold. */ + inline float + getCurvatureThreshold () const + { + return (curvature_threshold_); + } + + /** \brief Set the distance map threshold -- the number of pixel away from a border / nan + * \param[in] distance_map_threshold the distance map threshold + */ + void + setDistanceMapThreshold (float distance_map_threshold) + { + distance_map_threshold_ = distance_map_threshold; + } + + /** \brief Get the distance map threshold (in pixels). */ + inline float + getDistanceMapThreshold () const + { + return (distance_map_threshold_); + } + + /** \brief Set the euclidean distance threshold. + * \param[in] euclidean_distance_threshold the euclidean distance threshold in meters + */ + void + setEuclideanDistanceThreshold (float euclidean_distance_threshold) + { + euclidean_distance_threshold_ = euclidean_distance_threshold; + } + + /** \brief Get the euclidean distance threshold. */ + inline float + getEuclideanDistanceThreshold () const + { + return (euclidean_distance_threshold_); + } + + protected: + /** \brief Compare two neighboring points, by using normal information, curvature, and euclidean distance information. + * \param[in] idx1 The index of the first point. + * \param[in] idx2 The index of the second point. + */ + bool + compare (int idx1, int idx2) const override + { + // Note: there are two distance thresholds here that make sense to scale with depth. + // dist_threshold is on the perpendicular distance to the plane, as in plane comparator + // We additionally check euclidean distance to ensure that we don't have neighboring coplanar points + // that aren't close in euclidean space (think two tables separated by a meter, viewed from an angle + // where the surfaces are adjacent in image space). + float dist_threshold = distance_threshold_; + float euclidean_dist_threshold = euclidean_distance_threshold_; + if (depth_dependent_) + { + Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); + float z = vec.dot (z_axis_); + dist_threshold *= z * z; + euclidean_dist_threshold *= z * z; + } + + float dx = input_->points[idx1].x - input_->points[idx2].x; + float dy = input_->points[idx1].y - input_->points[idx2].y; + float dz = input_->points[idx1].z - input_->points[idx2].z; + float dist = std::sqrt (dx*dx + dy*dy + dz*dz); + + bool normal_ok = (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ); + bool dist_ok = (dist < euclidean_dist_threshold); + + bool curvature_ok = normals_->points[idx1].curvature < curvature_threshold_; + bool plane_d_ok = std::abs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < dist_threshold; + + if (distance_map_[idx1] < distance_map_threshold_) + curvature_ok = false; + + return (dist_ok && normal_ok && curvature_ok && plane_d_ok); + } + + protected: + const float* distance_map_; + int distance_map_threshold_; + float curvature_threshold_; + float euclidean_distance_threshold_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/euclidean_cluster_comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/euclidean_cluster_comparator.h new file mode 100755 index 0000000..7295978 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/euclidean_cluster_comparator.h @@ -0,0 +1,270 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace experimental + { + template + class EuclideanClusterComparator : public ::pcl::Comparator + { + protected: + + using pcl::Comparator::input_; + + public: + using typename Comparator::PointCloud; + using typename Comparator::PointCloudConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using ExcludeLabelSet = std::set; + using ExcludeLabelSetPtr = shared_ptr; + using ExcludeLabelSetConstPtr = shared_ptr; + + /** \brief Default constructor for EuclideanClusterComparator. */ + EuclideanClusterComparator () + : distance_threshold_ (0.005f) + , depth_dependent_ () + {} + + void + setInputCloud (const PointCloudConstPtr& cloud) override + { + input_ = cloud; + Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix (); + z_axis_ = rot.col (2); + } + + /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. + * \param[in] distance_threshold the tolerance in meters + * \param depth_dependent + */ + inline void + setDistanceThreshold (float distance_threshold, bool depth_dependent) + { + distance_threshold_ = distance_threshold; + depth_dependent_ = depth_dependent; + } + + /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */ + inline float + getDistanceThreshold () const + { + return (distance_threshold_); + } + + /** \brief Set label cloud + * \param[in] labels The label cloud + */ + void + setLabels (const PointCloudLPtr& labels) + { + labels_ = labels; + } + + const ExcludeLabelSetConstPtr& + getExcludeLabels () const + { + return exclude_labels_; + } + + /** \brief Set labels in the label cloud to exclude. + * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered + */ + void + setExcludeLabels (const ExcludeLabelSetConstPtr &exclude_labels) + { + exclude_labels_ = exclude_labels; + } + + /** \brief Compare points at two indices by their euclidean distance + * \param idx1 The first index for the comparison + * \param idx2 The second index for the comparison + */ + bool + compare (int idx1, int idx2) const override + { + if (labels_ && exclude_labels_) + { + assert (labels_->size () == input_->size ()); + const std::uint32_t &label1 = (*labels_)[idx1].label; + const std::uint32_t &label2 = (*labels_)[idx2].label; + + const std::set::const_iterator it1 = exclude_labels_->find (label1); + if (it1 == exclude_labels_->end ()) + return false; + + const std::set::const_iterator it2 = exclude_labels_->find (label2); + if (it2 == exclude_labels_->end ()) + return false; + } + + float dist_threshold = distance_threshold_; + if (depth_dependent_) + { + Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); + float z = vec.dot (z_axis_); + dist_threshold *= z * z; + } + + const float dist = ((*input_)[idx1].getVector3fMap () + - (*input_)[idx2].getVector3fMap ()).norm (); + return (dist < dist_threshold); + } + + protected: + + + /** \brief Set of labels with similar size as the input point cloud, + * aggregating points into groups based on a similar label identifier. + * + * It needs to be set in conjunction with the \ref exclude_labels_ + * member in order to provided a masking functionality. + */ + PointCloudLPtr labels_; + + /** \brief Specifies which labels should be excluded com being clustered. + * + * If a label is not specified, it's assumed by default that it's + * intended be excluded + */ + ExcludeLabelSetConstPtr exclude_labels_; + + float distance_threshold_; + + bool depth_dependent_; + + Eigen::Vector3f z_axis_; + }; + } // namespace experimental + + + /** \brief EuclideanClusterComparator is a comparator used for finding clusters based on euclidian distance. + * + * \author Alex Trevor + */ + template + class EuclideanClusterComparator : public experimental::EuclideanClusterComparator + { + protected: + + using experimental::EuclideanClusterComparator::exclude_labels_; + + public: + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using experimental::EuclideanClusterComparator::setExcludeLabels; + + /** \brief Default constructor for EuclideanClusterComparator. */ + PCL_DEPRECATED("remove PointNT from template parameters") + EuclideanClusterComparator () + : normals_ () + , angular_threshold_ (0.0f) + {} + + /** \brief Provide a pointer to the input normals. + * \param[in] normals the input normal cloud + */ + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") + inline void + setInputNormals (const PointCloudNConstPtr& normals) { normals_ = normals; } + + /** \brief Get the input normals. */ + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") + inline PointCloudNConstPtr + getInputNormals () const { return (normals_); } + + /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. + * \param[in] angular_threshold the tolerance in radians + */ + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") + inline void + setAngularThreshold (float angular_threshold) + { + angular_threshold_ = std::cos (angular_threshold); + } + + /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") + inline float + getAngularThreshold () const { return (std::acos (angular_threshold_) ); } + + /** \brief Set labels in the label cloud to exclude. + * \param[in] exclude_labels a vector of bools corresponding to whether or not a given label should be considered + */ + PCL_DEPRECATED("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead") + void + setExcludeLabels (const std::vector& exclude_labels) + { + exclude_labels_ = boost::make_shared > (); + for (std::size_t i = 0; i < exclude_labels.size (); ++i) + if (exclude_labels[i]) + exclude_labels_->insert (i); + } + + protected: + + PointCloudNConstPtr normals_; + + float angular_threshold_; + }; + + template + class EuclideanClusterComparator + : public experimental::EuclideanClusterComparator {}; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/euclidean_plane_coefficient_comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/euclidean_plane_coefficient_comparator.h new file mode 100755 index 0000000..938bcd5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/euclidean_plane_coefficient_comparator.h @@ -0,0 +1,98 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief EuclideanPlaneCoefficientComparator is a Comparator that operates on plane coefficients, + * for use in planar segmentation. + * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. + * + * \author Alex Trevor + */ + template + class EuclideanPlaneCoefficientComparator: public PlaneCoefficientComparator + { + public: + using PointCloud = typename Comparator::PointCloud; + using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::Comparator::input_; + using pcl::PlaneCoefficientComparator::normals_; + using pcl::PlaneCoefficientComparator::angular_threshold_; + using pcl::PlaneCoefficientComparator::distance_threshold_; + + /** \brief Empty constructor for PlaneCoefficientComparator. */ + EuclideanPlaneCoefficientComparator () + { + } + + /** \brief Destructor for PlaneCoefficientComparator. */ + + ~EuclideanPlaneCoefficientComparator () + { + } + + /** \brief Compare two neighboring points, by using normal information, and euclidean distance information. + * \param[in] idx1 The index of the first point. + * \param[in] idx2 The index of the second point. + */ + bool + compare (int idx1, int idx2) const override + { + float dx = input_->points[idx1].x - input_->points[idx2].x; + float dy = input_->points[idx1].y - input_->points[idx2].y; + float dz = input_->points[idx1].z - input_->points[idx2].z; + float dist = std::sqrt (dx*dx + dy*dy + dz*dz); + + return ( (dist < distance_threshold_) + && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) ); + } + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/extract_clusters.h b/deps_install/include/pcl-1.10/pcl/segmentation/extract_clusters.h new file mode 100755 index 0000000..9ab0c84 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/extract_clusters.h @@ -0,0 +1,425 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include + +#include + +namespace pcl +{ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param cloud the point cloud message + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud + * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) + * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \ingroup segmentation + */ + template void + extractEuclideanClusters ( + const PointCloud &cloud, const typename search::Search::Ptr &tree, + float tolerance, std::vector &clusters, + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param cloud the point cloud message + * \param indices a list of point indices to use from \a cloud + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud and \a indices + * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) + * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \ingroup segmentation + */ + template void + extractEuclideanClusters ( + const PointCloud &cloud, const std::vector &indices, + const typename search::Search::Ptr &tree, float tolerance, std::vector &clusters, + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal + * angular deviation + * \param cloud the point cloud message + * \param normals the point cloud message containing normal information + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud + * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space + * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) + * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing + * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \ingroup segmentation + */ + template void + extractEuclideanClusters ( + const PointCloud &cloud, const PointCloud &normals, + float tolerance, const typename KdTree::Ptr &tree, + std::vector &clusters, double eps_angle, + unsigned int min_pts_per_cluster = 1, + unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()) + { + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + if (cloud.points.size () != normals.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); + return; + } + + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + // Process all points in the indices vector + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + if (processed[i]) + continue; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (static_cast (i)); + + processed[i] = true; + + while (sq_idx < static_cast (seed_queue.size ())) + { + // Search for sq_idx + if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) + { + sq_idx++; + continue; + } + + for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + //processed[nn_indices[j]] = true; + // [-1;1] + double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] + + normals.points[i].normal[1] * normals.points[nn_indices[j]].normal[1] + + normals.points[i].normal[2] * normals.points[nn_indices[j]].normal[2]; + if ( std::abs (std::acos (dot_p)) < eps_angle ) + { + processed[nn_indices[j]] = true; + seed_queue.push_back (nn_indices[j]); + } + } + + sq_idx++; + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + pcl::PointIndices r; + r.indices.resize (seed_queue.size ()); + for (std::size_t j = 0; j < seed_queue.size (); ++j) + r.indices[j] = seed_queue[j]; + + // These two lines should not be needed: (can anyone confirm?) -FF + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + r.header = cloud.header; + clusters.push_back (r); // We could avoid a copy by working directly in the vector + } + } + } + + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal + * angular deviation + * \param cloud the point cloud message + * \param normals the point cloud message containing normal information + * \param indices a list of point indices to use from \a cloud + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud + * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space + * \param clusters the resultant clusters containing point indices (as PointIndices) + * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing + * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \ingroup segmentation + */ + template + void extractEuclideanClusters ( + const PointCloud &cloud, const PointCloud &normals, + const std::vector &indices, const typename KdTree::Ptr &tree, + float tolerance, std::vector &clusters, double eps_angle, + unsigned int min_pts_per_cluster = 1, + unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()) + { + // \note If the tree was created over , we guarantee a 1-1 mapping between what the tree returns + //and indices[i] + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + if (tree->getIndices ()->size () != indices.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ()); + return; + } + if (cloud.points.size () != normals.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); + return; + } + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + // Process all points in the indices vector + for (std::size_t i = 0; i < indices.size (); ++i) + { + if (processed[indices[i]]) + continue; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (indices[i]); + + processed[indices[i]] = true; + + while (sq_idx < static_cast (seed_queue.size ())) + { + // Search for sq_idx + if (!tree->radiusSearch (cloud.points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances)) + { + sq_idx++; + continue; + } + + for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + //processed[nn_indices[j]] = true; + // [-1;1] + double dot_p = + normals.points[indices[i]].normal[0] * normals.points[indices[nn_indices[j]]].normal[0] + + normals.points[indices[i]].normal[1] * normals.points[indices[nn_indices[j]]].normal[1] + + normals.points[indices[i]].normal[2] * normals.points[indices[nn_indices[j]]].normal[2]; + if ( std::abs (std::acos (dot_p)) < eps_angle ) + { + processed[nn_indices[j]] = true; + seed_queue.push_back (nn_indices[j]); + } + } + + sq_idx++; + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + pcl::PointIndices r; + r.indices.resize (seed_queue.size ()); + for (std::size_t j = 0; j < seed_queue.size (); ++j) + r.indices[j] = seed_queue[j]; + + // These two lines should not be needed: (can anyone confirm?) -FF + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + r.header = cloud.header; + clusters.push_back (r); + } + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. + * \author Radu Bogdan Rusu + * \ingroup segmentation + */ + template + class EuclideanClusterExtraction: public PCLBase + { + using BasePCLBase = PCLBase; + + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Empty constructor. */ + EuclideanClusterExtraction () : tree_ (), + cluster_tolerance_ (0), + min_pts_per_cluster_ (1), + max_pts_per_cluster_ (std::numeric_limits::max ()) + {}; + + /** \brief Provide a pointer to the search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) + { + tree_ = tree; + } + + /** \brief Get a pointer to the search method used. + * @todo fix this for a generic search tree + */ + inline KdTreePtr + getSearchMethod () const + { + return (tree_); + } + + /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space + * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space + */ + inline void + setClusterTolerance (double tolerance) + { + cluster_tolerance_ = tolerance; + } + + /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */ + inline double + getClusterTolerance () const + { + return (cluster_tolerance_); + } + + /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. + * \param[in] min_cluster_size the minimum cluster size + */ + inline void + setMinClusterSize (int min_cluster_size) + { + min_pts_per_cluster_ = min_cluster_size; + } + + /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */ + inline int + getMinClusterSize () const + { + return (min_pts_per_cluster_); + } + + /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. + * \param[in] max_cluster_size the maximum cluster size + */ + inline void + setMaxClusterSize (int max_cluster_size) + { + max_pts_per_cluster_ = max_cluster_size; + } + + /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */ + inline int + getMaxClusterSize () const + { + return (max_pts_per_cluster_); + } + + /** \brief Cluster extraction in a PointCloud given by + * \param[out] clusters the resultant point clusters + */ + void + extract (std::vector &clusters); + + protected: + // Members derived from the base class + using BasePCLBase::input_; + using BasePCLBase::indices_; + using BasePCLBase::initCompute; + using BasePCLBase::deinitCompute; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */ + double cluster_tolerance_; + + /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */ + int min_pts_per_cluster_; + + /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */ + int max_pts_per_cluster_; + + /** \brief Class getName method. */ + virtual std::string getClassName () const { return ("EuclideanClusterExtraction"); } + + }; + + /** \brief Sort clusters method (for std::sort). + * \ingroup segmentation + */ + inline bool + comparePointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b) + { + return (a.indices.size () < b.indices.size ()); + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/extract_labeled_clusters.h b/deps_install/include/pcl-1.10/pcl/segmentation/extract_labeled_clusters.h new file mode 100755 index 0000000..e1a9b52 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/extract_labeled_clusters.h @@ -0,0 +1,190 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param[in] cloud the point cloud message + * \param[in] tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud + * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param[out] labeled_clusters the resultant clusters containing point indices (as a vector of PointIndices) + * \param[in] min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param[in] max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \param[in] max_label + * \ingroup segmentation + */ + template void + extractLabeledEuclideanClusters ( + const PointCloud &cloud, const typename search::Search::Ptr &tree, + float tolerance, std::vector > &labeled_clusters, + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max (), + unsigned int max_label = std::numeric_limits::max ()); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b LabeledEuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense, with label info. + * \author Koen Buys + * \ingroup segmentation + */ + template + class LabeledEuclideanClusterExtraction: public PCLBase + { + using BasePCLBase = PCLBase; + + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Empty constructor. */ + LabeledEuclideanClusterExtraction () : + tree_ (), + cluster_tolerance_ (0), + min_pts_per_cluster_ (1), + max_pts_per_cluster_ (std::numeric_limits::max ()), + max_label_ (std::numeric_limits::max ()) + {}; + + /** \brief Provide a pointer to the search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () const { return (tree_); } + + /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space + * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space + */ + inline void + setClusterTolerance (double tolerance) { cluster_tolerance_ = tolerance; } + + /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */ + inline double + getClusterTolerance () const { return (cluster_tolerance_); } + + /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. + * \param[in] min_cluster_size the minimum cluster size + */ + inline void + setMinClusterSize (int min_cluster_size) { min_pts_per_cluster_ = min_cluster_size; } + + /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */ + inline int + getMinClusterSize () const { return (min_pts_per_cluster_); } + + /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. + * \param[in] max_cluster_size the maximum cluster size + */ + inline void + setMaxClusterSize (int max_cluster_size) { max_pts_per_cluster_ = max_cluster_size; } + + /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */ + inline int + getMaxClusterSize () const { return (max_pts_per_cluster_); } + + /** \brief Set the maximum number of labels in the cloud. + * \param[in] max_label the maximum + */ + inline void + setMaxLabels (unsigned int max_label) { max_label_ = max_label; } + + /** \brief Get the maximum number of labels */ + inline unsigned int + getMaxLabels () const { return (max_label_); } + + /** \brief Cluster extraction in a PointCloud given by + * \param[out] labeled_clusters the resultant point clusters + */ + void + extract (std::vector > &labeled_clusters); + + protected: + // Members derived from the base class + using BasePCLBase::input_; + using BasePCLBase::indices_; + using BasePCLBase::initCompute; + using BasePCLBase::deinitCompute; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */ + double cluster_tolerance_; + + /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */ + int min_pts_per_cluster_; + + /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */ + int max_pts_per_cluster_; + + /** \brief The maximum number of labels we can find in this pointcloud (default = MAXINT)*/ + unsigned int max_label_; + + /** \brief Class getName method. */ + virtual std::string getClassName () const { return ("LabeledEuclideanClusterExtraction"); } + + }; + + /** \brief Sort clusters method (for std::sort). + * \ingroup segmentation + */ + inline bool + compareLabeledPointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b) + { + return (a.indices.size () < b.indices.size ()); + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/extract_polygonal_prism_data.h b/deps_install/include/pcl-1.10/pcl/segmentation/extract_polygonal_prism_data.h new file mode 100755 index 0000000..f2df3fd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/extract_polygonal_prism_data.h @@ -0,0 +1,214 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief General purpose method for checking if a 3D point is inside or + * outside a given 2D polygon. + * \note this method accepts any general 3D point that is projected onto the + * 2D polygon, but performs an internal XY projection of both the polygon and the point. + * \param point a 3D point projected onto the same plane as the polygon + * \param polygon a polygon + * \ingroup segmentation + */ + template bool + isPointIn2DPolygon (const PointT &point, const pcl::PointCloud &polygon); + + /** \brief Check if a 2d point (X and Y coordinates considered only!) is + * inside or outside a given polygon. This method assumes that both the point + * and the polygon are projected onto the XY plane. + * + * \note (This is highly optimized code taken from http://www.visibone.com/inpoly/) + * Copyright (c) 1995-1996 Galacticomm, Inc. Freeware source code. + * \param point a 2D point projected onto the same plane as the polygon + * \param polygon a polygon + * \ingroup segmentation + */ + template bool + isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud &polygon); + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ExtractPolygonalPrismData uses a set of point indices that + * represent a planar model, and together with a given height, generates a 3D + * polygonal prism. The polygonal prism is then used to segment all points + * lying inside it. + * + * An example of its usage is to extract the data lying within a set of 3D + * boundaries (e.g., objects supported by a plane). + * + * Example usage: + * \code{.cpp} + * double z_min = 0., z_max = 0.05; // we want the points above the plane, no farther than 5 cm from the surface + * pcl::PointCloud::Ptr hull_points (new pcl::PointCloud ()); + * pcl::ConvexHull hull; + * // hull.setDimension (2); // not necessarily needed, but we need to check the dimensionality of the output + * hull.setInputCloud (cloud); + * hull.reconstruct (hull_points); + * if (hull.getDimension () == 2) + * { + * pcl::ExtractPolygonalPrismData prism; + * prism.setInputCloud (point_cloud); + * prism.setInputPlanarHull (hull_points); + * prism.setHeightLimits (z_min, z_max); + * prism.segment (cloud_indices); + * } + * else + * PCL_ERROR ("The input cloud does not represent a planar surface.\n"); + * \endcode + * \author Radu Bogdan Rusu + * \ingroup segmentation + */ + template + class ExtractPolygonalPrismData : public PCLBase + { + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + /** \brief Empty constructor. */ + ExtractPolygonalPrismData () : planar_hull_ (), min_pts_hull_ (3), + height_limit_min_ (0), height_limit_max_ (FLT_MAX), + vpx_ (0), vpy_ (0), vpz_ (0) + {}; + + /** \brief Provide a pointer to the input planar hull dataset. + * \note Please see the example in the class description for how to obtain this. + * \param[in] hull the input planar hull dataset + */ + inline void + setInputPlanarHull (const PointCloudConstPtr &hull) { planar_hull_ = hull; } + + /** \brief Get a pointer the input planar hull dataset. */ + inline PointCloudConstPtr + getInputPlanarHull () const { return (planar_hull_); } + + /** \brief Set the height limits. All points having distances to the + * model outside this interval will be discarded. + * + * \param[in] height_min the minimum allowed distance to the plane model value + * \param[in] height_max the maximum allowed distance to the plane model value + */ + inline void + setHeightLimits (double height_min, double height_max) + { + height_limit_min_ = height_min; + height_limit_max_ = height_max; + } + + /** \brief Get the height limits (min/max) as set by the user. The + * default values are -FLT_MAX, FLT_MAX. + * \param[out] height_min the resultant min height limit + * \param[out] height_max the resultant max height limit + */ + inline void + getHeightLimits (double &height_min, double &height_max) const + { + height_min = height_limit_min_; + height_max = height_limit_max_; + } + + /** \brief Set the viewpoint. + * \param[in] vpx the X coordinate of the viewpoint + * \param[in] vpy the Y coordinate of the viewpoint + * \param[in] vpz the Z coordinate of the viewpoint + */ + inline void + setViewPoint (float vpx, float vpy, float vpz) + { + vpx_ = vpx; + vpy_ = vpy; + vpz_ = vpz; + } + + /** \brief Get the viewpoint. */ + inline void + getViewPoint (float &vpx, float &vpy, float &vpz) const + { + vpx = vpx_; + vpy = vpy_; + vpz = vpz_; + } + + /** \brief Cluster extraction in a PointCloud given by + * \param[out] output the resultant point indices that support the model found (inliers) + */ + void + segment (PointIndices &output); + + protected: + /** \brief A pointer to the input planar hull dataset. */ + PointCloudConstPtr planar_hull_; + + /** \brief The minimum number of points needed on the convex hull. */ + int min_pts_hull_; + + /** \brief The minimum allowed height (distance to the model) a point + * will be considered from. + */ + double height_limit_min_; + + /** \brief The maximum allowed height (distance to the model) a point + * will be considered from. + */ + double height_limit_max_; + + /** \brief Values describing the data acquisition viewpoint. Default: 0,0,0. */ + float vpx_, vpy_, vpz_; + + /** \brief Class getName method. */ + virtual std::string + getClassName () const { return ("ExtractPolygonalPrismData"); } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/grabcut_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/grabcut_segmentation.h new file mode 100755 index 0000000..d8ecbf5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/grabcut_segmentation.h @@ -0,0 +1,482 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace segmentation + { + namespace grabcut + { + /** boost implementation of Boykov and Kolmogorov's maxflow algorithm doesn't support + * negative flows which makes it inappropriate for this context. + * This implementation of Boykov and Kolmogorov's maxflow algorithm by Stephen Gould + * in DARWIN under BSD does the trick however solwer than original + * implementation. + */ + class PCL_EXPORTS BoykovKolmogorov + { + public: + using vertex_descriptor = int; + using edge_capacity_type = double; + + /// construct a maxflow/mincut problem with estimated max_nodes + BoykovKolmogorov (std::size_t max_nodes = 0); + /// destructor + virtual ~BoykovKolmogorov () {} + /// get number of nodes in the graph + std::size_t + numNodes () const { return nodes_.size (); } + /// reset all edge capacities to zero (but don't free the graph) + void + reset (); + /// clear the graph and internal datastructures + void + clear (); + /// add nodes to the graph (returns the id of the first node added) + int + addNodes (std::size_t n = 1); + /// add constant flow to graph + void + addConstant (double c) { flow_value_ += c; } + /// add edge from s to nodeId + void + addSourceEdge (int u, double cap); + /// add edge from nodeId to t + void + addTargetEdge (int u, double cap); + /// add edge from u to v and edge from v to u + /// (requires cap_uv + cap_vu >= 0) + void + addEdge (int u, int v, double cap_uv, double cap_vu = 0.0); + /// solve the max-flow problem and return the flow + double + solve (); + /// return true if \p u is in the s-set after calling \ref solve. + bool + inSourceTree (int u) const { return (cut_[u] == SOURCE); } + /// return true if \p u is in the t-set after calling \ref solve + bool + inSinkTree (int u) const { return (cut_[u] == TARGET); } + /// returns the residual capacity for an edge (use -1 for terminal (-1,-1) is the current flow + double + operator() (int u, int v) const; + + double + getSourceEdgeCapacity (int u) const; + + double + getTargetEdgeCapacity (int u) const; + + protected: + /// tree states + enum nodestate { FREE = 0x00, SOURCE = 0x01, TARGET = 0x02 }; + /// capacitated edge + using capacitated_edge = std::map; + /// edge pair + using edge_pair = std::pair; + /// pre-augment s-u-t and s-u-v-t paths + void + preAugmentPaths (); + /// initialize trees from source and target + void + initializeTrees (); + /// expand trees until a path is found (or no path (-1, -1)) + std::pair + expandTrees (); + /// augment the path found by expandTrees; return orphaned subtrees + void + augmentPath (const std::pair& path, std::deque& orphans); + /// adopt orphaned subtrees + void + adoptOrphans (std::deque& orphans); + /// clear active set + void clearActive (); + /// \return true if active set is empty + inline bool + isActiveSetEmpty () const { return (active_head_ == TERMINAL); } + /// active if head or previous node is not the terminal + inline bool + isActive (int u) const { return ((u == active_head_) || (active_list_[u].first != TERMINAL)); } + /// mark vertex as active + void + markActive (int u); + /// mark vertex as inactive + void + markInactive (int u); + /// edges leaving the source + std::vector source_edges_; + /// edges entering the target + std::vector target_edges_; + /// nodes and their outgoing internal edges + std::vector nodes_; + /// current flow value (includes constant) + double flow_value_; + /// identifies which side of the cut a node falls + std::vector cut_; + + private: + /// parents_ flag for terminal state + static const int TERMINAL; // -1 + /// search tree (also uses cut_) + std::vector > parents_; + /// doubly-linked list (prev, next) + std::vector > active_list_; + int active_head_, active_tail_; + }; + + /**\brief Structure to save RGB colors into floats */ + struct Color + { + Color () : r (0), g (0), b (0) {} + Color (float _r, float _g, float _b) : r(_r), g(_g), b(_b) {} + Color (const pcl::RGB& color) : r (color.r), g (color.g), b (color.b) {} + + template + Color (const PointT& p); + + template + operator PointT () const; + + float r, g, b; + }; + /// An Image is a point cloud of Color + using Image = pcl::PointCloud; + /** \brief Compute squared distance between two colors + * \param[in] c1 first color + * \param[in] c2 second color + * \return the squared distance measure in RGB space + */ + float + colorDistance (const Color& c1, const Color& c2); + /// User supplied Trimap values + enum TrimapValue { TrimapUnknown = -1, TrimapForeground, TrimapBackground }; + /// Grabcut derived hard segmentation values + enum SegmentationValue { SegmentationForeground = 0, SegmentationBackground }; + /// Gaussian structure + struct Gaussian + { + Gaussian () {} + /// mean of the gaussian + Color mu; + /// covariance matrix of the gaussian + Eigen::Matrix3f covariance; + /// determinant of the covariance matrix + float determinant; + /// inverse of the covariance matrix + Eigen::Matrix3f inverse; + /// weighting of this gaussian in the GMM. + float pi; + /// highest eigenvalue of covariance matrix + float eigenvalue; + /// eigenvector corresponding to the highest eigenvector + Eigen::Vector3f eigenvector; + }; + + class PCL_EXPORTS GMM + { + public: + /// Initialize GMM with ddesired number of gaussians. + GMM () : gaussians_ (0) {} + /// Initialize GMM with ddesired number of gaussians. + GMM (std::size_t K) : gaussians_ (K) {} + /// Destructor + ~GMM () {} + /// \return K + std::size_t + getK () const { return gaussians_.size (); } + /// resize gaussians + void + resize (std::size_t K) { gaussians_.resize (K); } + /// \return a reference to the gaussian at a given position + Gaussian& + operator[] (std::size_t pos) { return (gaussians_[pos]); } + /// \return a const reference to the gaussian at a given position + const Gaussian& + operator[] (std::size_t pos) const { return (gaussians_[pos]); } + /// \brief \return the computed probability density of a color in this GMM + float + probabilityDensity (const Color &c); + /// \brief \return the computed probability density of a color in just one Gaussian + float + probabilityDensity(std::size_t i, const Color &c); + + private: + /// array of gaussians + std::vector gaussians_; + }; + + /** Helper class that fits a single Gaussian to color samples */ + class GaussianFitter + { + public: + GaussianFitter (float epsilon = 0.0001) + : sum_ (Eigen::Vector3f::Zero ()) + , accumulator_ (Eigen::Matrix3f::Zero ()) + , count_ (0) + , epsilon_ (epsilon) + { } + + /// Add a color sample + void + add (const Color &c); + /// Build the gaussian out of all the added color samples + void + fit (Gaussian& g, std::size_t total_count, bool compute_eigens = false) const; + /// \return epsilon + float + getEpsilon () { return (epsilon_); } + /** set epsilon which will be added to the covariance matrix diagonal which avoids singular + * covariance matrix + * \param[in] epsilon user defined epsilon + */ + void + setEpsilon (float epsilon) { epsilon_ = epsilon; } + + private: + /// sum of r,g, and b + Eigen::Vector3f sum_; + /// matrix of products (i.e. r*r, r*g, r*b), some values are duplicated. + Eigen::Matrix3f accumulator_; + /// count of color samples added to the gaussian + std::uint32_t count_; + /// small value to add to covariance matrix diagonal to avoid singular values + float epsilon_; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** Build the initial GMMs using the Orchard and Bouman color clustering algorithm */ + PCL_EXPORTS void + buildGMMs (const Image &image, + const std::vector& indices, + const std::vector &hardSegmentation, + std::vector &components, + GMM &background_GMM, GMM &foreground_GMM); + /** Iteratively learn GMMs using GrabCut updating algorithm */ + PCL_EXPORTS void + learnGMMs (const Image& image, + const std::vector& indices, + const std::vector& hard_segmentation, + std::vector& components, + GMM& background_GMM, GMM& foreground_GMM); + } + }; + + /** \brief Implementation of the GrabCut segmentation in + * "GrabCut — Interactive Foreground Extraction using Iterated Graph Cuts" by + * Carsten Rother, Vladimir Kolmogorov and Andrew Blake. + * + * \author Justin Talbot, jtalbot@stanford.edu placed in Public Domain, 2010 + * \author Nizar Sallem port to PCL and adaptation of original code. + * \ingroup segmentation + */ + template + class GrabCut : public pcl::PCLBase + { + public: + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + using PointCloudConstPtr = typename PCLBase::PointCloudConstPtr; + using PointCloudPtr = typename PCLBase::PointCloudPtr; + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::fake_indices_; + + /// Constructor + GrabCut (std::uint32_t K = 5, float lambda = 50.f) + : K_ (K) + , lambda_ (lambda) + , nb_neighbours_ (9) + , initialized_ (false) + {} + /// Destructor + ~GrabCut () {}; + // /// Set input cloud + void + setInputCloud (const PointCloudConstPtr& cloud) override; + /// Set background points, foreground points = points \ background points + void + setBackgroundPoints (const PointCloudConstPtr& background_points); + /// Set background indices, foreground indices = indices \ background indices + void + setBackgroundPointsIndices (int x1, int y1, int x2, int y2); + /// Set background indices, foreground indices = indices \ background indices + void + setBackgroundPointsIndices (const PointIndicesConstPtr& indices); + /// Run Grabcut refinement on the hard segmentation + virtual void + refine (); + /// \return the number of pixels that have changed from foreground to background or vice versa + virtual int + refineOnce (); + /// \return lambda + float + getLambda () { return (lambda_); } + /** Set lambda parameter to user given value. Suggested value by the authors is 50 + * \param[in] lambda + */ + void + setLambda (float lambda) { lambda_ = lambda; } + /// \return the number of components in the GMM + std::uint32_t + getK () { return (K_); } + /** Set K parameter to user given value. Suggested value by the authors is 5 + * \param[in] K the number of components used in GMM + */ + void + setK (std::uint32_t K) { K_ = K; } + /** \brief Provide a pointer to the search object. + * \param tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () { return (tree_); } + /** \brief Allows to set the number of neighbours to find. + * \param[in] nb_neighbours new number of neighbours + */ + void + setNumberOfNeighbours (int nb_neighbours) { nb_neighbours_ = nb_neighbours; } + /** \brief Returns the number of neighbours to find. */ + int + getNumberOfNeighbours () const { return (nb_neighbours_); } + /** \brief This method launches the segmentation algorithm and returns the clusters that were + * obtained during the segmentation. The indices of points belonging to the object will be stored + * in the cluster with index 1, other indices will be stored in the cluster with index 0. + * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices. + */ + void + extract (std::vector& clusters); + + protected: + // Storage for N-link weights, each pixel stores links to nb_neighbours + struct NLinks + { + NLinks () : nb_links (0), indices (0), dists (0), weights (0) {} + + int nb_links; + std::vector indices; + std::vector dists; + std::vector weights; + }; + bool + initCompute (); + using vertex_descriptor = pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor; + /// Compute beta from image + void + computeBetaOrganized (); + /// Compute beta from cloud + void + computeBetaNonOrganized (); + /// Compute L parameter from given lambda + void + computeL (); + /// Compute NLinks from image + void + computeNLinksOrganized (); + /// Compute NLinks from cloud + void + computeNLinksNonOrganized (); + /// Edit Trimap + void + setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t); + int + updateHardSegmentation (); + /// Fit Gaussian Multi Models + virtual void + fitGMMs (); + /// Build the graph for GraphCut + void + initGraph (); + /// Add an edge to the graph, graph must be oriented so we add the edge and its reverse + void + addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity); + /// Set the weights of SOURCE --> v and v --> SINK + void + setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity); + /// \return true if v is in source tree + inline bool + isSource (vertex_descriptor v) { return (graph_.inSourceTree (v)); } + /// image width + std::uint32_t width_; + /// image height + std::uint32_t height_; + // Variables used in formulas from the paper. + /// Number of GMM components + std::uint32_t K_; + /// lambda = 50. This value was suggested the GrabCut paper. + float lambda_; + /// beta = 1/2 * average of the squared color distances between all pairs of 8-neighboring pixels. + float beta_; + /// L = a large value to force a pixel to be foreground or background + float L_; + /// Pointer to the spatial search object. + KdTreePtr tree_; + /// Number of neighbours + int nb_neighbours_; + /// is segmentation initialized + bool initialized_; + /// Precomputed N-link weights + std::vector n_links_; + /// Converted input + segmentation::grabcut::Image::Ptr image_; + std::vector trimap_; + std::vector GMM_component_; + std::vector hard_segmentation_; + // Not yet implemented (this would be interpreted as alpha) + std::vector soft_segmentation_; + segmentation::grabcut::GMM background_GMM_, foreground_GMM_; + // Graph part + /// Graph for Graphcut + pcl::segmentation::grabcut::BoykovKolmogorov graph_; + /// Graph nodes + std::vector graph_nodes_; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/ground_plane_comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/ground_plane_comparator.h new file mode 100755 index 0000000..3ca912b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/ground_plane_comparator.h @@ -0,0 +1,247 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving. + * In conjunction with OrganizedConnectedComponentSegmentation, this allows smooth groundplanes / road surfaces to be segmented from point clouds. + * + * \author Alex Trevor + */ + template + class GroundPlaneComparator: public Comparator + { + public: + using PointCloud = typename Comparator::PointCloud; + using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::Comparator::input_; + + /** \brief Empty constructor for GroundPlaneComparator. */ + GroundPlaneComparator () + : normals_ () + , angular_threshold_ (std::cos (pcl::deg2rad (2.0f))) + , road_angular_threshold_ ( std::cos(pcl::deg2rad (10.0f))) + , distance_threshold_ (0.1f) + , depth_dependent_ (true) + , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) ) + , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0)) + { + } + + /** \brief Constructor for GroundPlaneComparator. + * \param[in] plane_coeff_d a reference to a vector of d coefficients of plane equations. Must be the same size as the input cloud and input normals. a, b, and c coefficients are in the input normals. + */ + GroundPlaneComparator (shared_ptr >& plane_coeff_d) + : normals_ () + , plane_coeff_d_ (plane_coeff_d) + , angular_threshold_ (std::cos (pcl::deg2rad (3.0f))) + , distance_threshold_ (0.1f) + , depth_dependent_ (true) + , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f)) + , road_angular_threshold_ ( std::cos(pcl::deg2rad (40.0f))) + , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0)) + { + } + + /** \brief Destructor for GroundPlaneComparator. */ + + ~GroundPlaneComparator () + { + } + /** \brief Provide the input cloud. + * \param[in] cloud the input point cloud. + */ + void + setInputCloud (const PointCloudConstPtr& cloud) override + { + input_ = cloud; + } + + /** \brief Provide a pointer to the input normals. + * \param[in] normals the input normal cloud. + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) + { + normals_ = normals; + } + + /** \brief Get the input normals. */ + inline PointCloudNConstPtr + getInputNormals () const + { + return (normals_); + } + + /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud. + * \param[in] plane_coeff_d a pointer to the plane coefficients. + */ + void + setPlaneCoeffD (shared_ptr >& plane_coeff_d) + { + plane_coeff_d_ = plane_coeff_d; + } + + /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud. + * \param[in] plane_coeff_d a pointer to the plane coefficients. + */ + void + setPlaneCoeffD (std::vector& plane_coeff_d) + { + plane_coeff_d_ = boost::make_shared >(plane_coeff_d); + } + + /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */ + const std::vector& + getPlaneCoeffD () const + { + return (*plane_coeff_d_); + } + + /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. + * \param[in] angular_threshold the tolerance in radians + */ + virtual void + setAngularThreshold (float angular_threshold) + { + angular_threshold_ = std::cos (angular_threshold); + } + + /** \brief Set the tolerance in radians for difference in normal direction between a point and the expected ground normal. + * \param[in] angular_threshold the + */ + virtual void + setGroundAngularThreshold (float angular_threshold) + { + road_angular_threshold_ = std::cos (angular_threshold); + } + + /** \brief Set the expected ground plane normal with respect to the sensor. Pixels labeled as ground must be within ground_angular_threshold radians of this normal to be labeled as ground. + * \param[in] normal The normal direction of the expected ground plane. + */ + void + setExpectedGroundNormal (Eigen::Vector3f normal) + { + desired_road_axis_ = normal; + } + + + /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ + inline float + getAngularThreshold () const + { + return (std::acos (angular_threshold_) ); + } + + /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. + * \param[in] distance_threshold the tolerance in meters (at 1m) + * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false) + */ + void + setDistanceThreshold (float distance_threshold, + bool depth_dependent = false) + { + distance_threshold_ = distance_threshold; + depth_dependent_ = depth_dependent; + } + + /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */ + inline float + getDistanceThreshold () const + { + return distance_threshold_; + } + + /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, + * and the difference between the d component of the normals is less than distance threshold, else false + * \param idx1 The first index for the comparison + * \param idx2 The second index for the comparison + */ + bool + compare (int idx1, int idx2) const override + { + // Normal must be similar to neighbor + // Normal must be similar to expected normal + float threshold = distance_threshold_; + if (depth_dependent_) + { + Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); + + float z = vec.dot (z_axis_); + threshold *= z * z; + } + + return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) && + (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ )); + + // Euclidean proximity of neighbors does not seem to be required -- pixel adjacency handles this well enough + //return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) && + // (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) && + // (pcl::euclideanDistance (input_->points[idx1], input_->points[idx2]) < distance_threshold_ )); + } + + protected: + PointCloudNConstPtr normals_; + shared_ptr > plane_coeff_d_; + float angular_threshold_; + float road_angular_threshold_; + float distance_threshold_; + bool depth_dependent_; + Eigen::Vector3f z_axis_; + Eigen::Vector3f desired_road_axis_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp new file mode 100755 index 0000000..47335f7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp @@ -0,0 +1,264 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ +#define PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ApproximateProgressiveMorphologicalFilter::ApproximateProgressiveMorphologicalFilter () : + max_window_size_ (33), + slope_ (0.7f), + max_distance_ (10.0f), + initial_distance_ (0.15f), + cell_size_ (1.0f), + base_ (2.0f), + exponential_ (true), + threads_ (0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ApproximateProgressiveMorphologicalFilter::~ApproximateProgressiveMorphologicalFilter () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ApproximateProgressiveMorphologicalFilter::extract (std::vector& ground) +{ + bool segmentation_is_possible = initCompute (); + if (!segmentation_is_possible) + { + deinitCompute (); + return; + } + + // Compute the series of window sizes and height thresholds + std::vector height_thresholds; + std::vector window_sizes; + std::vector half_sizes; + int iteration = 0; + int half_size = 0.0f; + float window_size = 0.0f; + float height_threshold = 0.0f; + + while (window_size < max_window_size_) + { + // Determine the initial window size. + if (exponential_) + half_size = static_cast (std::pow (static_cast (base_), iteration)); + else + half_size = (iteration+1) * base_; + + window_size = 2 * half_size + 1; + + // Calculate the height threshold to be used in the next iteration. + if (iteration == 0) + height_threshold = initial_distance_; + else + height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_; + + // Enforce max distance on height threshold + if (height_threshold > max_distance_) + height_threshold = max_distance_; + + half_sizes.push_back (half_size); + window_sizes.push_back (window_size); + height_thresholds.push_back (height_threshold); + + iteration++; + } + + // setup grid based on scale and extents + Eigen::Vector4f global_max, global_min; + pcl::getMinMax3D (*input_, global_min, global_max); + + float xextent = global_max.x () - global_min.x (); + float yextent = global_max.y () - global_min.y (); + + int rows = static_cast (std::floor (yextent / cell_size_) + 1); + int cols = static_cast (std::floor (xextent / cell_size_) + 1); + + Eigen::MatrixXf A (rows, cols); + A.setConstant (std::numeric_limits::quiet_NaN ()); + + Eigen::MatrixXf Z (rows, cols); + Z.setConstant (std::numeric_limits::quiet_NaN ()); + + Eigen::MatrixXf Zf (rows, cols); + Zf.setConstant (std::numeric_limits::quiet_NaN ()); + +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < (int)input_->points.size (); ++i) + { + // ...then test for lower points within the cell + PointT p = input_->points[i]; + int row = std::floor((p.y - global_min.y ()) / cell_size_); + int col = std::floor((p.x - global_min.x ()) / cell_size_); + + if (p.z < A (row, col) || std::isnan (A (row, col))) + { + A (row, col) = p.z; + } + } + + // Ground indices are initially limited to those points in the input cloud we + // wish to process + ground = *indices_; + + // Progressively filter ground returns using morphological open + for (std::size_t i = 0; i < window_sizes.size (); ++i) + { + PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f, half size = %d)...", + i, height_thresholds[i], window_sizes[i], half_sizes[i]); + + // Limit filtering to those points currently considered ground returns + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::copyPointCloud (*input_, ground, *cloud); + + // Apply the morphological opening operation at the current window size. +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int row = 0; row < rows; ++row) + { + int rs, re; + rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i]; + re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i]; + + for (int col = 0; col < cols; ++col) + { + int cs, ce; + cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i]; + ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i]; + + float min_coeff = std::numeric_limits::max (); + + for (int j = rs; j < (re + 1); ++j) + { + for (int k = cs; k < (ce + 1); ++k) + { + if (A (j, k) != std::numeric_limits::quiet_NaN ()) + { + if (A (j, k) < min_coeff) + min_coeff = A (j, k); + } + } + } + + if (min_coeff != std::numeric_limits::max ()) + Z(row, col) = min_coeff; + } + } + +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int row = 0; row < rows; ++row) + { + int rs, re; + rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i]; + re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i]; + + for (int col = 0; col < cols; ++col) + { + int cs, ce; + cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i]; + ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i]; + + float max_coeff = -std::numeric_limits::max (); + + for (int j = rs; j < (re + 1); ++j) + { + for (int k = cs; k < (ce + 1); ++k) + { + if (Z (j, k) != std::numeric_limits::quiet_NaN ()) + { + if (Z (j, k) > max_coeff) + max_coeff = Z (j, k); + } + } + } + + if (max_coeff != -std::numeric_limits::max ()) + Zf (row, col) = max_coeff; + } + } + + // Find indices of the points whose difference between the source and + // filtered point clouds is less than the current height threshold. + std::vector pt_indices; + for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx) + { + PointT p = cloud->points[p_idx]; + int erow = static_cast (std::floor ((p.y - global_min.y ()) / cell_size_)); + int ecol = static_cast (std::floor ((p.x - global_min.x ()) / cell_size_)); + + float diff = p.z - Zf (erow, ecol); + if (diff < height_thresholds[i]) + pt_indices.push_back (ground[p_idx]); + } + + A.swap (Zf); + + // Ground is now limited to pt_indices + ground.swap (pt_indices); + + PCL_DEBUG ("ground now has %d points\n", ground.size ()); + } + + deinitCompute (); +} + + +#define PCL_INSTANTIATE_ApproximateProgressiveMorphologicalFilter(T) template class pcl::ApproximateProgressiveMorphologicalFilter; + +#endif // PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/conditional_euclidean_clustering.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/conditional_euclidean_clustering.hpp new file mode 100755 index 0000000..d1634b5 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/conditional_euclidean_clustering.hpp @@ -0,0 +1,144 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_ +#define PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_ + +#include + +template void +pcl::ConditionalEuclideanClustering::segment (pcl::IndicesClusters &clusters) +{ + // Prepare output (going to use push_back) + clusters.clear (); + if (extract_removed_clusters_) + { + small_clusters_->clear (); + large_clusters_->clear (); + } + + // Validity checks + if (!initCompute () || input_->points.empty () || indices_->empty () || !condition_function_) + return; + + // Initialize the search class + if (!searcher_) + { + if (input_->isOrganized ()) + searcher_.reset (new pcl::search::OrganizedNeighbor ()); + else + searcher_.reset (new pcl::search::KdTree ()); + } + searcher_->setInputCloud (input_, indices_); + + // Temp variables used by search class + std::vector nn_indices; + std::vector nn_distances; + + // Create a bool vector of processed point indices, and initialize it to false + // Need to have it contain all possible points because radius search can not return indices into indices + std::vector processed (input_->points.size (), false); + + // Process all points indexed by indices_ + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Has this point been processed before? + if ((*indices_)[iii] == -1 || processed[(*indices_)[iii]]) + continue; + + // Set up a new growing cluster + std::vector current_cluster; + int cii = 0; // cii = cluster indices iterator + + // Add the point to the cluster + current_cluster.push_back ((*indices_)[iii]); + processed[(*indices_)[iii]] = true; + + // Process the current cluster (it can be growing in size as it is being processed) + while (cii < static_cast (current_cluster.size ())) + { + // Search for neighbors around the current seed point of the current cluster + if (searcher_->radiusSearch (input_->points[current_cluster[cii]], cluster_tolerance_, nn_indices, nn_distances) < 1) + { + cii++; + continue; + } + + // Process the neighbors + for (int nii = 1; nii < static_cast (nn_indices.size ()); ++nii) // nii = neighbor indices iterator + { + // Has this point been processed before? + if (nn_indices[nii] == -1 || processed[nn_indices[nii]]) + continue; + + // Validate if condition holds + if (condition_function_ (input_->points[current_cluster[cii]], input_->points[nn_indices[nii]], nn_distances[nii])) + { + // Add the point to the cluster + current_cluster.push_back (nn_indices[nii]); + processed[nn_indices[nii]] = true; + } + } + cii++; + } + + // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range + if (extract_removed_clusters_ || + (static_cast (current_cluster.size ()) >= min_cluster_size_ && + static_cast (current_cluster.size ()) <= max_cluster_size_)) + { + pcl::PointIndices pi; + pi.header = input_->header; + pi.indices.resize (current_cluster.size ()); + for (int ii = 0; ii < static_cast (current_cluster.size ()); ++ii) // ii = indices iterator + pi.indices[ii] = current_cluster[ii]; + + if (extract_removed_clusters_ && static_cast (current_cluster.size ()) < min_cluster_size_) + small_clusters_->push_back (pi); + else if (extract_removed_clusters_ && static_cast (current_cluster.size ()) > max_cluster_size_) + large_clusters_->push_back (pi); + else + clusters.push_back (pi); + } + } + + deinitCompute (); +} + +#define PCL_INSTANTIATE_ConditionalEuclideanClustering(T) template class PCL_EXPORTS pcl::ConditionalEuclideanClustering; + +#endif // PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/cpc_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/cpc_segmentation.hpp new file mode 100755 index 0000000..b0f4037 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/cpc_segmentation.hpp @@ -0,0 +1,374 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_ +#define PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_ + +#include + +template +pcl::CPCSegmentation::CPCSegmentation () : + max_cuts_ (20), + min_segment_size_for_cutting_ (400), + min_cut_score_ (0.16), + use_local_constrains_ (true), + use_directed_weights_ (true), + ransac_itrs_ (10000) +{ +} + +template +pcl::CPCSegmentation::~CPCSegmentation () +{ +} + +template void +pcl::CPCSegmentation::segment () +{ + if (supervoxels_set_) + { + // Calculate for every Edge if the connection is convex or invalid + // This effectively performs the segmentation. + calculateConvexConnections (sv_adjacency_list_); + + // Correct edge relations using extended convexity definition if k>0 + applyKconvexity (k_factor_); + + // Determine whether to use cutting planes + doGrouping (); + + grouping_data_valid_ = true; + + applyCuttingPlane (max_cuts_); + + // merge small segments + mergeSmallSegments (); + } + else + PCL_WARN ("[pcl::CPCSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n"); +} + +template void +pcl::CPCSegmentation::applyCuttingPlane (std::uint32_t depth_levels_left) +{ + using SegLabel2ClusterMap = std::map::Ptr>; + + pcl::console::print_info ("Cutting at level %d (maximum %d)\n", max_cuts_ - depth_levels_left + 1, max_cuts_); + // stop if we reached the 0 level + if (depth_levels_left <= 0) + return; + + pcl::IndicesPtr support_indices (new pcl::Indices); + SegLabel2ClusterMap seg_to_edge_points_map; + std::map > seg_to_edgeIDs_map; + EdgeIterator edge_itr, edge_itr_end, next_edge; + boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_); + for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge) + { + next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge + std::uint32_t source_sv_label = sv_adjacency_list_[boost::source (*edge_itr, sv_adjacency_list_)]; + std::uint32_t target_sv_label = sv_adjacency_list_[boost::target (*edge_itr, sv_adjacency_list_)]; + + std::uint32_t source_segment_label = sv_label_to_seg_label_map_[source_sv_label]; + std::uint32_t target_segment_label = sv_label_to_seg_label_map_[target_sv_label]; + + // do not process edges which already split two segments + if (source_segment_label != target_segment_label) + continue; + + // if edge has been used for cutting already do not use it again + if (sv_adjacency_list_[*edge_itr].used_for_cutting) + continue; + // get centroids of vertices + const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_; + const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_; + + // stores the information about the edge cloud (used for the weighted ransac) + // we use the normal to express the direction of the connection + // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave + WeightSACPointType edge_centroid; + edge_centroid.getVector3fMap () = (source_centroid.getVector3fMap () + target_centroid.getVector3fMap ()) / 2; + + // we use the normal to express the direction of the connection! + edge_centroid.getNormalVector3fMap () = (target_centroid.getVector3fMap () - source_centroid.getVector3fMap ()).normalized (); + + // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave + edge_centroid.intensity = sv_adjacency_list_[*edge_itr].is_convex ? -sv_adjacency_list_[*edge_itr].normal_difference : sv_adjacency_list_[*edge_itr].normal_difference; + if (seg_to_edge_points_map.find (source_segment_label) == seg_to_edge_points_map.end ()) + { + seg_to_edge_points_map[source_segment_label] = pcl::PointCloud::Ptr (new pcl::PointCloud ()); + } + seg_to_edge_points_map[source_segment_label]->push_back (edge_centroid); + seg_to_edgeIDs_map[source_segment_label].push_back (*edge_itr); + } + bool cut_found = false; + // do the following processing for each segment separately + for (const auto &seg_to_edge_points : seg_to_edge_points_map) + { + // if too small do not process + if (seg_to_edge_points.second->size () < min_segment_size_for_cutting_) + { + continue; + } + + std::vector weights; + weights.resize (seg_to_edge_points.second->size ()); + for (std::size_t cp = 0; cp < seg_to_edge_points.second->size (); ++cp) + { + float& cur_weight = seg_to_edge_points.second->points[cp].intensity; + cur_weight = cur_weight < concavity_tolerance_threshold_ ? 0 : 1; + weights[cp] = cur_weight; + } + + pcl::PointCloud::Ptr edge_cloud_cluster = seg_to_edge_points.second; + pcl::SampleConsensusModelPlane::Ptr model_p (new pcl::SampleConsensusModelPlane (edge_cloud_cluster)); + + WeightedRandomSampleConsensus weight_sac (model_p, seed_resolution_, true); + + weight_sac.setWeights (weights, use_directed_weights_); + weight_sac.setMaxIterations (ransac_itrs_); + + // if not enough inliers are found + if (!weight_sac.computeModel ()) + { + continue; + } + + Eigen::VectorXf model_coefficients; + weight_sac.getModelCoefficients (model_coefficients); + + model_coefficients[3] += std::numeric_limits::epsilon (); + + weight_sac.getInliers (*support_indices); + + // the support_indices which are actually cut (if not locally constrain: cut_support_indices = support_indices + pcl::Indices cut_support_indices; + + if (use_local_constrains_) + { + Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + // Cut the connections. + // We only iterate through the points which are within the support (when we are local, otherwise all points in the segment). + // We also just actually cut when the edge goes through the plane. This is why we check the planedistance + std::vector cluster_indices; + pcl::EuclideanClusterExtraction euclidean_clusterer; + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (edge_cloud_cluster); + euclidean_clusterer.setClusterTolerance (seed_resolution_); + euclidean_clusterer.setMinClusterSize (1); + euclidean_clusterer.setMaxClusterSize (25000); + euclidean_clusterer.setSearchMethod (tree); + euclidean_clusterer.setInputCloud (edge_cloud_cluster); + euclidean_clusterer.setIndices (support_indices); + euclidean_clusterer.extract (cluster_indices); +// sv_adjacency_list_[seg_to_edgeID_map[seg_to_edge_points.first][point_index]].used_for_cutting = true; + + for (const auto &cluster_index : cluster_indices) + { + // get centroids of vertices + int cluster_concave_pts = 0; + float cluster_score = 0; +// std::cout << "Cluster has " << cluster_indices[cc].indices.size () << " points" << std::endl; + for (const int ¤t_index : cluster_index.indices) + { + double index_score = weights[current_index]; + if (use_directed_weights_) + index_score *= 1.414 * (std::abs (plane_normal.dot (edge_cloud_cluster->at (current_index).getNormalVector3fMap ()))); + cluster_score += index_score; + if (weights[current_index] > 0) + ++cluster_concave_pts; + } + // check if the score is below the threshold. If that is the case this segment should not be split + cluster_score /= cluster_index.indices.size (); +// std::cout << "Cluster score: " << cluster_score << std::endl; + if (cluster_score >= min_cut_score_) + { + cut_support_indices.insert (cut_support_indices.end (), cluster_index.indices.begin (), cluster_index.indices.end ()); + } + } + if (cut_support_indices.empty ()) + { +// std::cout << "Could not find planes which exceed required minimum score (threshold " << min_cut_score_ << "), not cutting" << std::endl; + continue; + } + } + else + { + double current_score = weight_sac.getBestScore (); + cut_support_indices = *support_indices; + // check if the score is below the threshold. If that is the case this segment should not be split + if (current_score < min_cut_score_) + { +// std::cout << "Score too low, no cutting" << std::endl; + continue; + } + } + + int number_connections_cut = 0; + for (const int &point_index : cut_support_indices) + { + if (use_clean_cutting_) + { + // skip edges where both centroids are on one side of the cutting plane + std::uint32_t source_sv_label = sv_adjacency_list_[boost::source (seg_to_edgeIDs_map[seg_to_edge_points.first][point_index], sv_adjacency_list_)]; + std::uint32_t target_sv_label = sv_adjacency_list_[boost::target (seg_to_edgeIDs_map[seg_to_edge_points.first][point_index], sv_adjacency_list_)]; + // get centroids of vertices + const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_; + const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_; + // this makes a clean cut + if (pcl::pointToPlaneDistanceSigned (source_centroid, model_coefficients) * pcl::pointToPlaneDistanceSigned (target_centroid, model_coefficients) > 0) + { + continue; + } + } + sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].used_for_cutting = true; + if (sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid) + { + ++number_connections_cut; + sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid = false; + } + } +// std::cout << "We cut " << number_connections_cut << " connections" << std::endl; + if (number_connections_cut > 0) + cut_found = true; + } + + // if not cut has been performed we can stop the recursion + if (cut_found) + { + doGrouping (); + --depth_levels_left; + applyCuttingPlane (depth_levels_left); + } + else + pcl::console::print_info ("Could not find any more cuts, stopping recursion\n"); +} + +/******************************************* Directional weighted RANSAC definitions ******************************************************************/ + + +template bool +pcl::CPCSegmentation::WeightedRandomSampleConsensus::computeModel (int) +{ + // Warn and exit if no threshold was set + if (threshold_ == std::numeric_limits::max ()) + { + PCL_ERROR ("[pcl::CPCSegmentation::WeightedRandomSampleConsensus::computeModel] No threshold set!\n"); + return (false); + } + + iterations_ = 0; + best_score_ = -std::numeric_limits::max (); + + std::vector selection; + Eigen::VectorXf model_coefficients; + + unsigned skipped_count = 0; + // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! + const unsigned max_skip = max_iterations_ * 10; + + // Iterate + while (iterations_ < max_iterations_ && skipped_count < max_skip) + { + // Get X samples which satisfy the model criteria and which have a weight > 0 + sac_model_->setIndices (model_pt_indices_); + sac_model_->getSamples (iterations_, selection); + + if (selection.empty ()) + { + PCL_ERROR ("[pcl::CPCSegmentation::WeightedRandomSampleConsensus::computeModel] No samples could be selected!\n"); + 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; + } + // weight distances to get the score (only using connected inliers) + sac_model_->setIndices (full_cloud_pt_indices_); + + pcl::IndicesPtr current_inliers (new pcl::Indices); + sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers); + double current_score = 0; + Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]); + for (const int ¤t_index : *current_inliers) + { + double index_score = weights_[current_index]; + if (use_directed_weights_) + // the sqrt(2) factor was used in the paper and was meant for making the scores better comparable between directed and undirected weights + index_score *= 1.414 * (std::abs (plane_normal.dot (point_cloud_ptr_->at (current_index).getNormalVector3fMap ()))); + + current_score += index_score; + } + // normalize by the total number of inliers + current_score /= current_inliers->size (); + + // Better match ? + if (current_score > best_score_) + { + best_score_ = current_score; + // Save the current model/inlier/coefficients selection as being the best so far + model_ = selection; + model_coefficients_ = model_coefficients; + } + + ++iterations_; + PCL_DEBUG ("[pcl::CPCSegmentation::WeightedRandomSampleConsensus::computeModel] Trial %d (max %d): score is %f (best is: %f so far).\n", iterations_, max_iterations_, current_score, best_score_); + if (iterations_ > max_iterations_) + { + PCL_DEBUG ("[pcl::CPCSegmentation::WeightedRandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n"); + break; + } + } +// std::cout << "Took us " << iterations_ - 1 << " iterations" << std::endl; + PCL_DEBUG ("[pcl::CPCSegmentation::WeightedRandomSampleConsensus::computeModel] Model: %lu size, %f score.\n", model_.size (), best_score_); + + if (model_.empty ()) + { + inliers_.clear (); + return (false); + } + + // Get the set of inliers that correspond to the best model found so far + sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_); + return (true); +} + +#endif // PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/crf_normal_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/crf_normal_segmentation.hpp new file mode 100755 index 0000000..360749e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/crf_normal_segmentation.hpp @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#ifndef PCL_CRF_NORMAL_SEGMENTATION_HPP_ +#define PCL_CRF_NORMAL_SEGMENTATION_HPP_ + +#include + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::CrfNormalSegmentation::CrfNormalSegmentation () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::CrfNormalSegmentation::~CrfNormalSegmentation () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfNormalSegmentation::setCloud (typename pcl::PointCloud::Ptr ) +{ +/* + if (cloud_for_segmentation_ != 0) + cloud_for_segmentation_.reset (); + + cloud_for_segmentation_ = input_cloud; +*/ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfNormalSegmentation::segmentPoints () +{ +} + +#define PCL_INSTANTIATE_CrfNormalSegmentation(T) template class pcl::CrfNormalSegmentation; + +#endif // PCL_CRF_NORMAL_SEGMENTATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/crf_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/crf_segmentation.hpp new file mode 100755 index 0000000..41ec799 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/crf_segmentation.hpp @@ -0,0 +1,602 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#ifndef PCL_CRF_SEGMENTATION_HPP_ +#define PCL_CRF_SEGMENTATION_HPP_ + +#include + +#include + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::CrfSegmentation::CrfSegmentation () : + voxel_grid_ (), + input_cloud_ (new pcl::PointCloud), + normal_cloud_ (new pcl::PointCloud), + filtered_cloud_ (new pcl::PointCloud), + filtered_anno_ (new pcl::PointCloud), + filtered_normal_ (new pcl::PointCloud), + voxel_grid_leaf_size_ (Eigen::Vector4f (0.001f, 0.001f, 0.001f, 0.0f)) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::CrfSegmentation::~CrfSegmentation () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::setInputCloud (typename pcl::PointCloud::Ptr input_cloud) +{ + input_cloud_ = input_cloud; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::setAnnotatedCloud (typename pcl::PointCloud::Ptr anno_cloud) +{ + anno_cloud_ = anno_cloud; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::setNormalCloud (typename pcl::PointCloud::Ptr normal_cloud) +{ + normal_cloud_ = normal_cloud; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::setVoxelGridLeafSize (const float x, const float y, const float z) +{ + voxel_grid_leaf_size_.x () = x; + voxel_grid_leaf_size_.y () = y; + voxel_grid_leaf_size_.z () = z; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::setSmoothnessKernelParameters (const float sx, const float sy, const float sz, + const float w) +{ + smoothness_kernel_param_[0] = sx; + smoothness_kernel_param_[1] = sy; + smoothness_kernel_param_[2] = sz; + smoothness_kernel_param_[3] = w; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::setAppearanceKernelParameters (float sx, float sy, float sz, + float sr, float sg, float sb, + float w) +{ + appearance_kernel_param_[0] = sx; + appearance_kernel_param_[1] = sy; + appearance_kernel_param_[2] = sz; + appearance_kernel_param_[3] = sr; + appearance_kernel_param_[4] = sg; + appearance_kernel_param_[5] = sb; + appearance_kernel_param_[6] = w; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::setSurfaceKernelParameters (float sx, float sy, float sz, + float snx, float sny, float snz, + float w) +{ + surface_kernel_param_[0] = sx; + surface_kernel_param_[1] = sy; + surface_kernel_param_[2] = sz; + surface_kernel_param_[3] = snx; + surface_kernel_param_[4] = sny; + surface_kernel_param_[5] = snz; + surface_kernel_param_[6] = w; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::createVoxelGrid () +{ + // Filter the input cloud + // Set the voxel grid input cloud + voxel_grid_.setInputCloud (input_cloud_); + // Set the voxel grid leaf size + voxel_grid_.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () ); + // Only downsample XYZ (if this is set to false, color values set to 0) + voxel_grid_.setDownsampleAllData (true); + // Save leaf information + //voxel_grid_.setSaveLeafLayout (true); + // apply the filter + voxel_grid_.filter (*filtered_cloud_); + + // Filter the annotated cloud + if (!anno_cloud_->points.empty ()) + { + pcl::VoxelGridLabel vg; + + vg.setInputCloud (anno_cloud_); + // Set the voxel grid leaf size + vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () ); + // Only downsample XYZ + vg.setDownsampleAllData (true); + // Save leaf information + //vg.setSaveLeafLayout (false); + // apply the filter + vg.filter (*filtered_anno_); + } + + // Filter the annotated cloud + if (!normal_cloud_->points.empty ()) + { + pcl::VoxelGrid vg; + vg.setInputCloud (normal_cloud_); + // Set the voxel grid leaf size + vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () ); + // Only downsample XYZ + vg.setDownsampleAllData (true); + // Save leaf information + //vg.setSaveLeafLayout (false); + // apply the filter + vg.filter (*filtered_normal_); + } + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::createDataVectorFromVoxelGrid () +{ + // get the dimension of the voxel grid + //Eigen::Vector3i min_b, max_b; + //min_b = voxel_grid_.getMinBoxCoordinates (); + //max_b = voxel_grid_.getMaxBoxCoordinates (); + + //std::cout << "min_b: " << min_b.x () << " " << min_b.y () << " " << min_b.z () << std::endl; + //std::cout << "max_b: " << max_b.x () << " " << max_b.y () << " " << max_b.z () << std::endl; + + // compute the voxel grid dimensions + //dim_.x () = std::abs (max_b.x () - min_b.x ()); + //dim_.y () = std::abs (max_b.y () - min_b.y ()); + //dim_.z () = std::abs (max_b.z () - min_b.z ()); + + //std::cout << dim_.x () * dim_.y () * dim_.z () << std::endl; + + // reserve the space for the data vector + //data_.reserve (dim_.x () * dim_.y () * dim_.z ()); + +/* + std::vector > data; + std::vector > color; + // fill the data vector + for (int kk = min_b.z (), k = 0; kk <= max_b.z (); kk++, k++) + { + for (int jj = min_b.y (), j = 0; jj <= max_b.y (); jj++, j++) + { + for (int ii = min_b.x (), i = 0; ii <= max_b.x (); ii++, i++) + { + Eigen::Vector3i ijk (ii, jj, kk); + int index = voxel_grid_.getCentroidIndexAt (ijk); + if (index != -1) + { + data_.push_back (Eigen::Vector3i (i, j, k)); + color_.push_back (input_cloud_->points[index].getRGBVector3i ()); + } + } + } + } +*/ + + +/* + // get the size of the input fields + std::vector< pcl::PCLPointField > fields; + pcl::getFields (*input_cloud_, fields); + + for (int i = 0; i < fields.size (); i++) + std::cout << fields[i] << std::endl; +*/ + + + // reserve space for the data vector + data_.resize (filtered_cloud_->points.size ()); + + std::vector< pcl::PCLPointField > fields; + // check if we have color data + bool color_data = false; + int rgba_index = -1; + rgba_index = pcl::getFieldIndex ("rgb", fields); + if (rgba_index == -1) + rgba_index = pcl::getFieldIndex ("rgba", fields); + if (rgba_index >= 0) + { + color_data = true; + color_.resize (filtered_cloud_->points.size ()); + } + + +/* + // check if we have normal data + bool normal_data = false; + int normal_index = -1; + rgba_index = pcl::getFieldIndex ("normal_x", fields); + if (rgba_index >= 0) + { + normal_data = true; + normal_.resize (filtered_cloud_->points.size ()); + } +*/ + + // fill the data vector + for (std::size_t i = 0; i < filtered_cloud_->points.size (); i++) + { + Eigen::Vector3f p (filtered_anno_->points[i].x, + filtered_anno_->points[i].y, + filtered_anno_->points[i].z); + Eigen::Vector3i c = voxel_grid_.getGridCoordinates (p.x (), p.y (), p.y ()); + data_[i] = c; + + if (color_data) + { + std::uint32_t rgb = *reinterpret_cast(&filtered_cloud_->points[i].rgba); + std::uint8_t r = (rgb >> 16) & 0x0000ff; + std::uint8_t g = (rgb >> 8) & 0x0000ff; + std::uint8_t b = (rgb) & 0x0000ff; + color_[i] = Eigen::Vector3i (r, g, b); + } + +/* + if (normal_data) + { + float n_x = filtered_cloud_->points[i].normal_x; + float n_y = filtered_cloud_->points[i].normal_y; + float n_z = filtered_cloud_->points[i].normal_z; + normal_[i] = Eigen::Vector3f (n_x, n_y, n_z); + } +*/ + } + + normal_.resize (filtered_normal_->points.size ()); + for (std::size_t i = 0; i < filtered_normal_->points.size (); i++) + { + float n_x = filtered_normal_->points[i].normal_x; + float n_y = filtered_normal_->points[i].normal_y; + float n_z = filtered_normal_->points[i].normal_z; + normal_[i] = Eigen::Vector3f (n_x, n_y, n_z); + } + + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::createUnaryPotentials (std::vector &unary, + std::vector &labels, + unsigned int n_labels) +{ + /* initialize random seed: */ + srand ( static_cast (time (nullptr)) ); + //srand ( time (NULL) ); + + // Certainty that the groundtruth is correct + const float GT_PROB = 0.9f; + const float u_energy = -std::log ( 1.0f / static_cast (n_labels) ); + const float n_energy = -std::log ( (1.0f - GT_PROB) / static_cast(n_labels - 1) ); + const float p_energy = -std::log ( GT_PROB ); + + for (std::size_t k = 0; k < filtered_anno_->points.size (); k++) + { + int label = filtered_anno_->points[k].label; + + if (labels.empty () && label > 0) + labels.push_back (label); + + // add color to the color vector if not added yet + for (int c_idx = 0; c_idx < static_cast (labels.size ()) ; c_idx++) + { + if (labels[c_idx] == label) + break; + + if (c_idx == static_cast(labels.size () -1) && label > 0) + { + if (labels.size () < n_labels) + labels.push_back (label); + else + label = 0; + } + } + + // set the engeries for the labels + std::size_t u_idx = k * n_labels; + if (label > 0) + { + for (std::size_t i = 0; i < n_labels; i++) + unary[u_idx + i] = n_energy; + unary[u_idx + labels.size ()] = p_energy; + + if (label == 1) + { + const float PROB = 0.2f; + const float n_energy2 = -std::log ( (1.0f - PROB) / static_cast(n_labels - 1) ); + const float p_energy2 = -std::log ( PROB ); + + for (std::size_t i = 0; i < n_labels; i++) + unary[u_idx + i] = n_energy2; + unary[u_idx + labels.size ()] = p_energy2; + } + + } + else + { + for (std::size_t i = 0; i < n_labels; i++) + unary[u_idx + i] = u_energy; + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CrfSegmentation::segmentPoints (pcl::PointCloud &output) +{ + // create the voxel grid + createVoxelGrid (); + std::cout << "create Voxel Grid - DONE" << std::endl; + + // create the data Vector + createDataVectorFromVoxelGrid (); + std::cout << "create Data Vector from Voxel Grid - DONE" << std::endl; + + // number of labels + const int n_labels = 10; + // number of data points + int N = static_cast (data_.size ()); + + // create unary potentials + std::vector labels; + std::vector unary; + if (!anno_cloud_->points.empty ()) + { + unary.resize (N * n_labels); + createUnaryPotentials (unary, labels, n_labels); + + + std::cout << "labels size: " << labels.size () << std::endl; + for (const int &label : labels) + { + std::cout << label << std::endl; + } + + } + std::cout << "create unary potentials - DONE" << std::endl; + + +/* + pcl::PointCloud tmp_cloud_OLD; + tmp_cloud_OLD = *filtered_anno_; + + // Setup the CRF model + DenseCRF2D crfOLD(N, 1, n_labels); + + float * unaryORI = new float[N*n_labels]; + for (int i = 0; i < N*n_labels; i++) + unaryORI[i] = unary[i]; + crfOLD.setUnaryEnergy( unaryORI ); + + float * pos = new float[N*3]; + for (int i = 0; i < N; i++) + { + pos[i * 3] = data_[i].x (); + pos[i * 3 +1] = data_[i].y (); + pos[i * 3 +2] = data_[i].z (); + } + crfOLD.addPairwiseGaussian( pos, 3, 3, 3, 2.0 ); + + float * col = new float[N*3]; + for (int i = 0; i < N; i++) + { + col[i * 3] = color_[i].x (); + col[i * 3 +1] = color_[i].y (); + col[i * 3 +2] = color_[i].z (); + } + crfOLD.addPairwiseBilateral(pos, col, 20, 20, 20, 10, 10, 10, 1.3 ); + + short * map = new short[N]; + crfOLD.map(10, map); + + for (std::size_t i = 0; i < N; i++) + { + tmp_cloud_OLD.points[i].label = map[i]; + } + + +*/ + + //float * resultOLD = new float[N*n_labels]; + //crfOLD.inference (10, resultOLD); + + //std::vector baryOLD; + //crfOLD.getBarycentric (0, baryOLD); + //std::vector featuresOLD; + //crfOLD.getFeature (1, featuresOLD); + +/* + for(int i = 0; i < 25; i++) + { + std::cout << baryOLD[i] << std::endl; + } +*/ + + + // create the output cloud + //output = *filtered_anno_; + + + + // ----------------------------------// + // -------- -------------------// + + pcl::PointCloud tmp_cloud; + tmp_cloud = *filtered_anno_; + + // create dense CRF + DenseCrf crf (N, n_labels); + + // set the unary potentials + crf.setUnaryEnergy (unary); + + // set the data vector + crf.setDataVector (data_); + + // set the color vector + crf.setColorVector (color_); + + std::cout << "create dense CRF - DONE" << std::endl; + + + // add the smoothness kernel + crf.addPairwiseGaussian (smoothness_kernel_param_[0], + smoothness_kernel_param_[1], + smoothness_kernel_param_[2], + smoothness_kernel_param_[3]); + std::cout << "add smoothness kernel - DONE" << std::endl; + + // add the appearance kernel + crf.addPairwiseBilateral (appearance_kernel_param_[0], + appearance_kernel_param_[1], + appearance_kernel_param_[2], + appearance_kernel_param_[3], + appearance_kernel_param_[4], + appearance_kernel_param_[5], + appearance_kernel_param_[6]); + std::cout << "add appearance kernel - DONE" << std::endl; + + crf.addPairwiseNormals (data_, normal_, + surface_kernel_param_[0], + surface_kernel_param_[1], + surface_kernel_param_[2], + surface_kernel_param_[3], + surface_kernel_param_[4], + surface_kernel_param_[5], + surface_kernel_param_[6]); + std::cout << "add surface kernel - DONE" << std::endl; + + // map inference + std::vector r (N); + crf.mapInference (n_iterations_, r); + + //std::vector result (N*n_labels); + //crf.inference (n_iterations_, result); + + //std::vector bary; + //crf.getBarycentric (0, bary); + //std::vector features; + //crf.getFeatures (1, features); + + std::cout << "Map inference - DONE" << std::endl; + + // create the output cloud + output = *filtered_anno_; + + for (int i = 0; i < N; i++) + { + output.points[i].label = labels[r[i]]; + } + + +/* + bool c = true; + for (std::size_t i = 0; i < tmp_cloud.points.size (); i++) + { + if (tmp_cloud.points[i].label != tmp_cloud_OLD.points[i].label) + { + + std::cout << "idx: " << i << " = " <; + +#endif // PCL_CRF_SEGMENTATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_clusters.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_clusters.hpp new file mode 100755 index 0000000..17ac604 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_clusters.hpp @@ -0,0 +1,246 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_ +#define PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::extractEuclideanClusters (const PointCloud &cloud, + const typename search::Search::Ptr &tree, + float tolerance, std::vector &clusters, + unsigned int min_pts_per_cluster, + unsigned int max_pts_per_cluster) +{ + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + // Check if the tree is sorted -- if it is we don't need to check the first element + int nn_start_idx = tree->getSortedResults () ? 1 : 0; + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + // Process all points in the indices vector + for (int i = 0; i < static_cast (cloud.points.size ()); ++i) + { + if (processed[i]) + continue; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (i); + + processed[i] = true; + + while (sq_idx < static_cast (seed_queue.size ())) + { + // Search for sq_idx + if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) + { + sq_idx++; + continue; + } + + for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!) + { + if (nn_indices[j] == -1 || processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + // Perform a simple Euclidean clustering + seed_queue.push_back (nn_indices[j]); + processed[nn_indices[j]] = true; + } + + sq_idx++; + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + pcl::PointIndices r; + r.indices.resize (seed_queue.size ()); + for (std::size_t j = 0; j < seed_queue.size (); ++j) + r.indices[j] = seed_queue[j]; + + // These two lines should not be needed: (can anyone confirm?) -FF + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + r.header = cloud.header; + clusters.push_back (r); // We could avoid a copy by working directly in the vector + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/** @todo: fix the return value, make sure the exit is not needed anymore*/ +template void +pcl::extractEuclideanClusters (const PointCloud &cloud, + const std::vector &indices, + const typename search::Search::Ptr &tree, + float tolerance, std::vector &clusters, + unsigned int min_pts_per_cluster, + unsigned int max_pts_per_cluster) +{ + // \note If the tree was created over , we guarantee a 1-1 mapping between what the tree returns + //and indices[i] + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + if (tree->getIndices ()->size () != indices.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ()); + return; + } + // Check if the tree is sorted -- if it is we don't need to check the first element + int nn_start_idx = tree->getSortedResults () ? 1 : 0; + + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + // Process all points in the indices vector + for (const int &index : indices) + { + if (processed[index]) + continue; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (index); + + processed[index] = true; + + while (sq_idx < static_cast (seed_queue.size ())) + { + // Search for sq_idx + int ret = tree->radiusSearch (cloud.points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances); + if( ret == -1) + { + PCL_ERROR("[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n"); + exit(0); + } + if (!ret) + { + sq_idx++; + continue; + } + + for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!) + { + if (nn_indices[j] == -1 || processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + // Perform a simple Euclidean clustering + seed_queue.push_back (nn_indices[j]); + processed[nn_indices[j]] = true; + } + + sq_idx++; + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + pcl::PointIndices r; + r.indices.resize (seed_queue.size ()); + for (std::size_t j = 0; j < seed_queue.size (); ++j) + // This is the only place where indices come into play + r.indices[j] = seed_queue[j]; + + // These two lines should not be needed: (can anyone confirm?) -FF + //r.indices.assign(seed_queue.begin(), seed_queue.end()); + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + r.header = cloud.header; + clusters.push_back (r); // We could avoid a copy by working directly in the vector + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + +template void +pcl::EuclideanClusterExtraction::extract (std::vector &clusters) +{ + if (!initCompute () || + (input_ && input_->points.empty ()) || + (indices_ && indices_->empty ())) + { + clusters.clear (); + return; + } + + // Initialize the spatial locator + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the input dataset to the spatial locator + tree_->setInputCloud (input_, indices_); + extractEuclideanClusters (*input_, *indices_, tree_, static_cast (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_); + + //tree_->setInputCloud (input_); + //extractEuclideanClusters (*input_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_); + + // Sort the clusters based on their size (largest one first) + std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters); + + deinitCompute (); +} + +#define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction; +#define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::extractEuclideanClusters(const pcl::PointCloud &, const typename pcl::search::Search::Ptr &, float , std::vector &, unsigned int, unsigned int); +#define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters(const pcl::PointCloud &, const std::vector &, const typename pcl::search::Search::Ptr &, float , std::vector &, unsigned int, unsigned int); + +#endif // PCL_EXTRACT_CLUSTERS_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_labeled_clusters.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_labeled_clusters.hpp new file mode 100755 index 0000000..92aa157 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_labeled_clusters.hpp @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $id $ + */ + +#ifndef PCL_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_ +#define PCL_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::extractLabeledEuclideanClusters (const PointCloud &cloud, + const typename search::Search::Ptr &tree, + float tolerance, + std::vector > &labeled_clusters, + unsigned int min_pts_per_cluster, + unsigned int max_pts_per_cluster, + unsigned int) +{ + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + + // Process all points in the indices vector + for (int i = 0; i < static_cast (cloud.points.size ()); ++i) + { + if (processed[i]) + continue; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (i); + + processed[i] = true; + + while (sq_idx < static_cast (seed_queue.size ())) + { + // Search for sq_idx + int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits::max()); + if(ret == -1) + PCL_ERROR("radiusSearch on tree came back with error -1"); + if (!ret) + { + sq_idx++; + continue; + } + + for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + if (cloud.points[i].label == cloud.points[nn_indices[j]].label) + { + // Perform a simple Euclidean clustering + seed_queue.push_back (nn_indices[j]); + processed[nn_indices[j]] = true; + } + } + + sq_idx++; + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) + { + pcl::PointIndices r; + r.indices.resize (seed_queue.size ()); + for (std::size_t j = 0; j < seed_queue.size (); ++j) + r.indices[j] = seed_queue[j]; + + std::sort (r.indices.begin (), r.indices.end ()); + r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + + r.header = cloud.header; + labeled_clusters[cloud.points[i].label].push_back (r); // We could avoid a copy by working directly in the vector + } + } +} +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + +template void +pcl::LabeledEuclideanClusterExtraction::extract (std::vector > &labeled_clusters) +{ + if (!initCompute () || + (input_ && input_->points.empty ()) || + (indices_ && indices_->empty ())) + { + labeled_clusters.clear (); + return; + } + + // Initialize the spatial locator + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the input dataset to the spatial locator + tree_->setInputCloud (input_); + extractLabeledEuclideanClusters (*input_, tree_, static_cast (cluster_tolerance_), labeled_clusters, min_pts_per_cluster_, max_pts_per_cluster_, max_label_); + + // Sort the clusters based on their size (largest one first) + for (auto &labeled_cluster : labeled_clusters) + std::sort (labeled_cluster.rbegin (), labeled_cluster.rend (), comparePointClusters); + + deinitCompute (); +} + +#define PCL_INSTANTIATE_LabeledEuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::LabeledEuclideanClusterExtraction; +#define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters(const pcl::PointCloud &, const typename pcl::search::Search::Ptr &, float , std::vector > &, unsigned int, unsigned int, unsigned int); + +#endif // PCL_EXTRACT_CLUSTERS_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_polygonal_prism_data.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_polygonal_prism_data.hpp new file mode 100755 index 0000000..6f2bf95 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/extract_polygonal_prism_data.hpp @@ -0,0 +1,258 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ +#define PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud &polygon) +{ + // Compute the plane coefficients + Eigen::Vector4f model_coefficients; + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f xyz_centroid; + + computeMeanAndCovarianceMatrix (polygon, covariance_matrix, xyz_centroid); + + // Compute the model coefficients + EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + eigen33 (covariance_matrix, eigen_value, eigen_vector); + + model_coefficients[0] = eigen_vector [0]; + model_coefficients[1] = eigen_vector [1]; + model_coefficients[2] = eigen_vector [2]; + model_coefficients[3] = 0; + + // Hessian form (D = nc . p_plane (centroid here) + p) + model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); + + float distance_to_plane = model_coefficients[0] * point.x + + model_coefficients[1] * point.y + + model_coefficients[2] * point.z + + model_coefficients[3]; + PointT ppoint; + // Calculate the projection of the point on the plane + ppoint.x = point.x - distance_to_plane * model_coefficients[0]; + ppoint.y = point.y - distance_to_plane * model_coefficients[1]; + ppoint.z = point.z - distance_to_plane * model_coefficients[2]; + + // Create a X-Y projected representation for within bounds polygonal checking + int k0, k1, k2; + // Determine the best plane to project points onto + k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1; + k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2; + k1 = (k0 + 1) % 3; + k2 = (k0 + 2) % 3; + // Project the convex hull + pcl::PointCloud xy_polygon; + xy_polygon.points.resize (polygon.points.size ()); + for (std::size_t i = 0; i < polygon.points.size (); ++i) + { + Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0); + xy_polygon.points[i].x = pt[k1]; + xy_polygon.points[i].y = pt[k2]; + xy_polygon.points[i].z = 0; + } + PointT xy_point; + xy_point.z = 0; + Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0); + xy_point.x = pt[k1]; + xy_point.y = pt[k2]; + + return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon)); +} + +////////////////////////////////////////////////////////////////////////// +template bool +pcl::isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud &polygon) +{ + bool in_poly = false; + double x1, x2, y1, y2; + + int nr_poly_points = static_cast (polygon.points.size ()); + // start with the last point to make the check last point<->first point the first one + double xold = polygon.points[nr_poly_points - 1].x; + double yold = polygon.points[nr_poly_points - 1].y; + for (int i = 0; i < nr_poly_points; i++) + { + double xnew = polygon.points[i].x; + double ynew = polygon.points[i].y; + if (xnew > xold) + { + x1 = xold; + x2 = xnew; + y1 = yold; + y2 = ynew; + } + else + { + x1 = xnew; + x2 = xold; + y1 = ynew; + y2 = yold; + } + + if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) ) + { + in_poly = !in_poly; + } + xold = xnew; + yold = ynew; + } + + return (in_poly); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ExtractPolygonalPrismData::segment (pcl::PointIndices &output) +{ + output.header = input_->header; + + if (!initCompute ()) + { + output.indices.clear (); + return; + } + + if (static_cast (planar_hull_->points.size ()) < min_pts_hull_) + { + PCL_ERROR ("[pcl::%s::segment] Not enough points (%lu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ()); + output.indices.clear (); + return; + } + + // Compute the plane coefficients + Eigen::Vector4f model_coefficients; + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f xyz_centroid; + + computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid); + + // Compute the model coefficients + EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + eigen33 (covariance_matrix, eigen_value, eigen_vector); + + model_coefficients[0] = eigen_vector [0]; + model_coefficients[1] = eigen_vector [1]; + model_coefficients[2] = eigen_vector [2]; + model_coefficients[3] = 0; + + // Hessian form (D = nc . p_plane (centroid here) + p) + model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); + + // Need to flip the plane normal towards the viewpoint + Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0); + // See if we need to flip any plane normals + vp -= planar_hull_->points[0].getVector4fMap (); + vp[3] = 0; + // Dot product between the (viewpoint - point) and the plane normal + float cos_theta = vp.dot (model_coefficients); + // Flip the plane normal + if (cos_theta < 0) + { + model_coefficients *= -1; + model_coefficients[3] = 0; + // Hessian form (D = nc . p_plane (centroid here) + p) + model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ())); + } + + // Project all points + PointCloud projected_points; + SampleConsensusModelPlane sacmodel (input_); + sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false); + + // Create a X-Y projected representation for within bounds polygonal checking + int k0, k1, k2; + // Determine the best plane to project points onto + k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1; + k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2; + k1 = (k0 + 1) % 3; + k2 = (k0 + 2) % 3; + // Project the convex hull + pcl::PointCloud polygon; + polygon.points.resize (planar_hull_->points.size ()); + for (std::size_t i = 0; i < planar_hull_->points.size (); ++i) + { + Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0); + polygon.points[i].x = pt[k1]; + polygon.points[i].y = pt[k2]; + polygon.points[i].z = 0; + } + + PointT pt_xy; + pt_xy.z = 0; + + output.indices.resize (indices_->size ()); + int l = 0; + for (std::size_t i = 0; i < projected_points.points.size (); ++i) + { + // Check the distance to the user imposed limits from the table planar model + double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients); + if (distance < height_limit_min_ || distance > height_limit_max_) + continue; + + // Check what points are inside the hull + Eigen::Vector4f pt (projected_points.points[i].x, + projected_points.points[i].y, + projected_points.points[i].z, 0); + pt_xy.x = pt[k1]; + pt_xy.y = pt[k2]; + + if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon)) + continue; + + output.indices[l++] = (*indices_)[i]; + } + output.indices.resize (l); + + deinitCompute (); +} + +#define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData; +#define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon(const T&, const pcl::PointCloud &); +#define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon(const T &, const pcl::PointCloud &); + +#endif // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/grabcut_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/grabcut_segmentation.hpp new file mode 100755 index 0000000..e87d883 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/grabcut_segmentation.hpp @@ -0,0 +1,512 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP +#define PCL_SEGMENTATION_IMPL_GRABCUT_HPP + +#include +#include +#include + +namespace pcl +{ + template <> + float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1, + const pcl::segmentation::grabcut::Color &c2) + { + return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b)); + } +} + +template +pcl::segmentation::grabcut::Color::Color (const PointT& p) +{ + r = static_cast (p.r) / 255.0; + g = static_cast (p.g) / 255.0; + b = static_cast (p.b) / 255.0; +} + +template +pcl::segmentation::grabcut::Color::operator PointT () const +{ + PointT p; + p.r = static_cast (r * 255); + p.g = static_cast (g * 255); + p.b = static_cast (b * 255); + return (p); +} + +template void +pcl::GrabCut::setInputCloud (const PointCloudConstPtr &cloud) +{ + input_ = cloud; +} + +template bool +pcl::GrabCut::initCompute () +{ + using namespace pcl::segmentation::grabcut; + if (!pcl::PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!"); + return (false); + } + + std::vector in_fields_; + if ((pcl::getFieldIndex ("rgb", in_fields_) == -1) && + (pcl::getFieldIndex ("rgba", in_fields_) == -1)) + { + PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!"); + return (false); + } + + // Initialize the working image + image_.reset (new Image (input_->width, input_->height)); + for (std::size_t i = 0; i < input_->size (); ++i) + { + (*image_) [i] = Color (input_->points[i]); + } + width_ = image_->width; + height_ = image_->height; + + // Initialize the spatial locator + if (!tree_ && !input_->isOrganized ()) + { + tree_.reset (new pcl::search::KdTree (true)); + tree_->setInputCloud (input_); + } + + const std::size_t indices_size = indices_->size (); + trimap_ = std::vector (indices_size, TrimapUnknown); + hard_segmentation_ = std::vector (indices_size, + SegmentationBackground); + GMM_component_.resize (indices_size); + n_links_.resize (indices_size); + + // soft_segmentation_ = 0; // Not yet implemented + foreground_GMM_.resize (K_); + background_GMM_.resize (K_); + + //set some constants + computeL (); + + if (image_->isOrganized ()) + { + computeBetaOrganized (); + computeNLinksOrganized (); + } + else + { + computeBetaNonOrganized (); + computeNLinksNonOrganized (); + } + + initialized_ = false; + return (true); +} + +template void +pcl::GrabCut::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity) +{ + graph_.addEdge (v1, v2, capacity, rev_capacity); +} + +template void +pcl::GrabCut::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity) +{ + graph_.addSourceEdge (v, source_capacity); + graph_.addTargetEdge (v, sink_capacity); +} + +template void +pcl::GrabCut::setBackgroundPointsIndices (const PointIndicesConstPtr &indices) +{ + using namespace pcl::segmentation::grabcut; + if (!initCompute ()) + return; + + std::fill (trimap_.begin (), trimap_.end (), TrimapBackground); + std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground); + for (const int &index : indices->indices) + { + trimap_[index] = TrimapUnknown; + hard_segmentation_[index] = SegmentationForeground; + } + + if (!initialized_) + { + fitGMMs (); + initialized_ = true; + } +} + +template void +pcl::GrabCut::fitGMMs () +{ + // Step 3: Build GMMs using Orchard-Bouman clustering algorithm + buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); + + // Initialize the graph for graphcut (do this here so that the T-Link debugging image will be initialized) + initGraph (); +} + +template int +pcl::GrabCut::refineOnce () +{ + // Steps 4 and 5: Learn new GMMs from current segmentation + learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); + + // Step 6: Run GraphCut and update segmentation + initGraph (); + + float flow = graph_.solve (); + + int changed = updateHardSegmentation (); + PCL_INFO ("%d pixels changed segmentation (max flow = %f)\n", changed, flow); + + return (changed); +} + +template void +pcl::GrabCut::refine () +{ + std::size_t changed = indices_->size (); + + while (changed) + changed = refineOnce (); +} + +template int +pcl::GrabCut::updateHardSegmentation () +{ + using namespace pcl::segmentation::grabcut; + + int changed = 0; + + const int number_of_indices = static_cast (indices_->size ()); + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + SegmentationValue old_value = hard_segmentation_ [i_point]; + + if (trimap_ [i_point] == TrimapBackground) + hard_segmentation_ [i_point] = SegmentationBackground; + else + if (trimap_ [i_point] == TrimapForeground) + hard_segmentation_ [i_point] = SegmentationForeground; + else // TrimapUnknown + { + if (isSource (graph_nodes_[i_point])) + hard_segmentation_ [i_point] = SegmentationForeground; + else + hard_segmentation_ [i_point] = SegmentationBackground; + } + + if (old_value != hard_segmentation_ [i_point]) + ++changed; + } + return (changed); +} + +template void +pcl::GrabCut::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t) +{ + using namespace pcl::segmentation::grabcut; + for (const int &index : indices->indices) + trimap_[index] = t; + + // Immediately set the hard segmentation as well so that the display will update. + if (t == TrimapForeground) + for (const int &index : indices->indices) + hard_segmentation_[index] = SegmentationForeground; + else + if (t == TrimapBackground) + for (const int &index : indices->indices) + hard_segmentation_[index] = SegmentationBackground; +} + +template void +pcl::GrabCut::initGraph () +{ + using namespace pcl::segmentation::grabcut; + const int number_of_indices = static_cast (indices_->size ()); + // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated) + graph_.clear (); + graph_nodes_.clear (); + graph_nodes_.resize (indices_->size ()); + int start = graph_.addNodes (indices_->size ()); + for (std::size_t idx = 0; idx < indices_->size (); ++idx) + { + graph_nodes_[idx] = start; + ++start; + } + + // Set T-Link weights + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + int point_index = (*indices_) [i_point]; + float back, fore; + + switch (trimap_[point_index]) + { + case TrimapUnknown : + { + fore = static_cast (-std::log (background_GMM_.probabilityDensity (image_->points[point_index]))); + back = static_cast (-std::log (foreground_GMM_.probabilityDensity (image_->points[point_index]))); + break; + } + case TrimapBackground : + { + fore = 0; + back = L_; + break; + } + default : + { + fore = L_; + back = 0; + } + } + + setTerminalWeights (graph_nodes_[i_point], fore, back); + } + + // Set N-Link weights from precomputed values + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + const NLinks &n_link = n_links_[i_point]; + if (n_link.nb_links > 0) + { + int point_index = (*indices_) [i_point]; + std::vector::const_iterator weights_it = n_link.weights.begin (); + for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++weights_it) + { + if ((*indices_it != point_index) && (*indices_it > -1)) + { + addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it); + } + } + } + } +} + +template void +pcl::GrabCut::computeNLinksNonOrganized () +{ + const int number_of_indices = static_cast (indices_->size ()); + for (int i_point = 0; i_point < number_of_indices; ++i_point) + { + NLinks &n_link = n_links_[i_point]; + if (n_link.nb_links > 0) + { + int point_index = (*indices_) [i_point]; + auto dists_it = n_link.dists.cbegin (); + auto weights_it = n_link.weights.begin (); + for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++dists_it, ++weights_it) + { + if (*indices_it != point_index) + { + // We saved the color distance previously at the computeBeta stage for optimization purpose + float color_distance = *weights_it; + // Set the real weight + *weights_it = static_cast (lambda_ * std::exp (-beta_ * color_distance) / sqrt (*dists_it)); + } + } + } + } +} + +template void +pcl::GrabCut::computeNLinksOrganized () +{ + for( unsigned int y = 0; y < image_->height; ++y ) + { + for( unsigned int x = 0; x < image_->width; ++x ) + { + // We saved the color and euclidean distance previously at the computeBeta stage for + // optimization purpose but here we compute the real weight + std::size_t point_index = y * input_->width + x; + NLinks &links = n_links_[point_index]; + + if( x > 0 && y < image_->height-1 ) + links.weights[0] = lambda_ * std::exp (-beta_ * links.weights[0]) / links.dists[0]; + + if( y < image_->height-1 ) + links.weights[1] = lambda_ * std::exp (-beta_ * links.weights[1]) / links.dists[1]; + + if( x < image_->width-1 && y < image_->height-1 ) + links.weights[2] = lambda_ * std::exp (-beta_ * links.weights[2]) / links.dists[2]; + + if( x < image_->width-1 ) + links.weights[3] = lambda_ * std::exp (-beta_ * links.weights[3]) / links.dists[3]; + } + } +} + +template void +pcl::GrabCut::computeBetaNonOrganized () +{ + float result = 0; + std::size_t edges = 0; + + const int number_of_indices = static_cast (indices_->size ()); + + for (int i_point = 0; i_point < number_of_indices; i_point++) + { + int point_index = (*indices_)[i_point]; + const PointT& point = input_->points [point_index]; + if (pcl::isFinite (point)) + { + NLinks &links = n_links_[i_point]; + int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists); + if (found > 1) + { + links.nb_links = found - 1; + links.weights.reserve (links.nb_links); + for (std::vector::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it) + { + if (*nn_it != point_index) + { + float color_distance = squaredEuclideanDistance (image_->points[point_index], image_->points[*nn_it]); + links.weights.push_back (color_distance); + result+= color_distance; + ++edges; + } + else + links.weights.push_back (0.f); + } + } + } + } + + beta_ = 1e5 / (2*result / edges); +} + +template void +pcl::GrabCut::computeBetaOrganized () +{ + float result = 0; + std::size_t edges = 0; + + for (unsigned int y = 0; y < input_->height; ++y) + { + for (unsigned int x = 0; x < input_->width; ++x) + { + std::size_t point_index = y * input_->width + x; + NLinks &links = n_links_[point_index]; + links.nb_links = 4; + links.weights.resize (links.nb_links, 0); + links.dists.resize (links.nb_links, 0); + links.indices.resize (links.nb_links, -1); + + if (x > 0 && y < input_->height-1) + { + std::size_t upleft = (y+1) * input_->width + x - 1; + links.indices[0] = upleft; + links.dists[0] = std::sqrt (2.f); + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points[upleft]); + links.weights[0] = color_dist; + result+= color_dist; + edges++; + } + + if (y < input_->height-1) + { + std::size_t up = (y+1) * input_->width + x; + links.indices[1] = up; + links.dists[1] = 1; + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points[up]); + links.weights[1] = color_dist; + result+= color_dist; + edges++; + } + + if (x < input_->width-1 && y < input_->height-1) + { + std::size_t upright = (y+1) * input_->width + x + 1; + links.indices[2] = upright; + links.dists[2] = std::sqrt (2.f); + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points [upright]); + links.weights[2] = color_dist; + result+= color_dist; + edges++; + } + + if (x < input_->width-1) + { + std::size_t right = y * input_->width + x + 1; + links.indices[3] = right; + links.dists[3] = 1; + float color_dist = squaredEuclideanDistance (image_->points[point_index], + image_->points[right]); + links.weights[3] = color_dist; + result+= color_dist; + edges++; + } + } + } + + beta_ = 1e5 / (2*result / edges); +} + +template void +pcl::GrabCut::computeL () +{ + L_ = 8*lambda_ + 1; +} + +template void +pcl::GrabCut::extract (std::vector& clusters) +{ + using namespace pcl::segmentation::grabcut; + clusters.clear (); + clusters.resize (2); + clusters[0].indices.reserve (indices_->size ()); + clusters[1].indices.reserve (indices_->size ()); + refine (); + assert (hard_segmentation_.size () == indices_->size ()); + const int indices_size = static_cast (indices_->size ()); + for (int i = 0; i < indices_size; ++i) + if (hard_segmentation_[i] == SegmentationForeground) + clusters[1].indices.push_back (i); + else + clusters[0].indices.push_back (i); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/lccp_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/lccp_segmentation.hpp new file mode 100755 index 0000000..a43243f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/lccp_segmentation.hpp @@ -0,0 +1,525 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_ +#define PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_ + +#include +#include + + +////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////// +/////////////////// Public Functions ///////////////////// +////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////// + + + +template +pcl::LCCPSegmentation::LCCPSegmentation () : + concavity_tolerance_threshold_ (10), + grouping_data_valid_ (false), + supervoxels_set_ (false), + use_smoothness_check_ (false), + smoothness_threshold_ (0.1), + use_sanity_check_ (false), + seed_resolution_ (0), + voxel_resolution_ (0), + k_factor_ (0), + min_segment_size_ (0) +{ +} + +template +pcl::LCCPSegmentation::~LCCPSegmentation () +{ +} + +template void +pcl::LCCPSegmentation::reset () +{ + sv_adjacency_list_.clear (); + processed_.clear (); + sv_label_to_supervoxel_map_.clear (); + sv_label_to_seg_label_map_.clear (); + seg_label_to_sv_list_map_.clear (); + seg_label_to_neighbor_set_map_.clear (); + grouping_data_valid_ = false; + supervoxels_set_ = false; +} + +template void +pcl::LCCPSegmentation::segment () +{ + if (supervoxels_set_) + { + // Calculate for every Edge if the connection is convex or invalid + // This effectively performs the segmentation. + calculateConvexConnections (sv_adjacency_list_); + + // Correct edge relations using extended convexity definition if k>0 + applyKconvexity (k_factor_); + + // group supervoxels + doGrouping (); + + grouping_data_valid_ = true; + + // merge small segments + mergeSmallSegments (); + } + else + PCL_WARN ("[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n"); +} + + +template void +pcl::LCCPSegmentation::relabelCloud (pcl::PointCloud &labeled_cloud_arg) +{ + if (grouping_data_valid_) + { + // Relabel all Points in cloud with new labels + for (auto &voxel : labeled_cloud_arg) + { + voxel.label = sv_label_to_seg_label_map_[voxel.label]; + } + } + else + { + PCL_WARN ("[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n"); + } +} + + + +////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////// +/////////////////// Protected Functions ////////////////// +////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////// + +template void +pcl::LCCPSegmentation::computeSegmentAdjacency () +{ + seg_label_to_neighbor_set_map_.clear (); + + //The vertices in the supervoxel adjacency list are the supervoxel centroids + std::pair vertex_iterator_range; + vertex_iterator_range = boost::vertices (sv_adjacency_list_); + + std::uint32_t current_segLabel; + std::uint32_t neigh_segLabel; + + // For every Supervoxel.. + for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels + { + const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr]; + current_segLabel = sv_label_to_seg_label_map_[sv_label]; + + // ..look at all neighbors and insert their labels into the neighbor set + std::pair neighbors = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); + for (AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor) + { + const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor]; + neigh_segLabel = sv_label_to_seg_label_map_[neigh_label]; + + if (current_segLabel != neigh_segLabel) + { + seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel); + } + } + } +} + +template void +pcl::LCCPSegmentation::mergeSmallSegments () +{ + if (min_segment_size_ == 0) + return; + + computeSegmentAdjacency (); + + std::set filteredSegLabels; + + std::uint32_t largest_neigh_size = 0; + std::uint32_t largest_neigh_seg_label = 0; + std::uint32_t current_seg_label; + + std::pair vertex_iterator_range; + vertex_iterator_range = boost::vertices (sv_adjacency_list_); + + bool continue_filtering = true; + + while (continue_filtering) + { + continue_filtering = false; + unsigned int nr_filtered = 0; + + // Iterate through all supervoxels, check if they are in a "small" segment -> change label to largest neighborID + for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels + { + const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr]; + current_seg_label = sv_label_to_seg_label_map_[sv_label]; + largest_neigh_seg_label = current_seg_label; + largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size (); + + const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size (); + if (nr_neighbors == 0) + continue; + + if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_) + { + continue_filtering = true; + nr_filtered++; + + // Find largest neighbor + for (auto neighbors_itr = seg_label_to_neighbor_set_map_[current_seg_label].cbegin (); neighbors_itr != seg_label_to_neighbor_set_map_[current_seg_label].cend (); ++neighbors_itr) + { + if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size) + { + largest_neigh_seg_label = *neighbors_itr; + largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size (); + } + } + + // Add to largest neighbor + if (largest_neigh_seg_label != current_seg_label) + { + if (filteredSegLabels.count (largest_neigh_seg_label) > 0) + continue; // If neighbor was already assigned to someone else + + sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label; + filteredSegLabels.insert (current_seg_label); + + // Assign supervoxel labels of filtered segment to new owner + for (auto sv_ID_itr = seg_label_to_sv_list_map_[current_seg_label].cbegin (); sv_ID_itr != seg_label_to_sv_list_map_[current_seg_label].cend (); ++sv_ID_itr) + { + seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr); + } + } + } + } + + // Erase filtered Segments from segment map + for (const unsigned int &filteredSegLabel : filteredSegLabels) + { + seg_label_to_sv_list_map_.erase (filteredSegLabel); + } + + // After filtered Segments are deleted, compute completely new adjacency map + // NOTE Recomputing the adjacency of every segment in every iteration is an easy but inefficient solution. + // Because the number of segments in an average scene is usually well below 1000, the time spend for noise filtering is still negligible in most cases + computeSegmentAdjacency (); + } // End while (Filtering) +} + +template void +pcl::LCCPSegmentation::prepareSegmentation (const std::map::Ptr>& supervoxel_clusters_arg, + const std::multimap& label_adjaceny_arg) +{ + // Clear internal data + reset (); + + // Copy map with supervoxel pointers + sv_label_to_supervoxel_map_ = supervoxel_clusters_arg; + + // Build a boost adjacency list from the adjacency multimap + std::map label_ID_map; + + // Add all supervoxel labels as vertices + for (typename std::map::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin (); + svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr) + { + const std::uint32_t& sv_label = svlabel_itr->first; + VertexID node_id = boost::add_vertex (sv_adjacency_list_); + sv_adjacency_list_[node_id] = sv_label; + label_ID_map[sv_label] = node_id; + } + + // Add all edges + for (const auto &sv_neighbors_itr : label_adjaceny_arg) + { + const std::uint32_t& sv_label = sv_neighbors_itr.first; + const std::uint32_t& neighbor_label = sv_neighbors_itr.second; + + VertexID u = label_ID_map[sv_label]; + VertexID v = label_ID_map[neighbor_label]; + + boost::add_edge (u, v, sv_adjacency_list_); + } + + // Initialization + // clear the processed_ map + seg_label_to_sv_list_map_.clear (); + for (typename std::map::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin (); + svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr) + { + const std::uint32_t& sv_label = svlabel_itr->first; + processed_[sv_label] = false; + sv_label_to_seg_label_map_[sv_label] = 0; + } +} + + + + +template void +pcl::LCCPSegmentation::doGrouping () +{ + // clear the processed_ map + seg_label_to_sv_list_map_.clear (); + for (typename std::map::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin (); + svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr) + { + const std::uint32_t& sv_label = svlabel_itr->first; + processed_[sv_label] = false; + sv_label_to_seg_label_map_[sv_label] = 0; + } + + // Perform depth search on the graph and recursively group all supervoxels with convex connections + //The vertices in the supervoxel adjacency list are the supervoxel centroids + std::pair< VertexIterator, VertexIterator> vertex_iterator_range; + vertex_iterator_range = boost::vertices (sv_adjacency_list_); + + // Note: *sv_itr is of type " boost::graph_traits::vertex_descriptor " which it nothing but a typedef of std::size_t.. + unsigned int segment_label = 1; // This starts at 1, because 0 is reserved for errors + for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels + { + const VertexID sv_vertex_id = *sv_itr; + const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id]; + if (!processed_[sv_label]) + { + // Add neighbors (and their neighbors etc.) to group if similarity constraint is met + recursiveSegmentGrowing (sv_vertex_id, segment_label); + ++segment_label; // After recursive grouping ended (no more neighbors to consider) -> go to next group + } + } +} + +template void +pcl::LCCPSegmentation::recursiveSegmentGrowing (const VertexID &query_point_id, + const unsigned int segment_label) +{ + const std::uint32_t& sv_label = sv_adjacency_list_[query_point_id]; + + processed_[sv_label] = true; + + // The next two lines add the supervoxel to the segment + sv_label_to_seg_label_map_[sv_label] = segment_label; + seg_label_to_sv_list_map_[segment_label].insert (sv_label); + + // Iterate through all neighbors of this supervoxel and check whether they should be merged with the current supervoxel + std::pair out_edge_iterator_range; + out_edge_iterator_range = boost::out_edges (query_point_id, sv_adjacency_list_); // adjacent vertices to node (*itr) in graph sv_adjacency_list_ + for (OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr) + { + const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_); + const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID]; + + if (!processed_[neighbor_label]) // If neighbor was not already processed + { + if (sv_adjacency_list_[*out_Edge_itr].is_valid) + { + recursiveSegmentGrowing (neighbor_ID, segment_label); + } + } + } // End neighbor loop +} + +template void +pcl::LCCPSegmentation::applyKconvexity (const unsigned int k_arg) +{ + if (k_arg == 0) + return; + + unsigned int kcount = 0; + + EdgeIterator edge_itr, edge_itr_end, next_edge; + boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_); + + std::pair source_neighbors_range; + std::pair target_neighbors_range; + + // Check all edges in the graph for k-convexity + for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge) + { + next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge + + bool is_convex = sv_adjacency_list_[*edge_itr].is_convex; + + if (is_convex) // If edge is (0-)convex + { + kcount = 0; + + const VertexID source = boost::source (*edge_itr, sv_adjacency_list_); + const VertexID target = boost::target (*edge_itr, sv_adjacency_list_); + + source_neighbors_range = boost::out_edges (source, sv_adjacency_list_); + target_neighbors_range = boost::out_edges (target, sv_adjacency_list_); + + // Find common neighbors, check their connection + for (OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr) // For all supervoxels + { + VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_); + + for (OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr) // For all supervoxels + { + VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_); + if (source_neighbor_ID == target_neighbor_ID) // Common neighbor + { + EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first; + EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first; + + bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex; + bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex; + + if (src_is_convex && tar_is_convex) + ++kcount; + + break; + } + } + + if (kcount >= k_arg) // Connection is k-convex, stop search + break; + } + + // Check k convexity + if (kcount < k_arg) + (sv_adjacency_list_)[*edge_itr].is_valid = false; + } + } +} + +template void +pcl::LCCPSegmentation::calculateConvexConnections (SupervoxelAdjacencyList& adjacency_list_arg) +{ + + EdgeIterator edge_itr, edge_itr_end, next_edge; + boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg); + + for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge) + { + next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge + + std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)]; + std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)]; + + float normal_difference; + bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference); + adjacency_list_arg[*edge_itr].is_convex = is_convex; + adjacency_list_arg[*edge_itr].is_valid = is_convex; + adjacency_list_arg[*edge_itr].normal_difference = normal_difference; + } +} + +template bool +pcl::LCCPSegmentation::connIsConvex (const std::uint32_t source_label_arg, + const std::uint32_t target_label_arg, + float &normal_angle) +{ + typename pcl::Supervoxel::Ptr& sv_source = sv_label_to_supervoxel_map_[source_label_arg]; + typename pcl::Supervoxel::Ptr& sv_target = sv_label_to_supervoxel_map_[target_label_arg]; + + const Eigen::Vector3f& source_centroid = sv_source->centroid_.getVector3fMap (); + const Eigen::Vector3f& target_centroid = sv_target->centroid_.getVector3fMap (); + + const Eigen::Vector3f& source_normal = sv_source->normal_.getNormalVector3fMap (). normalized (); + const Eigen::Vector3f& target_normal = sv_target->normal_.getNormalVector3fMap (). normalized (); + + //NOTE For angles below 0 nothing will be merged + if (concavity_tolerance_threshold_ < 0) + { + return (false); + } + + bool is_convex = true; + bool is_smooth = true; + + normal_angle = getAngle3D (source_normal, target_normal, true); + // Geometric comparisons + Eigen::Vector3f vec_t_to_s, vec_s_to_t; + + vec_t_to_s = source_centroid - target_centroid; + vec_s_to_t = -vec_t_to_s; + + Eigen::Vector3f ncross; + ncross = source_normal.cross (target_normal); + + // Smoothness Check: Check if there is a step between adjacent patches + if (use_smoothness_check_) + { + float expected_distance = ncross.norm () * seed_resolution_; + float dot_p_1 = vec_t_to_s.dot (source_normal); + float dot_p_2 = vec_s_to_t.dot (target_normal); + float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2); + const float dist_smoothing = smoothness_threshold_ * voxel_resolution_; // This is a slacking variable especially important for patches with very similar normals + + if (point_dist > (expected_distance + dist_smoothing)) + { + is_smooth &= false; + } + } + // ---------------- + + // Sanity Criterion: Check if definition convexity/concavity makes sense for connection of given patches + float intersection_angle = getAngle3D (ncross, vec_t_to_s, true); + float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle; + + float intersect_thresh = 60. * 1. / (1. + std::exp (-0.25 * (normal_angle - 25.))); + if (min_intersect_angle < intersect_thresh && use_sanity_check_) + { + // std::cout << "Concave/Convex not defined for given case!" << std::endl; + is_convex &= false; + } + + + // vec_t_to_s is the reference direction for angle measurements + // Convexity Criterion: Check if connection of patches is convex. If this is the case the two supervoxels should be merged. + if ((getAngle3D (vec_t_to_s, source_normal) - getAngle3D (vec_t_to_s, target_normal)) <= 0) + { + is_convex &= true; // connection convex + } + else + { + is_convex &= (normal_angle < concavity_tolerance_threshold_); // concave connections will be accepted if difference of normals is small + } + return (is_convex && is_smooth); +} + +#endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/min_cut_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/min_cut_segmentation.hpp new file mode 100755 index 0000000..c95aa40 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/min_cut_segmentation.hpp @@ -0,0 +1,621 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id:$ + * + */ + +#ifndef PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ +#define PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ + +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MinCutSegmentation::MinCutSegmentation () : + inverse_sigma_ (16.0), + binary_potentials_are_valid_ (false), + epsilon_ (0.0001), + radius_ (16.0), + unary_potentials_are_valid_ (false), + source_weight_ (0.8), + search_ (), + number_of_neighbours_ (14), + graph_is_valid_ (false), + foreground_points_ (0), + background_points_ (0), + clusters_ (0), + vertices_ (0), + edge_marker_ (0), + source_ (),///////////////////////////////// + sink_ (),/////////////////////////////////// + max_flow_ (0.0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MinCutSegmentation::~MinCutSegmentation () +{ + foreground_points_.clear (); + background_points_.clear (); + clusters_.clear (); + vertices_.clear (); + edge_marker_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setInputCloud (const PointCloudConstPtr &cloud) +{ + input_ = cloud; + graph_is_valid_ = false; + unary_potentials_are_valid_ = false; + binary_potentials_are_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::MinCutSegmentation::getSigma () const +{ + return (pow (1.0 / inverse_sigma_, 0.5)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setSigma (double sigma) +{ + if (sigma > epsilon_) + { + inverse_sigma_ = 1.0 / (sigma * sigma); + binary_potentials_are_valid_ = false; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::MinCutSegmentation::getRadius () const +{ + return (pow (radius_, 0.5)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setRadius (double radius) +{ + if (radius > epsilon_) + { + radius_ = radius * radius; + unary_potentials_are_valid_ = false; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::MinCutSegmentation::getSourceWeight () const +{ + return (source_weight_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setSourceWeight (double weight) +{ + if (weight > epsilon_) + { + source_weight_ = weight; + unary_potentials_are_valid_ = false; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::MinCutSegmentation::KdTreePtr +pcl::MinCutSegmentation::getSearchMethod () const +{ + return (search_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setSearchMethod (const KdTreePtr& tree) +{ + search_ = tree; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::MinCutSegmentation::getNumberOfNeighbours () const +{ + return (number_of_neighbours_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setNumberOfNeighbours (unsigned int neighbour_number) +{ + if (number_of_neighbours_ != neighbour_number && neighbour_number != 0) + { + number_of_neighbours_ = neighbour_number; + graph_is_valid_ = false; + unary_potentials_are_valid_ = false; + binary_potentials_are_valid_ = false; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector > +pcl::MinCutSegmentation::getForegroundPoints () const +{ + return (foreground_points_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setForegroundPoints (typename pcl::PointCloud::Ptr foreground_points) +{ + foreground_points_.clear (); + foreground_points_.reserve (foreground_points->points.size ()); + for (std::size_t i_point = 0; i_point < foreground_points->points.size (); i_point++) + foreground_points_.push_back (foreground_points->points[i_point]); + + unary_potentials_are_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector > +pcl::MinCutSegmentation::getBackgroundPoints () const +{ + return (background_points_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::setBackgroundPoints (typename pcl::PointCloud::Ptr background_points) +{ + background_points_.clear (); + background_points_.reserve (background_points->points.size ()); + for (std::size_t i_point = 0; i_point < background_points->points.size (); i_point++) + background_points_.push_back (background_points->points[i_point]); + + unary_potentials_are_valid_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::extract (std::vector & clusters) +{ + clusters.clear (); + + bool segmentation_is_possible = initCompute (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + if ( graph_is_valid_ && unary_potentials_are_valid_ && binary_potentials_are_valid_ ) + { + clusters.reserve (clusters_.size ()); + std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters)); + deinitCompute (); + return; + } + + clusters_.clear (); + bool success = true; + + if ( !graph_is_valid_ ) + { + success = buildGraph (); + if (!success) + { + deinitCompute (); + return; + } + graph_is_valid_ = true; + unary_potentials_are_valid_ = true; + binary_potentials_are_valid_ = true; + } + + if ( !unary_potentials_are_valid_ ) + { + success = recalculateUnaryPotentials (); + if (!success) + { + deinitCompute (); + return; + } + unary_potentials_are_valid_ = true; + } + + if ( !binary_potentials_are_valid_ ) + { + success = recalculateBinaryPotentials (); + if (!success) + { + deinitCompute (); + return; + } + binary_potentials_are_valid_ = true; + } + + //IndexMap index_map = boost::get (boost::vertex_index, *graph_); + ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_); + + max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_); + + assembleLabels (residual_capacity); + + clusters.reserve (clusters_.size ()); + std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters)); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::MinCutSegmentation::getMaxFlow () const +{ + return (max_flow_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::MinCutSegmentation::mGraphPtr +pcl::MinCutSegmentation::getGraph () const +{ + return (graph_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MinCutSegmentation::buildGraph () +{ + int number_of_points = static_cast (input_->points.size ()); + int number_of_indices = static_cast (indices_->size ()); + + if (input_->points.empty () || number_of_points == 0 || foreground_points_.empty () == true ) + return (false); + + if (!search_) + search_.reset (new pcl::search::KdTree); + + graph_.reset (new mGraph); + + capacity_.reset (new CapacityMap); + *capacity_ = boost::get (boost::edge_capacity, *graph_); + + reverse_edges_.reset (new ReverseEdgeMap); + *reverse_edges_ = boost::get (boost::edge_reverse, *graph_); + + VertexDescriptor vertex_descriptor(0); + vertices_.clear (); + vertices_.resize (number_of_points + 2, vertex_descriptor); + + std::set out_edges_marker; + edge_marker_.clear (); + edge_marker_.resize (number_of_points + 2, out_edges_marker); + + for (int i_point = 0; i_point < number_of_points + 2; i_point++) + vertices_[i_point] = boost::add_vertex (*graph_); + + source_ = vertices_[number_of_points]; + sink_ = vertices_[number_of_points + 1]; + + for (int i_point = 0; i_point < number_of_indices; i_point++) + { + int point_index = (*indices_)[i_point]; + double source_weight = 0.0; + double sink_weight = 0.0; + calculateUnaryPotential (point_index, source_weight, sink_weight); + addEdge (static_cast (source_), point_index, source_weight); + addEdge (point_index, static_cast (sink_), sink_weight); + } + + std::vector neighbours; + std::vector distances; + search_->setInputCloud (input_, indices_); + for (int i_point = 0; i_point < number_of_indices; i_point++) + { + int point_index = (*indices_)[i_point]; + search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances); + for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++) + { + double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]); + addEdge (point_index, neighbours[i_nghbr], weight); + addEdge (neighbours[i_nghbr], point_index, weight); + } + neighbours.clear (); + distances.clear (); + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::calculateUnaryPotential (int point, double& source_weight, double& sink_weight) const +{ + double min_dist_to_foreground = std::numeric_limits::max (); + //double min_dist_to_background = std::numeric_limits::max (); + double closest_foreground_point[2]; + closest_foreground_point[0] = closest_foreground_point[1] = 0; + //double closest_background_point[] = {0.0, 0.0}; + double initial_point[] = {0.0, 0.0}; + + initial_point[0] = input_->points[point].x; + initial_point[1] = input_->points[point].y; + + for (std::size_t i_point = 0; i_point < foreground_points_.size (); i_point++) + { + double dist = 0.0; + dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]); + dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]); + if (min_dist_to_foreground > dist) + { + min_dist_to_foreground = dist; + closest_foreground_point[0] = foreground_points_[i_point].x; + closest_foreground_point[1] = foreground_points_[i_point].y; + } + } + + sink_weight = pow (min_dist_to_foreground / radius_, 0.5); + + source_weight = source_weight_; + return; +/* + if (background_points_.size () == 0) + return; + + for (int i_point = 0; i_point < background_points_.size (); i_point++) + { + double dist = 0.0; + dist += (background_points_[i_point].x - initial_point[0]) * (background_points_[i_point].x - initial_point[0]); + dist += (background_points_[i_point].y - initial_point[1]) * (background_points_[i_point].y - initial_point[1]); + if (min_dist_to_background > dist) + { + min_dist_to_background = dist; + closest_background_point[0] = background_points_[i_point].x; + closest_background_point[1] = background_points_[i_point].y; + } + } + + if (min_dist_to_background <= epsilon_) + { + source_weight = 0.0; + sink_weight = 1.0; + return; + } + + source_weight = 1.0 / (1.0 + pow (min_dist_to_background / min_dist_to_foreground, 0.5)); + sink_weight = 1 - source_weight; +*/ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MinCutSegmentation::addEdge (int source, int target, double weight) +{ + std::set::iterator iter_out = edge_marker_[source].find (target); + if ( iter_out != edge_marker_[source].end () ) + return (false); + + EdgeDescriptor edge; + EdgeDescriptor reverse_edge; + bool edge_was_added, reverse_edge_was_added; + + boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ ); + boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ ); + if ( !edge_was_added || !reverse_edge_was_added ) + return (false); + + (*capacity_)[edge] = weight; + (*capacity_)[reverse_edge] = 0.0; + (*reverse_edges_)[edge] = reverse_edge; + (*reverse_edges_)[reverse_edge] = edge; + edge_marker_[source].insert (target); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::MinCutSegmentation::calculateBinaryPotential (int source, int target) const +{ + double weight = 0.0; + double distance = 0.0; + distance += (input_->points[source].x - input_->points[target].x) * (input_->points[source].x - input_->points[target].x); + distance += (input_->points[source].y - input_->points[target].y) * (input_->points[source].y - input_->points[target].y); + distance += (input_->points[source].z - input_->points[target].z) * (input_->points[source].z - input_->points[target].z); + distance *= inverse_sigma_; + weight = std::exp (-distance); + + return (weight); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MinCutSegmentation::recalculateUnaryPotentials () +{ + OutEdgeIterator src_edge_iter; + OutEdgeIterator src_edge_end; + std::pair sink_edge; + + for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++) + { + double source_weight = 0.0; + double sink_weight = 0.0; + sink_edge.second = false; + calculateUnaryPotential (static_cast (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight); + sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_); + if (!sink_edge.second) + return (false); + + (*capacity_)[*src_edge_iter] = source_weight; + (*capacity_)[sink_edge.first] = sink_weight; + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::MinCutSegmentation::recalculateBinaryPotentials () +{ + int number_of_points = static_cast (indices_->size ()); + + VertexIterator vertex_iter; + VertexIterator vertex_end; + OutEdgeIterator edge_iter; + OutEdgeIterator edge_end; + + std::vector< std::set > edge_marker; + std::set out_edges_marker; + edge_marker.clear (); + edge_marker.resize (number_of_points + 2, out_edges_marker); + + for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++) + { + VertexDescriptor source_vertex = *vertex_iter; + if (source_vertex == source_ || source_vertex == sink_) + continue; + for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++) + { + //If this is not the edge of the graph, but the reverse fictitious edge that is needed for the algorithm then continue + EdgeDescriptor reverse_edge = (*reverse_edges_)[*edge_iter]; + if ((*capacity_)[reverse_edge] != 0.0) + continue; + + //If we already changed weight for this edge then continue + VertexDescriptor target_vertex = boost::target (*edge_iter, *graph_); + std::set::iterator iter_out = edge_marker[static_cast (source_vertex)].find (target_vertex); + if ( iter_out != edge_marker[static_cast (source_vertex)].end () ) + continue; + + if (target_vertex != source_ && target_vertex != sink_) + { + //Change weight and remember that this edges were updated + double weight = calculateBinaryPotential (static_cast (target_vertex), static_cast (source_vertex)); + (*capacity_)[*edge_iter] = weight; + edge_marker[static_cast (source_vertex)].insert (target_vertex); + } + } + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MinCutSegmentation::assembleLabels (ResidualCapacityMap& residual_capacity) +{ + std::vector labels; + labels.resize (input_->points.size (), 0); + int number_of_indices = static_cast (indices_->size ()); + for (int i_point = 0; i_point < number_of_indices; i_point++) + labels[(*indices_)[i_point]] = 1; + + clusters_.clear (); + + pcl::PointIndices segment; + clusters_.resize (2, segment); + + OutEdgeIterator edge_iter, edge_end; + for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; edge_iter++ ) + { + if (labels[edge_iter->m_target] == 1) + { + if (residual_capacity[*edge_iter] > epsilon_) + clusters_[1].indices.push_back (static_cast (edge_iter->m_target)); + else + clusters_[0].indices.push_back (static_cast (edge_iter->m_target)); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::MinCutSegmentation::getColoredCloud () +{ + pcl::PointCloud::Ptr colored_cloud; + + if (!clusters_.empty ()) + { + int num_of_pts_in_first_cluster = static_cast (clusters_[0].indices.size ()); + int num_of_pts_in_second_cluster = static_cast (clusters_[1].indices.size ()); + int number_of_points = num_of_pts_in_first_cluster + num_of_pts_in_second_cluster; + colored_cloud = (new pcl::PointCloud)->makeShared (); + unsigned char foreground_color[3] = {255, 255, 255}; + unsigned char background_color[3] = {255, 0, 0}; + colored_cloud->width = number_of_points; + colored_cloud->height = 1; + colored_cloud->is_dense = input_->is_dense; + + pcl::PointXYZRGB point; + int point_index = 0; + for (int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++) + { + point_index = clusters_[0].indices[i_point]; + point.x = *(input_->points[point_index].data); + point.y = *(input_->points[point_index].data + 1); + point.z = *(input_->points[point_index].data + 2); + point.r = background_color[0]; + point.g = background_color[1]; + point.b = background_color[2]; + colored_cloud->points.push_back (point); + } + + for (int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++) + { + point_index = clusters_[1].indices[i_point]; + point.x = *(input_->points[point_index].data); + point.y = *(input_->points[point_index].data + 1); + point.z = *(input_->points[point_index].data + 2); + point.r = foreground_color[0]; + point.g = foreground_color[1]; + point.b = foreground_color[2]; + colored_cloud->points.push_back (point); + } + } + + return (colored_cloud); +} + +#define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation; + +#endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/organized_connected_component_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/organized_connected_component_segmentation.hpp new file mode 100755 index 0000000..49bd05f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/organized_connected_component_segmentation.hpp @@ -0,0 +1,231 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_ +#define PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_ + +#include + +/** + * Directions: 1 2 3 + * 0 x 4 + * 7 6 5 + * e.g. direction y means we came from pixel with label y to the center pixel x + */ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedConnectedComponentSegmentation::findLabeledRegionBoundary (int start_idx, PointCloudLPtr labels, pcl::PointIndices& boundary_indices) +{ + boundary_indices.indices.clear (); + int curr_idx = start_idx; + int curr_x = start_idx % labels->width; + int curr_y = start_idx / labels->width; + unsigned label = labels->points[start_idx].label; + + // fill lookup table for next points to visit + Neighbor directions [8] = {Neighbor(-1, 0, -1), + Neighbor(-1, -1, -labels->width - 1), + Neighbor( 0, -1, -labels->width ), + Neighbor( 1, -1, -labels->width + 1), + Neighbor( 1, 0, 1), + Neighbor( 1, 1, labels->width + 1), + Neighbor( 0, 1, labels->width ), + Neighbor(-1, 1, labels->width - 1)}; + + // find one pixel with other label in the neighborhood -> assume that's the one we came from + int direction = -1; + int x; + int y; + int index; + for (unsigned dIdx = 0; dIdx < 8; ++dIdx) + { + x = curr_x + directions [dIdx].d_x; + y = curr_y + directions [dIdx].d_y; + index = curr_idx + directions [dIdx].d_index; + if (x >= 0 && x < int(labels->width) && y >= 0 && y < int(labels->height) && labels->points[index].label != label) + { + direction = dIdx; + break; + } + } + + // no connection to outer regions => start_idx is not on the border + if (direction == -1) + return; + + boundary_indices.indices.push_back (start_idx); + + do { + unsigned nIdx; + for (unsigned dIdx = 1; dIdx <= 8; ++dIdx) + { + nIdx = (direction + dIdx) & 7; + + x = curr_x + directions [nIdx].d_x; + y = curr_y + directions [nIdx].d_y; + index = curr_idx + directions [nIdx].d_index; + if (x >= 0 && x < int(labels->width) && y >= 0 && y < int(labels->height) && labels->points[index].label == label) + break; + } + + // update the direction + direction = (nIdx + 4) & 7; + curr_idx += directions [nIdx].d_index; + curr_x += directions [nIdx].d_x; + curr_y += directions [nIdx].d_y; + boundary_indices.indices.push_back(curr_idx); + } while ( curr_idx != start_idx); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedConnectedComponentSegmentation::segment (pcl::PointCloud& labels, std::vector& label_indices) const +{ + std::vector run_ids; + + unsigned invalid_label = std::numeric_limits::max (); + PointLT invalid_pt; + invalid_pt.label = std::numeric_limits::max (); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + std::size_t clust_id = 0; + + //First pixel + if (std::isfinite (input_->points[0].x)) + { + labels[0].label = clust_id++; + run_ids.push_back (labels[0].label ); + } + + // First row + for (int colIdx = 1; colIdx < static_cast (input_->width); ++colIdx) + { + if (!std::isfinite (input_->points[colIdx].x)) + continue; + if (compare_->compare (colIdx, colIdx - 1 )) + { + labels[colIdx].label = labels[colIdx - 1].label; + } + else + { + labels[colIdx].label = clust_id++; + run_ids.push_back (labels[colIdx].label ); + } + } + + // Everything else + unsigned int current_row = input_->width; + unsigned int previous_row = 0; + for (std::size_t rowIdx = 1; rowIdx < input_->height; ++rowIdx, previous_row = current_row, current_row += input_->width) + { + // First pixel + if (std::isfinite (input_->points[current_row].x)) + { + if (compare_->compare (current_row, previous_row)) + { + labels[current_row].label = labels[previous_row].label; + } + else + { + labels[current_row].label = clust_id++; + run_ids.push_back (labels[current_row].label); + } + } + + // Rest of row + for (int colIdx = 1; colIdx < static_cast (input_->width); ++colIdx) + { + if (std::isfinite (input_->points[current_row + colIdx].x)) + { + if (compare_->compare (current_row + colIdx, current_row + colIdx - 1)) + { + labels[current_row + colIdx].label = labels[current_row + colIdx - 1].label; + } + if (compare_->compare (current_row + colIdx, previous_row + colIdx) ) + { + if (labels[current_row + colIdx].label == invalid_label) + labels[current_row + colIdx].label = labels[previous_row + colIdx].label; + else if (labels[previous_row + colIdx].label != invalid_label) + { + unsigned root1 = findRoot (run_ids, labels[current_row + colIdx].label); + unsigned root2 = findRoot (run_ids, labels[previous_row + colIdx].label); + + if (root1 < root2) + run_ids[root2] = root1; + else + run_ids[root1] = root2; + } + } + + if (labels[current_row + colIdx].label == invalid_label) + { + labels[current_row + colIdx].label = clust_id++; + run_ids.push_back (labels[current_row + colIdx].label); + } + + } + } + } + + std::vector map (clust_id); + std::size_t max_id = 0; + for (std::size_t runIdx = 0; runIdx < run_ids.size (); ++runIdx) + { + // if it is its own root -> new region + if (run_ids[runIdx] == runIdx) + map[runIdx] = max_id++; + else // assign this sub-segment to the region (root) it belongs + map [runIdx] = map [findRoot (run_ids, runIdx)]; + } + + label_indices.resize (max_id + 1); + for (std::size_t idx = 0; idx < input_->points.size (); idx++) + { + if (labels[idx].label != invalid_label) + { + labels[idx].label = map[labels[idx].label]; + label_indices[labels[idx].label].indices.push_back (idx); + } + } +} + +#define PCL_INSTANTIATE_OrganizedConnectedComponentSegmentation(T,LT) template class PCL_EXPORTS pcl::OrganizedConnectedComponentSegmentation; + +#endif //#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp new file mode 100755 index 0000000..5928e92 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp @@ -0,0 +1,412 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_ +#define PCL_SEGMENTATION_IMPL_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_ + +#include +#include +#include +#include +#include + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud +projectToPlaneFromViewpoint (pcl::PointCloud& cloud, Eigen::Vector4f& normal, Eigen::Vector3f& centroid, Eigen::Vector3f& vp) +{ + Eigen::Vector3f norm (normal[0], normal[1], normal[2]); //(region.coefficients_[0], region.coefficients_[1], region.coefficients_[2]); + pcl::PointCloud projected_cloud; + projected_cloud.resize (cloud.points.size ()); + for (std::size_t i = 0; i < cloud.points.size (); i++) + { + Eigen::Vector3f pt (cloud.points[i].x, cloud.points[i].y, cloud.points[i].z); + //Eigen::Vector3f intersection = (vp, pt, norm, centroid); + float u = norm.dot ((centroid - vp)) / norm.dot ((pt - vp)); + Eigen::Vector3f intersection (vp + u * (pt - vp)); + projected_cloud[i].x = intersection[0]; + projected_cloud[i].y = intersection[1]; + projected_cloud[i].z = intersection[2]; + } + + return (projected_cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedMultiPlaneSegmentation::segment (std::vector& model_coefficients, + std::vector& inlier_indices) +{ + pcl::PointCloud labels; + std::vector label_indices; + std::vector > centroids; + std::vector > covariances; + segment (model_coefficients, inlier_indices, centroids, covariances, labels, label_indices); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedMultiPlaneSegmentation::segment (std::vector& model_coefficients, + std::vector& inlier_indices, + std::vector >& centroids, + std::vector >& covariances, + pcl::PointCloud& labels, + std::vector& label_indices) +{ + if (!initCompute ()) + return; + + // Check that we got the same number of points and normals + if (static_cast (normals_->points.size ()) != static_cast (input_->points.size ())) + { + PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%lu) and normal cloud (%lu) do not match!\n", + getClassName ().c_str (), input_->points.size (), + normals_->points.size ()); + return; + } + + // Check that the cloud is organized + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n", + getClassName ().c_str ()); + return; + } + + // Calculate range part of planes' hessian normal form + std::vector plane_d (input_->points.size ()); + + for (std::size_t i = 0; i < input_->size (); ++i) + plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ()); + + // Make a comparator + //PlaneCoefficientComparator plane_comparator (plane_d); + compare_->setPlaneCoeffD (plane_d); + compare_->setInputCloud (input_); + compare_->setInputNormals (normals_); + compare_->setAngularThreshold (static_cast (angular_threshold_)); + compare_->setDistanceThreshold (static_cast (distance_threshold_), true); + + // Set up the output + OrganizedConnectedComponentSegmentation connected_component (compare_); + connected_component.setInputCloud (input_); + connected_component.segment (labels, label_indices); + + Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero (); + Eigen::Vector4f vp = Eigen::Vector4f::Zero (); + Eigen::Matrix3f clust_cov; + pcl::ModelCoefficients model; + model.values.resize (4); + + // Fit Planes to each cluster + for (const auto &label_index : label_indices) + { + if (static_cast (label_index.indices.size ()) > min_inliers_) + { + pcl::computeMeanAndCovarianceMatrix (*input_, label_index.indices, clust_cov, clust_centroid); + Eigen::Vector4f plane_params; + + EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + pcl::eigen33 (clust_cov, eigen_value, eigen_vector); + plane_params[0] = eigen_vector[0]; + plane_params[1] = eigen_vector[1]; + plane_params[2] = eigen_vector[2]; + plane_params[3] = 0; + plane_params[3] = -1 * plane_params.dot (clust_centroid); + + vp -= clust_centroid; + float cos_theta = vp.dot (plane_params); + if (cos_theta < 0) + { + plane_params *= -1; + plane_params[3] = 0; + plane_params[3] = -1 * plane_params.dot (clust_centroid); + } + + // Compute the curvature surface change + float curvature; + float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8); + if (eig_sum != 0) + curvature = std::abs (eigen_value / eig_sum); + else + curvature = 0; + + if (curvature < maximum_curvature_) + { + model.values[0] = plane_params[0]; + model.values[1] = plane_params[1]; + model.values[2] = plane_params[2]; + model.values[3] = plane_params[3]; + model_coefficients.push_back (model); + inlier_indices.push_back (label_index); + centroids.push_back (clust_centroid); + covariances.push_back (clust_cov); + } + } + } + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedMultiPlaneSegmentation::segment (std::vector, Eigen::aligned_allocator > >& regions) +{ + std::vector model_coefficients; + std::vector inlier_indices; + PointCloudLPtr labels (new PointCloudL); + std::vector label_indices; + std::vector boundary_indices; + pcl::PointCloud boundary_cloud; + std::vector > centroids; + std::vector > covariances; + segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); + regions.resize (model_coefficients.size ()); + boundary_indices.resize (model_coefficients.size ()); + + for (std::size_t i = 0; i < model_coefficients.size (); i++) + { + boundary_cloud.resize (0); + pcl::OrganizedConnectedComponentSegmentation::findLabeledRegionBoundary (inlier_indices[i].indices[0], labels, boundary_indices[i]); + boundary_cloud.points.resize (boundary_indices[i].indices.size ()); + for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++) + boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; + + Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); + Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], + model_coefficients[i].values[1], + model_coefficients[i].values[2], + model_coefficients[i].values[3]); + regions[i] = PlanarRegion (centroid, + covariances[i], + static_cast (inlier_indices[i].indices.size ()), + boundary_cloud.points, + model); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedMultiPlaneSegmentation::segmentAndRefine (std::vector, Eigen::aligned_allocator > >& regions) +{ + std::vector model_coefficients; + std::vector inlier_indices; + PointCloudLPtr labels (new PointCloudL); + std::vector label_indices; + std::vector boundary_indices; + pcl::PointCloud boundary_cloud; + std::vector > centroids; + std::vector > covariances; + segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); + refine (model_coefficients, inlier_indices, labels, label_indices); + regions.resize (model_coefficients.size ()); + boundary_indices.resize (model_coefficients.size ()); + + for (std::size_t i = 0; i < model_coefficients.size (); i++) + { + boundary_cloud.resize (0); + int max_inlier_idx = static_cast (inlier_indices[i].indices.size ()) - 1; + pcl::OrganizedConnectedComponentSegmentation::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]); + boundary_cloud.points.resize (boundary_indices[i].indices.size ()); + for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++) + boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; + + Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); + Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], + model_coefficients[i].values[1], + model_coefficients[i].values[2], + model_coefficients[i].values[3]); + + Eigen::Vector3f vp (0.0, 0.0, 0.0); + if (project_points_) + boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp); + + regions[i] = PlanarRegion (centroid, + covariances[i], + static_cast (inlier_indices[i].indices.size ()), + boundary_cloud.points, + model); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedMultiPlaneSegmentation::segmentAndRefine (std::vector, Eigen::aligned_allocator > >& regions, + std::vector& model_coefficients, + std::vector& inlier_indices, + PointCloudLPtr& labels, + std::vector& label_indices, + std::vector& boundary_indices) +{ + pcl::PointCloud boundary_cloud; + std::vector > centroids; + std::vector > covariances; + segment (model_coefficients, inlier_indices, centroids, covariances, *labels, label_indices); + refine (model_coefficients, inlier_indices, labels, label_indices); + regions.resize (model_coefficients.size ()); + boundary_indices.resize (model_coefficients.size ()); + + for (std::size_t i = 0; i < model_coefficients.size (); i++) + { + boundary_cloud.resize (0); + int max_inlier_idx = static_cast (inlier_indices[i].indices.size ()) - 1; + pcl::OrganizedConnectedComponentSegmentation::findLabeledRegionBoundary (inlier_indices[i].indices[max_inlier_idx], labels, boundary_indices[i]); + boundary_cloud.points.resize (boundary_indices[i].indices.size ()); + for (std::size_t j = 0; j < boundary_indices[i].indices.size (); j++) + boundary_cloud.points[j] = input_->points[boundary_indices[i].indices[j]]; + + Eigen::Vector3f centroid = Eigen::Vector3f (centroids[i][0],centroids[i][1],centroids[i][2]); + Eigen::Vector4f model = Eigen::Vector4f (model_coefficients[i].values[0], + model_coefficients[i].values[1], + model_coefficients[i].values[2], + model_coefficients[i].values[3]); + + Eigen::Vector3f vp (0.0, 0.0, 0.0); + if (project_points_ && !boundary_cloud.points.empty ()) + boundary_cloud = projectToPlaneFromViewpoint (boundary_cloud, model, centroid, vp); + + regions[i] = PlanarRegion (centroid, + covariances[i], + static_cast (inlier_indices[i].indices.size ()), + boundary_cloud.points, + model); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedMultiPlaneSegmentation::refine (std::vector& model_coefficients, + std::vector& inlier_indices, + PointCloudLPtr& labels, + std::vector& label_indices) +{ + //List of labels to grow, and index of model corresponding to each label + std::vector grow_labels; + std::vector label_to_model; + grow_labels.resize (label_indices.size (), false); + label_to_model.resize (label_indices.size (), 0); + + for (std::size_t i = 0; i < model_coefficients.size (); i++) + { + int model_label = (*labels)[inlier_indices[i].indices[0]].label; + label_to_model[model_label] = static_cast (i); + grow_labels[model_label] = true; + } + + //refinement_compare_->setDistanceThreshold (0.015f, true); + refinement_compare_->setInputCloud (input_); + refinement_compare_->setLabels (labels); + refinement_compare_->setModelCoefficients (model_coefficients); + refinement_compare_->setRefineLabels (grow_labels); + refinement_compare_->setLabelToModel (label_to_model); + + //Do a first pass over the image, top to bottom, left to right + unsigned int current_row = 0; + unsigned int next_row = labels->width; + for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = next_row, next_row += labels->width) + { + + for (unsigned colIdx = 0; colIdx < labels->width - 1; ++colIdx) + { + int current_label = (*labels)[current_row+colIdx].label; + int right_label = (*labels)[current_row+colIdx+1].label; + if (current_label < 0 || right_label < 0) + continue; + + //Check right + //bool test1 = false; + if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx+1)) + { + //test1 = true; + labels->points[current_row+colIdx+1].label = current_label; + label_indices[current_label].indices.push_back (current_row+colIdx+1); + inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx+1); + } + + int lower_label = (*labels)[next_row+colIdx].label; + if (lower_label < 0) + continue; + + //Check down + if (refinement_compare_->compare (current_row+colIdx, next_row+colIdx)) + { + labels->points[next_row+colIdx].label = current_label; + label_indices[current_label].indices.push_back (next_row+colIdx); + inlier_indices[label_to_model[current_label]].indices.push_back (next_row+colIdx); + } + + }//col + }//row + + //Do a second pass over the image + current_row = labels->width * (labels->height - 1); + unsigned int prev_row = current_row - labels->width; + for (std::size_t rowIdx = 0; rowIdx < labels->height - 1; ++rowIdx, current_row = prev_row, prev_row -= labels->width) + { + for (int colIdx = labels->width - 1; colIdx >= 0; --colIdx) + { + int current_label = (*labels)[current_row+colIdx].label; + int left_label = (*labels)[current_row+colIdx-1].label; + if (current_label < 0 || left_label < 0) + continue; + + //Check left + if (refinement_compare_->compare (current_row+colIdx, current_row+colIdx-1)) + { + labels->points[current_row+colIdx-1].label = current_label; + label_indices[current_label].indices.push_back (current_row+colIdx-1); + inlier_indices[label_to_model[current_label]].indices.push_back (current_row+colIdx-1); + } + + int upper_label = (*labels)[prev_row+colIdx].label; + if (upper_label < 0) + continue; + //Check up + if (refinement_compare_->compare (current_row+colIdx, prev_row+colIdx)) + { + labels->points[prev_row+colIdx].label = current_label; + label_indices[current_label].indices.push_back (prev_row+colIdx); + inlier_indices[label_to_model[current_label]].indices.push_back (prev_row+colIdx); + } + }//col + }//row +} + +#define PCL_INSTANTIATE_OrganizedMultiPlaneSegmentation(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedMultiPlaneSegmentation; + +#endif // PCL_SEGMENTATION_IMPL_MULTI_PLANE_SEGMENTATION_H_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/planar_polygon_fusion.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/planar_polygon_fusion.hpp new file mode 100755 index 0000000..971bb3f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/planar_polygon_fusion.hpp @@ -0,0 +1,48 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEGMENTATION_PLANAR_POLYGON_FUSION_HPP_ +#define PCL_SEGMENTATION_PLANAR_POLYGON_FUSION_HPP_ + +#include + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +#define PCL_INSTANTIATE_PlanarPolygonFusion(T) template class PCL_EXPORTS pcl::PlanarPolygonFusion; + +#endif // PCL_SEGMENTATION_PLANAR_POLYGON_FUSION_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/progressive_morphological_filter.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/progressive_morphological_filter.hpp new file mode 100755 index 0000000..b3c6849 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/progressive_morphological_filter.hpp @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ +#define PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ProgressiveMorphologicalFilter::ProgressiveMorphologicalFilter () : + max_window_size_ (33), + slope_ (0.7f), + max_distance_ (10.0f), + initial_distance_ (0.15f), + cell_size_ (1.0f), + base_ (2.0f), + exponential_ (true) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::ProgressiveMorphologicalFilter::~ProgressiveMorphologicalFilter () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ProgressiveMorphologicalFilter::extract (std::vector& ground) +{ + bool segmentation_is_possible = initCompute (); + if (!segmentation_is_possible) + { + deinitCompute (); + return; + } + + // Compute the series of window sizes and height thresholds + std::vector height_thresholds; + std::vector window_sizes; + int iteration = 0; + float window_size = 0.0f; + float height_threshold = 0.0f; + + while (window_size < max_window_size_) + { + // Determine the initial window size. + if (exponential_) + window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f); + else + window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f); + + // Calculate the height threshold to be used in the next iteration. + if (iteration == 0) + height_threshold = initial_distance_; + else + height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_; + + // Enforce max distance on height threshold + if (height_threshold > max_distance_) + height_threshold = max_distance_; + + window_sizes.push_back (window_size); + height_thresholds.push_back (height_threshold); + + iteration++; + } + + // Ground indices are initially limited to those points in the input cloud we + // wish to process + ground = *indices_; + + // Progressively filter ground returns using morphological open + for (std::size_t i = 0; i < window_sizes.size (); ++i) + { + PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f)...", + i, height_thresholds[i], window_sizes[i]); + + // Limit filtering to those points currently considered ground returns + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::copyPointCloud (*input_, ground, *cloud); + + // Create new cloud to hold the filtered results. Apply the morphological + // opening operation at the current window size. + typename pcl::PointCloud::Ptr cloud_f (new pcl::PointCloud); + pcl::applyMorphologicalOperator (cloud, window_sizes[i], MORPH_OPEN, *cloud_f); + + // Find indices of the points whose difference between the source and + // filtered point clouds is less than the current height threshold. + std::vector pt_indices; + for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx) + { + float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z; + if (diff < height_thresholds[i]) + pt_indices.push_back (ground[p_idx]); + } + + // Ground is now limited to pt_indices + ground.swap (pt_indices); + + PCL_DEBUG ("ground now has %d points\n", ground.size ()); + } + + deinitCompute (); +} + +#define PCL_INSTANTIATE_ProgressiveMorphologicalFilter(T) template class pcl::ProgressiveMorphologicalFilter; + +#endif // PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ + diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/random_walker.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/random_walker.hpp new file mode 100755 index 0000000..feef7cf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/random_walker.hpp @@ -0,0 +1,348 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP +#define PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP + +#include + +#include + +namespace pcl +{ + + namespace segmentation + { + + namespace detail + { + + /** \brief Multilabel graph segmentation using random walks. + * + * This is an implementation of the algorithm described in "Random Walks + * for Image Segmentation" by Leo Grady. + * + * See the documentation of the randomWalker() function for details. + * + * \author Sergey Alexandrov + * \ingroup segmentation + */ + template + class RandomWalker + { + + public: + + using Color = typename boost::property_traits::value_type; + using Weight = typename boost::property_traits::value_type; + using GraphTraits = boost::graph_traits; + using EdgeDescriptor = typename GraphTraits::edge_descriptor; + using VertexDescriptor = typename GraphTraits::vertex_descriptor; + using EdgeIterator = typename GraphTraits::edge_iterator; + using OutEdgeIterator = typename GraphTraits::out_edge_iterator; + using VertexIterator = typename GraphTraits::vertex_iterator; + using VertexIndexMap = typename boost::property_map::type; + using VertexDegreeMap = boost::iterator_property_map::iterator, VertexIndexMap>; + using SparseMatrix = Eigen::SparseMatrix; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + + RandomWalker (Graph& g, EdgeWeightMap weights, VertexColorMap colors) + : g_ (g) + , weight_map_ (weights) + , color_map_ (colors) + , index_map_ (boost::get (boost::vertex_index, g_)) + , degree_storage_ (boost::num_vertices (g_), 0) + , degree_map_ (boost::make_iterator_property_map (degree_storage_.begin (), index_map_)) + { + } + + bool + segment () + { + computeVertexDegrees (); + buildLinearSystem (); + return solveLinearSystem (); + } + + void + computeVertexDegrees () + { + using namespace boost; + EdgeIterator ei, e_end; + for (tie (ei, e_end) = edges (g_); ei != e_end; ++ei) + { + Weight w = weight_map_[*ei]; + degree_map_[source (*ei, g_)] += w; + degree_map_[target (*ei, g_)] += w; + } + } + + void + buildLinearSystem () + { + using namespace boost; + + using T = Eigen::Triplet; + using Triplets = std::vector; + Triplets L_triplets; + Triplets B_triplets; + + VertexIterator vi, v_end; + for (tie (vi, v_end) = vertices (g_); vi != v_end; ++vi) + { + // If this is a labeled vertex add it to the seeds list and register its color + if (color_map_[*vi]) + { + seeds_.push_back (*vi); + colors_.insert (color_map_[*vi]); + } + // Skip seeds and vertices with zero connectivity + if (color_map_[*vi] || std::fabs (degree_map_[*vi]) < std::numeric_limits::epsilon ()) + continue; + // Create a row in L matrix for the vertex + std::size_t current_row = insertInBimap (L_vertex_bimap, *vi); + // Add diagonal degree entry for the vertex + L_triplets.push_back (T (current_row, current_row, degree_map_[*vi])); + // Iterate over incident vertices and add entries on corresponding columns of L or B + OutEdgeIterator ei, e_end; + for (tie (ei, e_end) = out_edges (*vi, g_); ei != e_end; ++ei) + { + Weight w = weight_map_[*ei]; + VertexDescriptor tgt = target (*ei, g_); + Color color = color_map_[tgt]; + if (color) + { + // This is a seed and will go to B matrix + std::size_t column; + if (B_color_bimap.right.count (color) == 0) + { + // This is the first time we encountered this color, create a new column in B + column = insertInBimap (B_color_bimap, color); + } + else + { + column = B_color_bimap.right.at (color); + } + B_triplets.push_back (T (current_row, column, w)); + } + else + { + // This is a non-seed and will go to L matrix, + // but only if a row for this vertex already exists + if (L_vertex_bimap.right.count (tgt) && L_vertex_bimap.right.at (tgt) != current_row) + { + L_triplets.push_back (T (current_row, L_vertex_bimap.right.at (tgt), -w)); + } + } + } + } + + std::size_t num_equations = L_vertex_bimap.size (); + std::size_t num_colors = B_color_bimap.size (); + L.resize (num_equations, num_equations); + B.resize (num_equations, num_colors); + if (!L_triplets.empty ()) + L.setFromTriplets(L_triplets.begin(), L_triplets.end()); + if (!B_triplets.empty ()) + B.setFromTriplets(B_triplets.begin(), B_triplets.end()); + } + + bool solveLinearSystem() + { + X.resize (L.rows (), B.cols ()); + + // Nothing to solve + if (L.rows () == 0 || B.cols () == 0) + return true; + + Eigen::SimplicialCholesky cg; + cg.compute (L); + bool succeeded = true; + for (Eigen::Index i = 0; i < B.cols (); ++i) + { + Vector b = B.col (i); + X.col (i) = cg.solve (b); + if (cg.info () != Eigen::Success) + succeeded = false; + } + + assignColors (); + return succeeded; + } + + void + assignColors () + { + using namespace boost; + if (X.cols ()) + for (Eigen::Index i = 0; i < X.rows (); ++i) + { + std::size_t max_column; + X.row (i).maxCoeff (&max_column); + VertexDescriptor vertex = L_vertex_bimap.left.at (i); + Color color = B_color_bimap.left.at (max_column); + color_map_[vertex] = color; + } + } + + void + getPotentials (Matrix& potentials, std::map& color_to_column_map) + { + using namespace boost; + potentials = Matrix::Zero (num_vertices (g_), colors_.size ()); + // Copy over rows from X + for (Eigen::Index i = 0; i < X.rows (); ++i) + potentials.row (L_vertex_bimap.left.at (i)).head (X.cols ()) = X.row (i); + // In rows that correspond to seeds put ones in proper columns + for (std::size_t i = 0; i < seeds_.size (); ++i) + { + VertexDescriptor v = seeds_[i]; + insertInBimap (B_color_bimap, color_map_[v]); + potentials (seeds_[i], B_color_bimap.right.at (color_map_[seeds_[i]])) = 1; + } + // Fill in a map that associates colors with columns in potentials matrix + color_to_column_map.clear (); + for (Eigen::Index i = 0; i < potentials.cols (); ++i) + color_to_column_map[B_color_bimap.left.at (i)] = i; + } + + template static inline std::size_t + insertInBimap (boost::bimap& bimap, T value) + { + if (bimap.right.count (value) != 0) + { + return bimap.right.at (value); + } + std::size_t s = bimap.size (); + bimap.insert (typename boost::bimap::value_type (s, value)); + return s; + } + + Graph& g_; + EdgeWeightMap weight_map_; + VertexColorMap color_map_; + VertexIndexMap index_map_; + + std::vector seeds_; + std::set colors_; + + std::vector degree_storage_; + VertexDegreeMap degree_map_; + SparseMatrix L; + SparseMatrix B; + Matrix X; + + // Map vertex identifiers to the rows/columns of L and vice versa + boost::bimap L_vertex_bimap; + // Map colors to the columns of B and vice versa + boost::bimap B_color_bimap; + + }; + + } + + template bool + randomWalker (Graph& graph) + { + return randomWalker (graph, + boost::get (boost::edge_weight, graph), + boost::get (boost::vertex_color, graph)); + } + + template bool + randomWalker (Graph& graph, + EdgeWeightMap weights, + VertexColorMap colors) + { + using namespace boost; + + using EdgeDescriptor = typename graph_traits::edge_descriptor; + using VertexDescriptor = typename graph_traits::vertex_descriptor; + + BOOST_CONCEPT_ASSERT ((VertexListGraphConcept)); // to have vertices(), num_vertices() + BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept)); // to have edges() + BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept)); // to have source(), target() and out_edges() + BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept)); // read weight-values from edges + BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept)); // read and write color-values from vertices + + ::pcl::segmentation::detail::RandomWalker + < + Graph, + EdgeWeightMap, + VertexColorMap + > + rw (graph, weights, colors); + return rw.segment (); + } + + template bool + randomWalker (Graph& graph, + EdgeWeightMap weights, + VertexColorMap colors, + Eigen::Matrix::value_type, Eigen::Dynamic, Eigen::Dynamic>& potentials, + std::map::value_type, std::size_t>& colors_to_columns_map) + { + using namespace boost; + + using EdgeDescriptor = typename graph_traits::edge_descriptor; + using VertexDescriptor = typename graph_traits::vertex_descriptor; + + BOOST_CONCEPT_ASSERT ((VertexListGraphConcept)); // to have vertices(), num_vertices() + BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept)); // to have edges() + BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept)); // to have source(), target() and out_edges() + BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept)); // read weight-values from edges + BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept)); // read and write color-values from vertices + + ::pcl::segmentation::detail::RandomWalker + < + Graph, + EdgeWeightMap, + VertexColorMap + > + rw (graph, weights, colors); + bool result = rw.segment (); + rw.getPotentials (potentials, colors_to_columns_map); + return result; + } + + } + +} + +#endif /* PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP */ + diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/region_growing.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/region_growing.hpp new file mode 100755 index 0000000..f65ad66 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/region_growing.hpp @@ -0,0 +1,744 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : mine_all_mine@bk.ru + * + */ + +#ifndef PCL_SEGMENTATION_REGION_GROWING_HPP_ +#define PCL_SEGMENTATION_REGION_GROWING_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::RegionGrowing::RegionGrowing () : + min_pts_per_cluster_ (1), + max_pts_per_cluster_ (std::numeric_limits::max ()), + smooth_mode_flag_ (true), + curvature_flag_ (true), + residual_flag_ (false), + theta_threshold_ (30.0f / 180.0f * static_cast (M_PI)), + residual_threshold_ (0.05f), + curvature_threshold_ (0.05f), + neighbour_number_ (30), + search_ (), + normals_ (), + point_neighbours_ (0), + point_labels_ (0), + normal_flag_ (true), + num_pts_in_segment_ (0), + clusters_ (0), + number_of_segments_ (0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::RegionGrowing::~RegionGrowing () +{ + point_neighbours_.clear (); + point_labels_.clear (); + num_pts_in_segment_.clear (); + clusters_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::RegionGrowing::getMinClusterSize () +{ + return (min_pts_per_cluster_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setMinClusterSize (int min_cluster_size) +{ + min_pts_per_cluster_ = min_cluster_size; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::RegionGrowing::getMaxClusterSize () +{ + return (max_pts_per_cluster_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setMaxClusterSize (int max_cluster_size) +{ + max_pts_per_cluster_ = max_cluster_size; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::getSmoothModeFlag () const +{ + return (smooth_mode_flag_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setSmoothModeFlag (bool value) +{ + smooth_mode_flag_ = value; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::getCurvatureTestFlag () const +{ + return (curvature_flag_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setCurvatureTestFlag (bool value) +{ + curvature_flag_ = value; + + if (curvature_flag_ == false && residual_flag_ == false) + residual_flag_ = true; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::getResidualTestFlag () const +{ + return (residual_flag_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setResidualTestFlag (bool value) +{ + residual_flag_ = value; + + if (curvature_flag_ == false && residual_flag_ == false) + curvature_flag_ = true; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowing::getSmoothnessThreshold () const +{ + return (theta_threshold_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setSmoothnessThreshold (float theta) +{ + theta_threshold_ = theta; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowing::getResidualThreshold () const +{ + return (residual_threshold_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setResidualThreshold (float residual) +{ + residual_threshold_ = residual; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowing::getCurvatureThreshold () const +{ + return (curvature_threshold_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setCurvatureThreshold (float curvature) +{ + curvature_threshold_ = curvature; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::RegionGrowing::getNumberOfNeighbours () const +{ + return (neighbour_number_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setNumberOfNeighbours (unsigned int neighbour_number) +{ + neighbour_number_ = neighbour_number; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::RegionGrowing::KdTreePtr +pcl::RegionGrowing::getSearchMethod () const +{ + return (search_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setSearchMethod (const KdTreePtr& tree) +{ + search_ = tree; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::RegionGrowing::NormalPtr +pcl::RegionGrowing::getInputNormals () const +{ + return (normals_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::setInputNormals (const NormalPtr& norm) +{ + normals_ = norm; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::extract (std::vector & clusters) +{ + clusters_.clear (); + clusters.clear (); + point_neighbours_.clear (); + point_labels_.clear (); + num_pts_in_segment_.clear (); + number_of_segments_ = 0; + + bool segmentation_is_possible = initCompute (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + segmentation_is_possible = prepareForSegmentation (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + findPointNeighbours (); + applySmoothRegionGrowingAlgorithm (); + assembleRegions (); + + clusters.resize (clusters_.size ()); + std::vector::iterator cluster_iter_input = clusters.begin (); + for (std::vector::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); ++cluster_iter) + { + if ((static_cast (cluster_iter->indices.size ()) >= min_pts_per_cluster_) && + (static_cast (cluster_iter->indices.size ()) <= max_pts_per_cluster_)) + { + *cluster_iter_input = *cluster_iter; + ++cluster_iter_input; + } + } + + clusters_ = std::vector (clusters.begin (), cluster_iter_input); + clusters.resize(clusters_.size()); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::prepareForSegmentation () +{ + // if user forgot to pass point cloud or if it is empty + if ( input_->points.empty () ) + return (false); + + // if user forgot to pass normals or the sizes of point and normal cloud are different + if ( !normals_ || input_->points.size () != normals_->points.size () ) + return (false); + + // if residual test is on then we need to check if all needed parameters were correctly initialized + if (residual_flag_) + { + if (residual_threshold_ <= 0.0f) + return (false); + } + + // if curvature test is on ... + // if (curvature_flag_) + // { + // in this case we do not need to check anything that related to it + // so we simply commented it + // } + + // from here we check those parameters that are always valuable + if (neighbour_number_ == 0) + return (false); + + // if user didn't set search method + if (!search_) + search_.reset (new pcl::search::KdTree); + + if (indices_) + { + if (indices_->empty ()) + PCL_ERROR ("[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n"); + search_->setInputCloud (input_, indices_); + } + else + search_->setInputCloud (input_); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::findPointNeighbours () +{ + int point_number = static_cast (indices_->size ()); + std::vector neighbours; + std::vector distances; + + point_neighbours_.resize (input_->points.size (), neighbours); + if (input_->is_dense) + { + for (int i_point = 0; i_point < point_number; i_point++) + { + int point_index = (*indices_)[i_point]; + neighbours.clear (); + search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); + point_neighbours_[point_index].swap (neighbours); + } + } + else + { + for (int i_point = 0; i_point < point_number; i_point++) + { + neighbours.clear (); + int point_index = (*indices_)[i_point]; + if (!pcl::isFinite (input_->points[point_index])) + continue; + search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); + point_neighbours_[point_index].swap (neighbours); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::applySmoothRegionGrowingAlgorithm () +{ + int num_of_pts = static_cast (indices_->size ()); + point_labels_.resize (input_->points.size (), -1); + + std::vector< std::pair > point_residual; + std::pair pair; + point_residual.resize (num_of_pts, pair); + + if (normal_flag_ == true) + { + for (int i_point = 0; i_point < num_of_pts; i_point++) + { + int point_index = (*indices_)[i_point]; + point_residual[i_point].first = normals_->points[point_index].curvature; + point_residual[i_point].second = point_index; + } + std::sort (point_residual.begin (), point_residual.end (), comparePair); + } + else + { + for (int i_point = 0; i_point < num_of_pts; i_point++) + { + int point_index = (*indices_)[i_point]; + point_residual[i_point].first = 0; + point_residual[i_point].second = point_index; + } + } + int seed_counter = 0; + int seed = point_residual[seed_counter].second; + + int segmented_pts_num = 0; + int number_of_segments = 0; + while (segmented_pts_num < num_of_pts) + { + int pts_in_segment; + pts_in_segment = growRegion (seed, number_of_segments); + segmented_pts_num += pts_in_segment; + num_pts_in_segment_.push_back (pts_in_segment); + number_of_segments++; + + //find next point that is not segmented yet + for (int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++) + { + int index = point_residual[i_seed].second; + if (point_labels_[index] == -1) + { + seed = index; + seed_counter = i_seed; + break; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::RegionGrowing::growRegion (int initial_seed, int segment_number) +{ + std::queue seeds; + seeds.push (initial_seed); + point_labels_[initial_seed] = segment_number; + + int num_pts_in_segment = 1; + + while (!seeds.empty ()) + { + int curr_seed; + curr_seed = seeds.front (); + seeds.pop (); + + std::size_t i_nghbr = 0; + while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () ) + { + int index = point_neighbours_[curr_seed][i_nghbr]; + if (point_labels_[index] != -1) + { + i_nghbr++; + continue; + } + + bool is_a_seed = false; + bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed); + + if (!belongs_to_segment) + { + i_nghbr++; + continue; + } + + point_labels_[index] = segment_number; + num_pts_in_segment++; + + if (is_a_seed) + { + seeds.push (index); + } + + i_nghbr++; + }// next neighbour + }// next seed + + return (num_pts_in_segment); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowing::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const +{ + is_a_seed = true; + + float cosine_threshold = std::cos (theta_threshold_); + float data[4]; + + data[0] = input_->points[point].data[0]; + data[1] = input_->points[point].data[1]; + data[2] = input_->points[point].data[2]; + data[3] = input_->points[point].data[3]; + Eigen::Map initial_point (static_cast (data)); + Eigen::Map initial_normal (static_cast (normals_->points[point].normal)); + + //check the angle between normals + if (smooth_mode_flag_ == true) + { + Eigen::Map nghbr_normal (static_cast (normals_->points[nghbr].normal)); + float dot_product = std::abs (nghbr_normal.dot (initial_normal)); + if (dot_product < cosine_threshold) + { + return (false); + } + } + else + { + Eigen::Map nghbr_normal (static_cast (normals_->points[nghbr].normal)); + Eigen::Map initial_seed_normal (static_cast (normals_->points[initial_seed].normal)); + float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal)); + if (dot_product < cosine_threshold) + return (false); + } + + // check the curvature if needed + if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) + { + is_a_seed = false; + } + + // check the residual if needed + float data_1[4]; + + data_1[0] = input_->points[nghbr].data[0]; + data_1[1] = input_->points[nghbr].data[1]; + data_1[2] = input_->points[nghbr].data[2]; + data_1[3] = input_->points[nghbr].data[3]; + Eigen::Map nghbr_point (static_cast (data_1)); + float residual = std::abs (initial_normal.dot (initial_point - nghbr_point)); + if (residual_flag_ && residual > residual_threshold_) + is_a_seed = false; + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::assembleRegions () +{ + int number_of_segments = static_cast (num_pts_in_segment_.size ()); + int number_of_points = static_cast (input_->points.size ()); + + pcl::PointIndices segment; + clusters_.resize (number_of_segments, segment); + + for (int i_seg = 0; i_seg < number_of_segments; i_seg++) + { + clusters_[i_seg].indices.resize ( num_pts_in_segment_[i_seg], 0); + } + + std::vector counter; + counter.resize (number_of_segments, 0); + + for (int i_point = 0; i_point < number_of_points; i_point++) + { + int segment_index = point_labels_[i_point]; + if (segment_index != -1) + { + int point_index = counter[segment_index]; + clusters_[segment_index].indices[point_index] = i_point; + counter[segment_index] = point_index + 1; + } + } + + number_of_segments_ = number_of_segments; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowing::getSegmentFromPoint (int index, pcl::PointIndices& cluster) +{ + cluster.indices.clear (); + + bool segmentation_is_possible = initCompute (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + // first of all we need to find out if this point belongs to cloud + bool point_was_found = false; + int number_of_points = static_cast (indices_->size ()); + for (int point = 0; point < number_of_points; point++) + if ( (*indices_)[point] == index) + { + point_was_found = true; + break; + } + + if (point_was_found) + { + if (clusters_.empty ()) + { + point_neighbours_.clear (); + point_labels_.clear (); + num_pts_in_segment_.clear (); + number_of_segments_ = 0; + + segmentation_is_possible = prepareForSegmentation (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + findPointNeighbours (); + applySmoothRegionGrowingAlgorithm (); + assembleRegions (); + } + // if we have already made the segmentation, then find the segment + // to which this point belongs + for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++) + { + bool segment_was_found = false; + for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++) + { + if (i_segment->indices[i_point] == index) + { + segment_was_found = true; + cluster.indices.clear (); + cluster.indices.reserve (i_segment->indices.size ()); + std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices)); + break; + } + } + if (segment_was_found) + { + break; + } + }// next segment + }// end if point was found + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::RegionGrowing::getColoredCloud () +{ + pcl::PointCloud::Ptr colored_cloud; + + if (!clusters_.empty ()) + { + colored_cloud = (new pcl::PointCloud)->makeShared (); + + srand (static_cast (time (nullptr))); + std::vector colors; + for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++) + { + colors.push_back (static_cast (rand () % 256)); + colors.push_back (static_cast (rand () % 256)); + colors.push_back (static_cast (rand () % 256)); + } + + colored_cloud->width = input_->width; + colored_cloud->height = input_->height; + colored_cloud->is_dense = input_->is_dense; + for (std::size_t i_point = 0; i_point < input_->points.size (); i_point++) + { + pcl::PointXYZRGB point; + point.x = *(input_->points[i_point].data); + point.y = *(input_->points[i_point].data + 1); + point.z = *(input_->points[i_point].data + 2); + point.r = 255; + point.g = 0; + point.b = 0; + colored_cloud->points.push_back (point); + } + + int next_color = 0; + for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++) + { + for (auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++) + { + int index; + index = *i_point; + colored_cloud->points[index].r = colors[3 * next_color]; + colored_cloud->points[index].g = colors[3 * next_color + 1]; + colored_cloud->points[index].b = colors[3 * next_color + 2]; + } + next_color++; + } + } + + return (colored_cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::RegionGrowing::getColoredCloudRGBA () +{ + pcl::PointCloud::Ptr colored_cloud; + + if (!clusters_.empty ()) + { + colored_cloud = (new pcl::PointCloud)->makeShared (); + + srand (static_cast (time (nullptr))); + std::vector colors; + for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++) + { + colors.push_back (static_cast (rand () % 256)); + colors.push_back (static_cast (rand () % 256)); + colors.push_back (static_cast (rand () % 256)); + } + + colored_cloud->width = input_->width; + colored_cloud->height = input_->height; + colored_cloud->is_dense = input_->is_dense; + for (std::size_t i_point = 0; i_point < input_->points.size (); i_point++) + { + pcl::PointXYZRGBA point; + point.x = *(input_->points[i_point].data); + point.y = *(input_->points[i_point].data + 1); + point.z = *(input_->points[i_point].data + 2); + point.r = 255; + point.g = 0; + point.b = 0; + point.a = 0; + colored_cloud->points.push_back (point); + } + + int next_color = 0; + for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++) + { + for (auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++) + { + int index = *i_point; + colored_cloud->points[index].r = colors[3 * next_color]; + colored_cloud->points[index].g = colors[3 * next_color + 1]; + colored_cloud->points[index].b = colors[3 * next_color + 2]; + } + next_color++; + } + } + + return (colored_cloud); +} + +#define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing; + +#endif // PCL_SEGMENTATION_REGION_GROWING_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/region_growing_rgb.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/region_growing_rgb.hpp new file mode 100755 index 0000000..14b232e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/region_growing_rgb.hpp @@ -0,0 +1,765 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : mine_all_mine@bk.ru + * + */ + +#ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ +#define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ + +#include +#include +#include + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::RegionGrowingRGB::RegionGrowingRGB () : + color_p2p_threshold_ (1225.0f), + color_r2r_threshold_ (10.0f), + distance_threshold_ (0.05f), + region_neighbour_number_ (100), + point_distances_ (0), + segment_neighbours_ (0), + segment_distances_ (0), + segment_labels_ (0) +{ + normal_flag_ = false; + curvature_flag_ = false; + residual_flag_ = false; + min_pts_per_cluster_ = 10; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::RegionGrowingRGB::~RegionGrowingRGB () +{ + point_distances_.clear (); + segment_neighbours_.clear (); + segment_distances_.clear (); + segment_labels_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowingRGB::getPointColorThreshold () const +{ + return (powf (color_p2p_threshold_, 0.5f)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::setPointColorThreshold (float thresh) +{ + color_p2p_threshold_ = thresh * thresh; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowingRGB::getRegionColorThreshold () const +{ + return (powf (color_r2r_threshold_, 0.5f)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::setRegionColorThreshold (float thresh) +{ + color_r2r_threshold_ = thresh * thresh; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowingRGB::getDistanceThreshold () const +{ + return (powf (distance_threshold_, 0.5f)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::setDistanceThreshold (float thresh) +{ + distance_threshold_ = thresh * thresh; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template unsigned int +pcl::RegionGrowingRGB::getNumberOfRegionNeighbours () const +{ + return (region_neighbour_number_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::setNumberOfRegionNeighbours (unsigned int nghbr_number) +{ + region_neighbour_number_ = nghbr_number; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowingRGB::getNormalTestFlag () const +{ + return (normal_flag_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::setNormalTestFlag (bool value) +{ + normal_flag_ = value; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::setCurvatureTestFlag (bool value) +{ + curvature_flag_ = value; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::setResidualTestFlag (bool value) +{ + residual_flag_ = value; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::extract (std::vector & clusters) +{ + clusters_.clear (); + clusters.clear (); + point_neighbours_.clear (); + point_labels_.clear (); + num_pts_in_segment_.clear (); + point_distances_.clear (); + segment_neighbours_.clear (); + segment_distances_.clear (); + segment_labels_.clear (); + number_of_segments_ = 0; + + bool segmentation_is_possible = initCompute (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + segmentation_is_possible = prepareForSegmentation (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + findPointNeighbours (); + applySmoothRegionGrowingAlgorithm (); + RegionGrowing::assembleRegions (); + + findSegmentNeighbours (); + applyRegionMergingAlgorithm (); + + std::vector::iterator cluster_iter = clusters_.begin (); + while (cluster_iter != clusters_.end ()) + { + if (static_cast (cluster_iter->indices.size ()) < min_pts_per_cluster_ || + static_cast (cluster_iter->indices.size ()) > max_pts_per_cluster_) + { + cluster_iter = clusters_.erase (cluster_iter); + } + else + ++cluster_iter; + } + + clusters.reserve (clusters_.size ()); + std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters)); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowingRGB::prepareForSegmentation () +{ + // if user forgot to pass point cloud or if it is empty + if ( input_->points.empty () ) + return (false); + + // if normal/smoothness test is on then we need to check if all needed variables and parameters + // were correctly initialized + if (normal_flag_) + { + // if user forgot to pass normals or the sizes of point and normal cloud are different + if ( !normals_ || input_->points.size () != normals_->points.size () ) + return (false); + } + + // if residual test is on then we need to check if all needed parameters were correctly initialized + if (residual_flag_) + { + if (residual_threshold_ <= 0.0f) + return (false); + } + + // if curvature test is on ... + // if (curvature_flag_) + // { + // in this case we do not need to check anything that related to it + // so we simply commented it + // } + + // here we check the parameters related to color-based segmentation + if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f ) + return (false); + + // from here we check those parameters that are always valuable + if (neighbour_number_ == 0) + return (false); + + // if user didn't set search method + if (!search_) + search_.reset (new pcl::search::KdTree); + + if (indices_) + { + if (indices_->empty ()) + PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n"); + search_->setInputCloud (input_, indices_); + } + else + search_->setInputCloud (input_); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::findPointNeighbours () +{ + int point_number = static_cast (indices_->size ()); + std::vector neighbours; + std::vector distances; + + point_neighbours_.resize (input_->points.size (), neighbours); + point_distances_.resize (input_->points.size (), distances); + + for (int i_point = 0; i_point < point_number; i_point++) + { + int point_index = (*indices_)[i_point]; + neighbours.clear (); + distances.clear (); + search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances); + point_neighbours_[point_index].swap (neighbours); + point_distances_[point_index].swap (distances); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::findSegmentNeighbours () +{ + std::vector neighbours; + std::vector distances; + segment_neighbours_.resize (number_of_segments_, neighbours); + segment_distances_.resize (number_of_segments_, distances); + + for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) + { + std::vector nghbrs; + std::vector dist; + findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist); + segment_neighbours_[i_seg].swap (nghbrs); + segment_distances_[i_seg].swap (dist); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::findRegionsKNN (int index, int nghbr_number, std::vector& nghbrs, std::vector& dist) +{ + std::vector distances; + float max_dist = std::numeric_limits::max (); + distances.resize (clusters_.size (), max_dist); + + int number_of_points = num_pts_in_segment_[index]; + //loop through every point in this segment and check neighbours + for (int i_point = 0; i_point < number_of_points; i_point++) + { + int point_index = clusters_[index].indices[i_point]; + int number_of_neighbours = static_cast (point_neighbours_[point_index].size ()); + //loop through every neighbour of the current point, find out to which segment it belongs + //and if it belongs to neighbouring segment and is close enough then remember segment and its distance + for (int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++) + { + // find segment + int segment_index = -1; + segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ]; + + if ( segment_index != index ) + { + // try to push it to the queue + if (distances[segment_index] > point_distances_[point_index][i_nghbr]) + distances[segment_index] = point_distances_[point_index][i_nghbr]; + } + } + }// next point + + std::priority_queue > segment_neighbours; + for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) + { + if (distances[i_seg] < max_dist) + { + segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) ); + if (int (segment_neighbours.size ()) > nghbr_number) + segment_neighbours.pop (); + } + } + + int size = std::min (static_cast (segment_neighbours.size ()), nghbr_number); + nghbrs.resize (size, 0); + dist.resize (size, 0); + int counter = 0; + while ( !segment_neighbours.empty () && counter < nghbr_number ) + { + dist[counter] = segment_neighbours.top ().first; + nghbrs[counter] = segment_neighbours.top ().second; + segment_neighbours.pop (); + counter++; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::applyRegionMergingAlgorithm () +{ + int number_of_points = static_cast (indices_->size ()); + + // calculate color of each segment + std::vector< std::vector > segment_color; + std::vector color; + color.resize (3, 0); + segment_color.resize (number_of_segments_, color); + + for (int i_point = 0; i_point < number_of_points; i_point++) + { + int point_index = (*indices_)[i_point]; + int segment_index = point_labels_[point_index]; + segment_color[segment_index][0] += input_->points[point_index].r; + segment_color[segment_index][1] += input_->points[point_index].g; + segment_color[segment_index][2] += input_->points[point_index].b; + } + for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) + { + segment_color[i_seg][0] = static_cast (static_cast (segment_color[i_seg][0]) / static_cast (num_pts_in_segment_[i_seg])); + segment_color[i_seg][1] = static_cast (static_cast (segment_color[i_seg][1]) / static_cast (num_pts_in_segment_[i_seg])); + segment_color[i_seg][2] = static_cast (static_cast (segment_color[i_seg][2]) / static_cast (num_pts_in_segment_[i_seg])); + } + + // now it is time to find out if there are segments with a similar color + // and merge them together + std::vector num_pts_in_homogeneous_region; + std::vector num_seg_in_homogeneous_region; + + segment_labels_.resize (number_of_segments_, -1); + + float dist_thresh = distance_threshold_; + int homogeneous_region_number = 0; + for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) + { + int curr_homogeneous_region = 0; + if (segment_labels_[i_seg] == -1) + { + segment_labels_[i_seg] = homogeneous_region_number; + curr_homogeneous_region = homogeneous_region_number; + num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]); + num_seg_in_homogeneous_region.push_back (1); + homogeneous_region_number++; + } + else + curr_homogeneous_region = segment_labels_[i_seg]; + + unsigned int i_nghbr = 0; + while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () ) + { + int index = segment_neighbours_[i_seg][i_nghbr]; + if (segment_distances_[i_seg][i_nghbr] > dist_thresh) + { + i_nghbr++; + continue; + } + if ( segment_labels_[index] == -1 ) + { + float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]); + if (difference < color_r2r_threshold_) + { + segment_labels_[index] = curr_homogeneous_region; + num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index]; + num_seg_in_homogeneous_region[curr_homogeneous_region] += 1; + } + } + i_nghbr++; + }// next neighbour + }// next segment + + segment_color.clear (); + color.clear (); + + std::vector< std::vector > final_segments; + std::vector region; + final_segments.resize (homogeneous_region_number, region); + for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++) + { + final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0); + } + + std::vector counter; + counter.resize (homogeneous_region_number, 0); + for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) + { + int index = segment_labels_[i_seg]; + final_segments[ index ][ counter[index] ] = i_seg; + counter[index] += 1; + } + + std::vector< std::vector< std::pair > > region_neighbours; + findRegionNeighbours (region_neighbours, final_segments); + + int final_segment_number = homogeneous_region_number; + for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++) + { + if (static_cast (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_) + { + if ( region_neighbours[i_reg].empty () ) + continue; + int nearest_neighbour = region_neighbours[i_reg][0].second; + if ( region_neighbours[i_reg][0].first == std::numeric_limits::max () ) + continue; + int reg_index = segment_labels_[nearest_neighbour]; + int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg]; + for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++) + { + int segment_index = final_segments[i_reg][i_seg]; + final_segments[reg_index].push_back (segment_index); + segment_labels_[segment_index] = reg_index; + } + final_segments[i_reg].clear (); + num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg]; + num_pts_in_homogeneous_region[i_reg] = 0; + num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg]; + num_seg_in_homogeneous_region[i_reg] = 0; + final_segment_number -= 1; + + int nghbr_number = static_cast (region_neighbours[reg_index].size ()); + for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) + { + if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index ) + { + region_neighbours[reg_index][i_nghbr].first = std::numeric_limits::max (); + region_neighbours[reg_index][i_nghbr].second = 0; + } + } + nghbr_number = static_cast (region_neighbours[i_reg].size ()); + for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) + { + if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index ) + { + std::pair pair; + pair.first = region_neighbours[i_reg][i_nghbr].first; + pair.second = region_neighbours[i_reg][i_nghbr].second; + region_neighbours[reg_index].push_back (pair); + } + } + region_neighbours[i_reg].clear (); + std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair); + } + } + + assembleRegions (num_pts_in_homogeneous_region, static_cast (num_pts_in_homogeneous_region.size ())); + + number_of_segments_ = final_segment_number; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::RegionGrowingRGB::calculateColorimetricalDifference (std::vector& first_color, std::vector& second_color) const +{ + float difference = 0.0f; + difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0])); + difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1])); + difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2])); + return (difference); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::findRegionNeighbours (std::vector< std::vector< std::pair > >& neighbours_out, std::vector< std::vector >& regions_in) +{ + int region_number = static_cast (regions_in.size ()); + neighbours_out.clear (); + neighbours_out.resize (region_number); + + for (int i_reg = 0; i_reg < region_number; i_reg++) + { + int segment_num = static_cast (regions_in[i_reg].size ()); + neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_); + for (int i_seg = 0; i_seg < segment_num; i_seg++) + { + int curr_segment = regions_in[i_reg][i_seg]; + int nghbr_number = static_cast (segment_neighbours_[curr_segment].size ()); + std::pair pair; + for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) + { + int segment_index = segment_neighbours_[curr_segment][i_nghbr]; + if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits::max () ) + continue; + if (segment_labels_[segment_index] != i_reg) + { + pair.first = segment_distances_[curr_segment][i_nghbr]; + pair.second = segment_index; + neighbours_out[i_reg].push_back (pair); + } + }// next neighbour + }// next segment + std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair); + }// next homogeneous region +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::assembleRegions (std::vector& num_pts_in_region, int num_regions) +{ + clusters_.clear (); + pcl::PointIndices segment; + clusters_.resize (num_regions, segment); + for (int i_seg = 0; i_seg < num_regions; i_seg++) + { + clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]); + } + + std::vector counter; + counter.resize (num_regions, 0); + int point_number = static_cast (indices_->size ()); + for (int i_point = 0; i_point < point_number; i_point++) + { + int point_index = (*indices_)[i_point]; + int index = point_labels_[point_index]; + index = segment_labels_[index]; + clusters_[index].indices[ counter[index] ] = point_index; + counter[index] += 1; + } + + // now we need to erase empty regions + if (clusters_.empty ()) + return; + + std::vector::iterator itr1, itr2; + itr1 = clusters_.begin (); + itr2 = clusters_.end () - 1; + + while (itr1 < itr2) + { + while (!(itr1->indices.empty ()) && itr1 < itr2) + ++itr1; + while ( itr2->indices.empty () && itr1 < itr2) + --itr2; + + if (itr1 != itr2) + itr1->indices.swap (itr2->indices); + } + + if (itr2->indices.empty ()) + clusters_.erase (itr2, clusters_.end ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::RegionGrowingRGB::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const +{ + is_a_seed = true; + + // check the color difference + std::vector point_color; + point_color.resize (3, 0); + std::vector nghbr_color; + nghbr_color.resize (3, 0); + point_color[0] = input_->points[point].r; + point_color[1] = input_->points[point].g; + point_color[2] = input_->points[point].b; + nghbr_color[0] = input_->points[nghbr].r; + nghbr_color[1] = input_->points[nghbr].g; + nghbr_color[2] = input_->points[nghbr].b; + float difference = calculateColorimetricalDifference (point_color, nghbr_color); + if (difference > color_p2p_threshold_) + return (false); + + float cosine_threshold = std::cos (theta_threshold_); + + // check the angle between normals if needed + if (normal_flag_) + { + float data[4]; + data[0] = input_->points[point].data[0]; + data[1] = input_->points[point].data[1]; + data[2] = input_->points[point].data[2]; + data[3] = input_->points[point].data[3]; + + Eigen::Map initial_point (static_cast (data)); + Eigen::Map initial_normal (static_cast (normals_->points[point].normal)); + if (smooth_mode_flag_ == true) + { + Eigen::Map nghbr_normal (static_cast (normals_->points[nghbr].normal)); + float dot_product = std::abs (nghbr_normal.dot (initial_normal)); + if (dot_product < cosine_threshold) + return (false); + } + else + { + Eigen::Map nghbr_normal (static_cast (normals_->points[nghbr].normal)); + Eigen::Map initial_seed_normal (static_cast (normals_->points[initial_seed].normal)); + float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal)); + if (dot_product < cosine_threshold) + return (false); + } + } + + // check the curvature if needed + if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) + is_a_seed = false; + + // check the residual if needed + if (residual_flag_) + { + float data_p[4]; + data_p[0] = input_->points[point].data[0]; + data_p[1] = input_->points[point].data[1]; + data_p[2] = input_->points[point].data[2]; + data_p[3] = input_->points[point].data[3]; + float data_n[4]; + data_n[0] = input_->points[nghbr].data[0]; + data_n[1] = input_->points[nghbr].data[1]; + data_n[2] = input_->points[nghbr].data[2]; + data_n[3] = input_->points[nghbr].data[3]; + Eigen::Map nghbr_point (static_cast (data_n)); + Eigen::Map initial_point (static_cast (data_p)); + Eigen::Map initial_normal (static_cast (normals_->points[point].normal)); + float residual = std::abs (initial_normal.dot (initial_point - nghbr_point)); + if (residual > residual_threshold_) + is_a_seed = false; + } + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::RegionGrowingRGB::getSegmentFromPoint (int index, pcl::PointIndices& cluster) +{ + cluster.indices.clear (); + + bool segmentation_is_possible = initCompute (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + // first of all we need to find out if this point belongs to cloud + bool point_was_found = false; + int number_of_points = static_cast (indices_->size ()); + for (int point = 0; point < number_of_points; point++) + if ( (*indices_)[point] == index) + { + point_was_found = true; + break; + } + + if (point_was_found) + { + if (clusters_.empty ()) + { + clusters_.clear (); + point_neighbours_.clear (); + point_labels_.clear (); + num_pts_in_segment_.clear (); + point_distances_.clear (); + segment_neighbours_.clear (); + segment_distances_.clear (); + segment_labels_.clear (); + number_of_segments_ = 0; + + segmentation_is_possible = prepareForSegmentation (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + findPointNeighbours (); + applySmoothRegionGrowingAlgorithm (); + RegionGrowing::assembleRegions (); + + findSegmentNeighbours (); + applyRegionMergingAlgorithm (); + } + // if we have already made the segmentation, then find the segment + // to which this point belongs + for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++) + { + bool segment_was_found = false; + for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++) + { + if (i_segment->indices[i_point] == index) + { + segment_was_found = true; + cluster.indices.clear (); + cluster.indices.reserve (i_segment->indices.size ()); + std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices)); + break; + } + } + if (segment_was_found) + { + break; + } + }// next segment + }// end if point was found + + deinitCompute (); +} + +#endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/sac_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/sac_segmentation.hpp new file mode 100755 index 0000000..0b36c56 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/sac_segmentation.hpp @@ -0,0 +1,511 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ +#define PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ + +#include + +// Sample Consensus methods +#include +#include +#include +#include +#include +#include +#include +#include + +// Sample Consensus models +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SACSegmentation::segment (PointIndices &inliers, ModelCoefficients &model_coefficients) +{ + // Copy the header information + inliers.header = model_coefficients.header = input_->header; + + if (!initCompute ()) + { + inliers.indices.clear (); model_coefficients.values.clear (); + return; + } + + // 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 ()); + deinitCompute (); + inliers.indices.clear (); model_coefficients.values.clear (); + return; + } + // Initialize the Sample Consensus method and set its parameters + initSAC (method_type_); + + if (!sac_->computeModel (0)) + { + PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ()); + deinitCompute (); + inliers.indices.clear (); model_coefficients.values.clear (); + return; + } + + // Get the model inliers + sac_->getInliers (inliers.indices); + + // Get the model coefficients + Eigen::VectorXf coeff; + sac_->getModelCoefficients (coeff); + + // If the user needs optimized coefficients + if (optimize_coefficients_) + { + Eigen::VectorXf coeff_refined; + model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined); + model_coefficients.values.resize (coeff_refined.size ()); + memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float)); + // Refine inliers + model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices); + } + else + { + model_coefficients.values.resize (coeff.size ()); + memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float)); + } + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SACSegmentation::initSACModel (const int model_type) +{ + if (model_) + model_.reset (); + + // 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 ()); + model_.reset (new SampleConsensusModelPlane (input_, *indices_, random_)); + break; + } + case SACMODEL_LINE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelLine (input_, *indices_, random_)); + break; + } + case SACMODEL_STICK: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelStick (input_, *indices_)); + double min_radius, max_radius; + model_->getRadiusLimits (min_radius, max_radius); + if (radius_min_ != min_radius && radius_max_ != max_radius) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); + model_->setRadiusLimits (radius_min_, radius_max_); + } + break; + } + case SACMODEL_CIRCLE2D: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCircle2D (input_, *indices_, random_)); + typename SampleConsensusModelCircle2D::Ptr model_circle = boost::static_pointer_cast > (model_); + double min_radius, max_radius; + model_circle->getRadiusLimits (min_radius, max_radius); + if (radius_min_ != min_radius && radius_max_ != max_radius) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); + model_circle->setRadiusLimits (radius_min_, radius_max_); + } + break; + } + case SACMODEL_CIRCLE3D: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCircle3D (input_, *indices_)); + typename SampleConsensusModelCircle3D::Ptr model_circle3d = boost::static_pointer_cast > (model_); + double min_radius, max_radius; + model_circle3d->getRadiusLimits (min_radius, max_radius); + if (radius_min_ != min_radius && radius_max_ != max_radius) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); + model_circle3d->setRadiusLimits (radius_min_, radius_max_); + } + break; + } + case SACMODEL_SPHERE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelSphere (input_, *indices_, random_)); + typename SampleConsensusModelSphere::Ptr model_sphere = boost::static_pointer_cast > (model_); + double min_radius, max_radius; + model_sphere->getRadiusLimits (min_radius, max_radius); + if (radius_min_ != min_radius && radius_max_ != max_radius) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); + model_sphere->setRadiusLimits (radius_min_, radius_max_); + } + break; + } + case SACMODEL_PARALLEL_LINE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelLine (input_, *indices_, random_)); + typename SampleConsensusModelParallelLine::Ptr model_parallel = boost::static_pointer_cast > (model_); + if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); + model_parallel->setAxis (axis_); + } + if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); + model_parallel->setEpsAngle (eps_angle_); + } + break; + } + case SACMODEL_PERPENDICULAR_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelPerpendicularPlane (input_, *indices_, random_)); + typename SampleConsensusModelPerpendicularPlane::Ptr model_perpendicular = boost::static_pointer_cast > (model_); + if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); + model_perpendicular->setAxis (axis_); + } + if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); + model_perpendicular->setEpsAngle (eps_angle_); + } + break; + } + case SACMODEL_PARALLEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelPlane (input_, *indices_, random_)); + typename SampleConsensusModelParallelPlane::Ptr model_parallel = boost::static_pointer_cast > (model_); + if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); + model_parallel->setAxis (axis_); + } + if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); + model_parallel->setEpsAngle (eps_angle_); + } + break; + } + default: + { + PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); + return (false); + } + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SACSegmentation::initSAC (const int method_type) +{ + if (sac_) + sac_.reset (); + // Build the sample consensus method + switch (method_type) + { + case SAC_RANSAC: + default: + { + PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); + sac_.reset (new RandomSampleConsensus (model_, threshold_)); + break; + } + case SAC_LMEDS: + { + PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_); + sac_.reset (new LeastMedianSquares (model_, threshold_)); + break; + } + case SAC_MSAC: + { + PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); + sac_.reset (new MEstimatorSampleConsensus (model_, threshold_)); + break; + } + case SAC_RRANSAC: + { + PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); + sac_.reset (new RandomizedRandomSampleConsensus (model_, threshold_)); + break; + } + case SAC_RMSAC: + { + PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); + sac_.reset (new RandomizedMEstimatorSampleConsensus (model_, threshold_)); + break; + } + case SAC_MLESAC: + { + PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); + sac_.reset (new MaximumLikelihoodSampleConsensus (model_, threshold_)); + break; + } + case SAC_PROSAC: + { + PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_); + sac_.reset (new ProgressiveSampleConsensus (model_, threshold_)); + break; + } + } + // Set the Sample Consensus parameters if they are given/changed + if (sac_->getProbability () != probability_) + { + PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_); + sac_->setProbability (probability_); + } + if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_) + { + PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_); + sac_->setMaxIterations (max_iterations_); + } + if (samples_radius_ > 0.) + { + PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_); + // Set maximum distance for radius search during random sampling + model_->setSamplesMaxDist (samples_radius_, samples_radius_search_); + } + if (sac_->getNumberOfThreads () != threads_) + { + PCL_DEBUG ("[pcl::%s::initSAC] Setting the number of threads to %i\n", getClassName ().c_str (), threads_); + sac_->setNumberOfThreads (threads_); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SACSegmentationFromNormals::initSACModel (const int model_type) +{ + if (!input_ || !normals_) + { + PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ()); + return (false); + } + // Check if input is synced with the normals + if (input_->points.size () != normals_->points.size ()) + { + PCL_ERROR ("[pcl::%s::initSACModel] The number of points in the input point cloud differs than the number of points in the normals!\n", getClassName ().c_str ()); + return (false); + } + + if (model_) + model_.reset (); + + // Build the model + switch (model_type) + { + case SACMODEL_CYLINDER: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCylinder (input_, *indices_, random_)); + typename SampleConsensusModelCylinder::Ptr model_cylinder = boost::static_pointer_cast > (model_); + + // Set the input normals + model_cylinder->setInputNormals (normals_); + double min_radius, max_radius; + model_cylinder->getRadiusLimits (min_radius, max_radius); + if (radius_min_ != min_radius && radius_max_ != max_radius) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); + model_cylinder->setRadiusLimits (radius_min_, radius_max_); + } + if (distance_weight_ != model_cylinder->getNormalDistanceWeight ()) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); + model_cylinder->setNormalDistanceWeight (distance_weight_); + } + if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); + model_cylinder->setAxis (axis_); + } + if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); + model_cylinder->setEpsAngle (eps_angle_); + } + break; + } + case SACMODEL_NORMAL_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalPlane (input_, *indices_, random_)); + typename SampleConsensusModelNormalPlane::Ptr model_normals = boost::static_pointer_cast > (model_); + // Set the input normals + model_normals->setInputNormals (normals_); + if (distance_weight_ != model_normals->getNormalDistanceWeight ()) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); + model_normals->setNormalDistanceWeight (distance_weight_); + } + break; + } + case SACMODEL_NORMAL_PARALLEL_PLANE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalParallelPlane (input_, *indices_, random_)); + typename SampleConsensusModelNormalParallelPlane::Ptr model_normals = boost::static_pointer_cast > (model_); + // Set the input normals + model_normals->setInputNormals (normals_); + if (distance_weight_ != model_normals->getNormalDistanceWeight ()) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); + model_normals->setNormalDistanceWeight (distance_weight_); + } + if (distance_from_origin_ != model_normals->getDistanceFromOrigin ()) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_); + model_normals->setDistanceFromOrigin (distance_from_origin_); + } + if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); + model_normals->setAxis (axis_); + } + if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); + model_normals->setEpsAngle (eps_angle_); + } + break; + } + case SACMODEL_CONE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCone (input_, *indices_, random_)); + typename SampleConsensusModelCone::Ptr model_cone = boost::static_pointer_cast > (model_); + + // Set the input normals + model_cone->setInputNormals (normals_); + double min_angle, max_angle; + model_cone->getMinMaxOpeningAngle(min_angle, max_angle); + if (min_angle_ != min_angle && max_angle_ != max_angle) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_); + model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_); + } + + if (distance_weight_ != model_cone->getNormalDistanceWeight ()) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); + model_cone->setNormalDistanceWeight (distance_weight_); + } + if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); + model_cone->setAxis (axis_); + } + if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); + model_cone->setEpsAngle (eps_angle_); + } + break; + } + case SACMODEL_NORMAL_SPHERE: + { + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelNormalSphere (input_, *indices_, random_)); + typename SampleConsensusModelNormalSphere::Ptr model_normals_sphere = boost::static_pointer_cast > (model_); + // Set the input normals + model_normals_sphere->setInputNormals (normals_); + double min_radius, max_radius; + model_normals_sphere->getRadiusLimits (min_radius, max_radius); + if (radius_min_ != min_radius && radius_max_ != max_radius) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); + model_normals_sphere->setRadiusLimits (radius_min_, radius_max_); + } + + if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ()) + { + PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); + model_normals_sphere->setNormalDistanceWeight (distance_weight_); + } + break; + } + // If nothing else, try SACSegmentation + default: + { + return (pcl::SACSegmentation::initSACModel (model_type)); + } + } + + return (true); +} + +#define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation; +#define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals; + +#endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/seeded_hue_segmentation.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/seeded_hue_segmentation.hpp new file mode 100755 index 0000000..8c9fb15 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/seeded_hue_segmentation.hpp @@ -0,0 +1,222 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $id $ + */ + +#ifndef PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_ +#define PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::seededHueSegmentation (const PointCloud &cloud, + const search::Search::Ptr &tree, + float tolerance, + PointIndices &indices_in, + PointIndices &indices_out, + float delta_hue) +{ + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + + // Process all points in the indices vector + for (const int &i : indices_in.indices) + { + if (processed[i]) + continue; + + processed[i] = true; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (i); + + PointXYZRGB p; + p = cloud.points[i]; + PointXYZHSV h; + PointXYZRGBtoXYZHSV(p, h); + + while (sq_idx < static_cast (seed_queue.size ())) + { + int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits::max()); + if(ret == -1) + PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1"); + // Search for sq_idx + if (!ret) + { + sq_idx++; + continue; + } + + for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + PointXYZRGB p_l; + p_l = cloud.points[nn_indices[j]]; + PointXYZHSV h_l; + PointXYZRGBtoXYZHSV(p_l, h_l); + + if (std::fabs(h_l.h - h.h) < delta_hue) + { + seed_queue.push_back (nn_indices[j]); + processed[nn_indices[j]] = true; + } + } + + sq_idx++; + } + // Copy the seed queue into the output indices + for (const int &l : seed_queue) + indices_out.indices.push_back(l); + } + // This is purely esthetical, can be removed for speed purposes + std::sort (indices_out.indices.begin (), indices_out.indices.end ()); +} +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::seededHueSegmentation (const PointCloud &cloud, + const search::Search::Ptr &tree, + float tolerance, + PointIndices &indices_in, + PointIndices &indices_out, + float delta_hue) +{ + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::seededHueSegmentation] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + std::vector nn_indices; + std::vector nn_distances; + + // Process all points in the indices vector + for (const int &i : indices_in.indices) + { + if (processed[i]) + continue; + + processed[i] = true; + + std::vector seed_queue; + int sq_idx = 0; + seed_queue.push_back (i); + + PointXYZRGB p; + p = cloud.points[i]; + PointXYZHSV h; + PointXYZRGBtoXYZHSV(p, h); + + while (sq_idx < static_cast (seed_queue.size ())) + { + int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits::max()); + if(ret == -1) + PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1"); + // Search for sq_idx + if (!ret) + { + sq_idx++; + continue; + } + for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + PointXYZRGB p_l; + p_l = cloud.points[nn_indices[j]]; + PointXYZHSV h_l; + PointXYZRGBtoXYZHSV(p_l, h_l); + + if (std::fabs(h_l.h - h.h) < delta_hue) + { + seed_queue.push_back (nn_indices[j]); + processed[nn_indices[j]] = true; + } + } + + sq_idx++; + } + // Copy the seed queue into the output indices + for (const int &l : seed_queue) + indices_out.indices.push_back(l); + } + // This is purely esthetical, can be removed for speed purposes + std::sort (indices_out.indices.begin (), indices_out.indices.end ()); +} +////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////// + +void +pcl::SeededHueSegmentation::segment (PointIndices &indices_in, PointIndices &indices_out) +{ + if (!initCompute () || + (input_ && input_->points.empty ()) || + (indices_ && indices_->empty ())) + { + indices_out.indices.clear (); + return; + } + + // Initialize the spatial locator + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the input dataset to the spatial locator + tree_->setInputCloud (input_); + seededHueSegmentation (*input_, tree_, static_cast (cluster_tolerance_), indices_in, indices_out, delta_hue_); + deinitCompute (); +} + +#endif // PCL_EXTRACT_CLUSTERS_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/segment_differences.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/segment_differences.hpp new file mode 100755 index 0000000..32c27bf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/segment_differences.hpp @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ +#define PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////// +template void +pcl::getPointCloudDifference ( + const pcl::PointCloud &src, + double threshold, + const typename pcl::search::Search::Ptr &tree, + pcl::PointCloud &output) +{ + // We're interested in a single nearest neighbor only + std::vector nn_indices (1); + std::vector nn_distances (1); + + // The input cloud indices that do not have a neighbor in the target cloud + std::vector src_indices; + + // Iterate through the source data set + for (int i = 0; i < static_cast (src.points.size ()); ++i) + { + // Ignore invalid points in the inpout cloud + 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 %lu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z); + continue; + } + // Add points without a corresponding point in the target cloud to the output cloud + if (nn_distances[0] > threshold) + src_indices.push_back (i); + } + + // Copy all the data fields from the input cloud to the output one + copyPointCloud (src, src_indices, output); + + // Output is always dense, as invalid points in the input cloud are ignored + output.is_dense = true; +} + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +template void +pcl::SegmentDifferences::segment (PointCloud &output) +{ + output.header = input_->header; + + if (!initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + + // If target is empty, input - target = input + if (target_->points.empty ()) + { + output = *input_; + return; + } + + // Initialize the spatial locator + if (!tree_) + { + if (target_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + // Send the input dataset to the spatial locator + tree_->setInputCloud (target_); + + getPointCloudDifference (*input_, distance_threshold_, tree_, output); + + deinitCompute (); +} + +#define PCL_INSTANTIATE_SegmentDifferences(T) template class PCL_EXPORTS pcl::SegmentDifferences; +#define PCL_INSTANTIATE_getPointCloudDifference(T) template PCL_EXPORTS void pcl::getPointCloudDifference(const pcl::PointCloud &, double, const typename pcl::search::Search::Ptr &, pcl::PointCloud &); + +#endif // PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/supervoxel_clustering.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/supervoxel_clustering.hpp new file mode 100755 index 0000000..eb320d0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/supervoxel_clustering.hpp @@ -0,0 +1,942 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : jpapon@gmail.com + * Email : jpapon@gmail.com + * + */ + +#ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_ +#define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::SupervoxelClustering::SupervoxelClustering (float voxel_resolution, float seed_resolution) : + resolution_ (voxel_resolution), + seed_resolution_ (seed_resolution), + adjacency_octree_ (), + voxel_centroid_cloud_ (), + color_importance_ (0.1f), + spatial_importance_ (0.4f), + normal_importance_ (1.0f), + use_default_transform_behaviour_ (true) +{ + adjacency_octree_.reset (new OctreeAdjacencyT (resolution_)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::SupervoxelClustering::~SupervoxelClustering () +{ + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setInputCloud (const typename pcl::PointCloud::ConstPtr& cloud) +{ + if ( cloud->empty () ) + { + PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n"); + return; + } + + input_ = cloud; + adjacency_octree_->setInputCloud (cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud) +{ + if ( normal_cloud->empty () ) + { + PCL_ERROR ("[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n"); + return; + } + + input_normals_ = normal_cloud; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::extract (std::map::Ptr > &supervoxel_clusters) +{ + //timer_.reset (); + //double t_start = timer_.getTime (); + //std::cout << "Init compute \n"; + bool segmentation_is_possible = initCompute (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + //std::cout << "Preparing for segmentation \n"; + segmentation_is_possible = prepareForSegmentation (); + if ( !segmentation_is_possible ) + { + deinitCompute (); + return; + } + + //double t_prep = timer_.getTime (); + //std::cout << "Placing Seeds" << std::endl; + std::vector seed_indices; + selectInitialSupervoxelSeeds (seed_indices); + //std::cout << "Creating helpers "< (1.8f*seed_resolution_/resolution_); + expandSupervoxels (max_depth); + //double t_iterate = timer_.getTime (); + + //std::cout << "Making Supervoxel structures" << std::endl; + makeSupervoxels (supervoxel_clusters); + //double t_supervoxels = timer_.getTime (); + + // std::cout << "--------------------------------- Timing Report --------------------------------- \n"; + // std::cout << "Time to prep (normals, neighbors, voxelization)="< void +pcl::SupervoxelClustering::refineSupervoxels (int num_itr, std::map::Ptr > &supervoxel_clusters) +{ + if (supervoxel_helpers_.empty ()) + { + PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n"); + return; + } + + int max_depth = static_cast (1.8f*seed_resolution_/resolution_); + for (int i = 0; i < num_itr; ++i) + { + for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) + { + sv_itr->refineNormals (); + } + + reseedSupervoxels (); + expandSupervoxels (max_depth); + } + + + makeSupervoxels (supervoxel_clusters); + +} +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +template bool +pcl::SupervoxelClustering::prepareForSegmentation () +{ + + // if user forgot to pass point cloud or if it is empty + if ( input_->points.empty () ) + return (false); + + //Add the new cloud of data to the octree + //std::cout << "Populating adjacency octree with new cloud \n"; + //double prep_start = timer_.getTime (); + if ( (use_default_transform_behaviour_ && input_->isOrganized ()) + || (!use_default_transform_behaviour_ && use_single_camera_transform_)) + adjacency_octree_->setTransformFunction ([this] (PointT &p) { transformFunction (p); }); + + adjacency_octree_->addPointsFromInputCloud (); + //double prep_end = timer_.getTime (); + //std::cout<<"Time elapsed populating octree with next frame ="< void +pcl::SupervoxelClustering::computeVoxelData () +{ + voxel_centroid_cloud_.reset (new PointCloudT); + voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ()); + typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin (); + typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin (); + for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx) + { + VoxelData& new_voxel_data = (*leaf_itr)->getData (); + //Add the point to the centroid cloud + new_voxel_data.getPoint (*cent_cloud_itr); + //voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ()); + new_voxel_data.idx_ = idx; + } + + //If normals were provided + if (input_normals_) + { + //Verify that input normal cloud size is same as input cloud size + assert (input_normals_->size () == input_->size ()); + //For every point in the input cloud, find its corresponding leaf + typename NormalCloudT::const_iterator normal_itr = input_normals_->begin (); + for (typename PointCloudT::const_iterator input_itr = input_->begin (); input_itr != input_->end (); ++input_itr, ++normal_itr) + { + //If the point is not finite we ignore it + if ( !pcl::isFinite (*input_itr)) + continue; + //Otherwise look up its leaf container + LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr); + + //Get the voxel data object + VoxelData& voxel_data = leaf->getData (); + //Add this normal in (we will normalize at the end) + voxel_data.normal_ += normal_itr->getNormalVector4fMap (); + voxel_data.curvature_ += normal_itr->curvature; + } + //Now iterate through the leaves and normalize + for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr) + { + VoxelData& voxel_data = (*leaf_itr)->getData (); + voxel_data.normal_.normalize (); + voxel_data.owner_ = nullptr; + voxel_data.distance_ = std::numeric_limits::max (); + //Get the number of points in this leaf + int num_points = (*leaf_itr)->getPointCounter (); + voxel_data.curvature_ /= num_points; + } + } + else //Otherwise just compute the normals + { + for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr) + { + VoxelData& new_voxel_data = (*leaf_itr)->getData (); + //For every point, get its neighbors, build an index vector, compute normal + std::vector indices; + indices.reserve (81); + //Push this point + indices.push_back (new_voxel_data.idx_); + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) + { + VoxelData& neighb_voxel_data = (*neighb_itr)->getData (); + //Push neighbor index + indices.push_back (neighb_voxel_data.idx_); + //Get neighbors neighbors, push onto cloud + for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr) + { + VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData (); + indices.push_back (neighb2_voxel_data.idx_); + } + } + //Compute normal + pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_); + pcl::flipNormalTowardsViewpoint (voxel_centroid_cloud_->points[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_); + new_voxel_data.normal_[3] = 0.0f; + new_voxel_data.normal_.normalize (); + new_voxel_data.owner_ = nullptr; + new_voxel_data.distance_ = std::numeric_limits::max (); + } + } + + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::expandSupervoxels ( int depth ) +{ + + + for (int i = 1; i < depth; ++i) + { + //Expand the the supervoxels by one iteration + for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) + { + sv_itr->expand (); + } + + //Update the centers to reflect new centers + for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ) + { + if (sv_itr->size () == 0) + { + sv_itr = supervoxel_helpers_.erase (sv_itr); + } + else + { + sv_itr->updateCentroid (); + ++sv_itr; + } + } + + } + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::makeSupervoxels (std::map::Ptr > &supervoxel_clusters) +{ + supervoxel_clusters.clear (); + for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) + { + std::uint32_t label = sv_itr->getLabel (); + supervoxel_clusters[label].reset (new Supervoxel); + sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z); + sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba); + sv_itr->getNormal (supervoxel_clusters[label]->normal_); + sv_itr->getVoxels (supervoxel_clusters[label]->voxels_); + sv_itr->getNormals (supervoxel_clusters[label]->normals_); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::createSupervoxelHelpers (std::vector &seed_indices) +{ + + supervoxel_helpers_.clear (); + for (std::size_t i = 0; i < seed_indices.size (); ++i) + { + supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this)); + //Find which leaf corresponds to this seed index + LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[i]);//adjacency_octree_->getLeafContainerAtPoint (seed_points[i]); + if (seed_leaf) + { + supervoxel_helpers_.back ().addLeaf (seed_leaf); + } + else + { + PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering::createSupervoxelHelpers - supervoxel will be deleted \n"); + } + } + +} +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::selectInitialSupervoxelSeeds (std::vector &seed_indices) +{ + //TODO THIS IS BAD - SEEDING SHOULD BE BETTER + //TODO Switch to assigning leaves! Don't use Octree! + + // std::cout << "Size of centroid cloud="<size ()<<", seeding resolution="< seed_octree (seed_resolution_); + seed_octree.setInputCloud (voxel_centroid_cloud_); + seed_octree.addPointsFromInputCloud (); + // std::cout << "Size of octree ="< > voxel_centers; + int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers); + //std::cout << "Number of seed points before filtering="< seed_indices_orig; + seed_indices_orig.resize (num_seeds, 0); + seed_indices.clear (); + std::vector closest_index; + std::vector distance; + closest_index.resize(1,0); + distance.resize(1,0); + if (!voxel_kdtree_) + { + voxel_kdtree_.reset (new pcl::search::KdTree); + voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_); + } + + for (int i = 0; i < num_seeds; ++i) + { + voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance); + seed_indices_orig[i] = closest_index[0]; + } + + std::vector neighbors; + std::vector sqr_distances; + seed_indices.reserve (seed_indices_orig.size ()); + float search_radius = 0.5f*seed_resolution_; + // This is 1/20th of the number of voxels which fit in a planar slice through search volume + // Area of planar slice / area of voxel side. (Note: This is smaller than the value mentioned in the original paper) + float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_); + for (const int &index_orig : seed_indices_orig) + { + int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances); + int min_index = index_orig; + if ( num > min_points) + { + seed_indices.push_back (min_index); + } + + } + // std::cout << "Number of seed points after filtering="< void +pcl::SupervoxelClustering::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 closest_index; + std::vector 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::reseedSupervoxels - supervoxel will be deleted \n"); + } + } + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::transformFunction (PointT &p) +{ + p.x /= p.z; + p.y /= p.z; + p.z = std::log (p.z); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::SupervoxelClustering::voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const +{ + + float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_; + float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f; + float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_)); + // std::cout << "s="< void +pcl::SupervoxelClustering::getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const +{ + adjacency_list_arg.clear (); + //Add a vertex for each label, store ids in map + std::map label_ID_map; + for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) + { + VoxelID node_id = add_vertex (adjacency_list_arg); + adjacency_list_arg[node_id] = (sv_itr->getLabel ()); + label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id)); + } + + for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) + { + std::uint32_t label = sv_itr->getLabel (); + std::set neighbor_labels; + sv_itr->getNeighborLabels (neighbor_labels); + for (const unsigned int &neighbor_label : neighbor_labels) + { + bool edge_added; + EdgeID edge; + VoxelID u = (label_ID_map.find (label))->second; + VoxelID v = (label_ID_map.find (neighbor_label))->second; + boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg); + //Calc distance between centers, set as edge weight + if (edge_added) + { + VoxelData centroid_data = (sv_itr)->getCentroid (); + //Find the neighbhor with this label + VoxelData neighb_centroid_data; + + for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr) + { + if (neighb_itr->getLabel () == neighbor_label) + { + neighb_centroid_data = neighb_itr->getCentroid (); + break; + } + } + + float length = voxelDataDistance (centroid_data, neighb_centroid_data); + adjacency_list_arg[edge] = length; + } + } + + } + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::getSupervoxelAdjacency (std::multimap &label_adjacency) const +{ + label_adjacency.clear (); + for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) + { + std::uint32_t label = sv_itr->getLabel (); + std::set neighbor_labels; + sv_itr->getNeighborLabels (neighbor_labels); + for (const unsigned int &neighbor_label : neighbor_labels) + label_adjacency.insert (std::pair (label, neighbor_label) ); + //if (neighbor_labels.size () == 0) + // std::cout << label<<"(size="<size () << ") has "< typename pcl::PointCloud::Ptr +pcl::SupervoxelClustering::getVoxelCentroidCloud () const +{ + typename PointCloudT::Ptr centroid_copy (new PointCloudT); + copyPointCloud (*voxel_centroid_cloud_, *centroid_copy); + return centroid_copy; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::SupervoxelClustering::getLabeledVoxelCloud () const +{ + pcl::PointCloud::Ptr labeled_voxel_cloud (new pcl::PointCloud); + for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) + { + typename PointCloudT::Ptr voxels; + sv_itr->getVoxels (voxels); + pcl::PointCloud xyzl_copy; + copyPointCloud (*voxels, xyzl_copy); + + pcl::PointCloud::iterator xyzl_copy_itr = xyzl_copy.begin (); + for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr) + xyzl_copy_itr->label = sv_itr->getLabel (); + + *labeled_voxel_cloud += xyzl_copy; + } + + return labeled_voxel_cloud; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::SupervoxelClustering::getLabeledCloud () const +{ + pcl::PointCloud::Ptr labeled_cloud (new pcl::PointCloud); + pcl::copyPointCloud (*input_,*labeled_cloud); + + typename pcl::PointCloud ::const_iterator i_input = input_->begin (); + for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input) + { + if ( !pcl::isFinite (*i_input)) + i_labeled->label = 0; + else + { + i_labeled->label = 0; + LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input); + VoxelData& voxel_data = leaf->getData (); + if (voxel_data.owner_) + i_labeled->label = voxel_data.owner_->getLabel (); + + } + + } + + return (labeled_cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template pcl::PointCloud::Ptr +pcl::SupervoxelClustering::makeSupervoxelNormalCloud (std::map::Ptr > &supervoxel_clusters) +{ + pcl::PointCloud::Ptr normal_cloud (new pcl::PointCloud); + normal_cloud->resize (supervoxel_clusters.size ()); + pcl::PointCloud::iterator normal_cloud_itr = normal_cloud->begin (); + for (auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend (); + sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr) + { + (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr); + } + return normal_cloud; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::SupervoxelClustering::getVoxelResolution () const +{ + return (resolution_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setVoxelResolution (float resolution) +{ + resolution_ = resolution; + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::SupervoxelClustering::getSeedResolution () const +{ + return (seed_resolution_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setSeedResolution (float seed_resolution) +{ + seed_resolution_ = seed_resolution; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setColorImportance (float val) +{ + color_importance_ = val; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setSpatialImportance (float val) +{ + spatial_importance_ = val; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setNormalImportance (float val) +{ + normal_importance_ = val; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::setUseSingleCameraTransform (bool val) +{ + use_default_transform_behaviour_ = false; + use_single_camera_transform_ = val; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::SupervoxelClustering::getMaxLabel () const +{ + int max_label = 0; + for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) + { + int temp = sv_itr->getLabel (); + if (temp > max_label) + max_label = temp; + } + return max_label; +} + +namespace pcl +{ + namespace octree + { + //Explicit overloads for RGB types + template<> + void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::addPoint (const pcl::PointXYZRGB &new_point); + + template<> + void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::addPoint (const pcl::PointXYZRGBA &new_point); + + //Explicit overloads for RGB types + template<> void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::computeData (); + + template<> void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::computeData (); + + //Explicit overloads for XYZ types + template<> + void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::addPoint (const pcl::PointXYZ &new_point); + + template<> void + pcl::octree::OctreePointCloudAdjacencyContainer::VoxelData>::computeData (); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl +{ + + template<> void + pcl::SupervoxelClustering::VoxelData::getPoint (pcl::PointXYZRGB &point_arg) const; + + template<> void + pcl::SupervoxelClustering::VoxelData::getPoint (pcl::PointXYZRGBA &point_arg ) const; + + template void + pcl::SupervoxelClustering::VoxelData::getPoint (PointT &point_arg ) const + { + //XYZ is required or this doesn't make much sense... + point_arg.x = xyz_[0]; + point_arg.y = xyz_[1]; + point_arg.z = xyz_[2]; + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void + pcl::SupervoxelClustering::VoxelData::getNormal (Normal &normal_arg) const + { + normal_arg.normal_x = normal_[0]; + normal_arg.normal_y = normal_[1]; + normal_arg.normal_z = normal_[2]; + normal_arg.curvature = curvature_; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::addLeaf (LeafContainerT* leaf_arg) +{ + leaves_.insert (leaf_arg); + VoxelData& voxel_data = leaf_arg->getData (); + voxel_data.owner_ = this; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::removeLeaf (LeafContainerT* leaf_arg) +{ + leaves_.erase (leaf_arg); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::removeAllLeaves () +{ + for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr) + { + VoxelData& voxel = ((*leaf_itr)->getData ()); + voxel.owner_ = nullptr; + voxel.distance_ = std::numeric_limits::max (); + } + leaves_.clear (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::expand () +{ + //std::cout << "Expanding sv "< new_owned; + new_owned.reserve (leaves_.size () * 9); + //For each leaf belonging to this supervoxel + for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr) + { + //for each neighbor of the leaf + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) + { + //Get a reference to the data contained in the leaf + VoxelData& neighbor_voxel = ((*neighb_itr)->getData ()); + //TODO this is a shortcut, really we should always recompute distance + if(neighbor_voxel.owner_ == this) + continue; + //Compute distance to the neighbor + float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel); + //If distance is less than previous, we remove it from its owner's list + //and change the owner to this and distance (we *steal* it!) + if (dist < neighbor_voxel.distance_) + { + neighbor_voxel.distance_ = dist; + if (neighbor_voxel.owner_ != this) + { + if (neighbor_voxel.owner_) + (neighbor_voxel.owner_)->removeLeaf(*neighb_itr); + neighbor_voxel.owner_ = this; + new_owned.push_back (*neighb_itr); + } + } + } + } + //Push all new owned onto the owned leaf set + for (auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr) + { + leaves_.insert (*new_owned_itr); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::refineNormals () +{ + //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal + for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr) + { + VoxelData& voxel_data = (*leaf_itr)->getData (); + std::vector indices; + indices.reserve (81); + //Push this point + indices.push_back (voxel_data.idx_); + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) + { + //Get a reference to the data contained in the leaf + VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ()); + //If the neighbor is in this supervoxel, use it + if (neighbor_voxel_data.owner_ == this) + { + indices.push_back (neighbor_voxel_data.idx_); + //Also check its neighbors + for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr) + { + VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData (); + if (neighb_neighb_voxel_data.owner_ == this) + indices.push_back (neighb_neighb_voxel_data.idx_); + } + + + } + } + //Compute normal + pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_); + pcl::flipNormalTowardsViewpoint (parent_->voxel_centroid_cloud_->points[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_); + voxel_data.normal_[3] = 0.0f; + voxel_data.normal_.normalize (); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::updateCentroid () +{ + centroid_.normal_ = Eigen::Vector4f::Zero (); + centroid_.xyz_ = Eigen::Vector3f::Zero (); + centroid_.rgb_ = Eigen::Vector3f::Zero (); + for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr) + { + const VoxelData& leaf_data = (*leaf_itr)->getData (); + centroid_.normal_ += leaf_data.normal_; + centroid_.xyz_ += leaf_data.xyz_; + centroid_.rgb_ += leaf_data.rgb_; + } + centroid_.normal_.normalize (); + centroid_.xyz_ /= static_cast (leaves_.size ()); + centroid_.rgb_ /= static_cast (leaves_.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::getVoxels (typename pcl::PointCloud::Ptr &voxels) const +{ + voxels.reset (new pcl::PointCloud); + voxels->clear (); + voxels->resize (leaves_.size ()); + typename pcl::PointCloud::iterator voxel_itr = voxels->begin (); + for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr) + { + const VoxelData& leaf_data = (*leaf_itr)->getData (); + leaf_data.getPoint (*voxel_itr); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::getNormals (typename pcl::PointCloud::Ptr &normals) const +{ + normals.reset (new pcl::PointCloud); + normals->clear (); + normals->resize (leaves_.size ()); + typename pcl::PointCloud::iterator normal_itr = normals->begin (); + for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr) + { + const VoxelData& leaf_data = (*leaf_itr)->getData (); + leaf_data.getNormal (*normal_itr); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SupervoxelClustering::SupervoxelHelper::getNeighborLabels (std::set &neighbor_labels) const +{ + neighbor_labels.clear (); + //For each leaf belonging to this supervoxel + for (auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr) + { + //for each neighbor of the leaf + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) + { + //Get a reference to the data contained in the leaf + VoxelData& neighbor_voxel = ((*neighb_itr)->getData ()); + //If it has an owner, and it's not us - get it's owner's label insert into set + if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_) + { + neighbor_labels.insert (neighbor_voxel.owner_->getLabel ()); + } + } + } +} + + +#endif // PCL_SUPERVOXEL_CLUSTERING_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/impl/unary_classifier.hpp b/deps_install/include/pcl-1.10/pcl/segmentation/impl/unary_classifier.hpp new file mode 100755 index 0000000..88a3d93 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/impl/unary_classifier.hpp @@ -0,0 +1,434 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#ifndef PCL_UNARY_CLASSIFIER_HPP_ +#define PCL_UNARY_CLASSIFIER_HPP_ + +#include +#include +#include + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::UnaryClassifier::UnaryClassifier () : + input_cloud_ (new pcl::PointCloud), + label_field_ (false), + normal_radius_search_ (0.01f), + fpfh_radius_search_ (0.05f), + feature_threshold_ (5.0) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::UnaryClassifier::~UnaryClassifier () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::setInputCloud (typename pcl::PointCloud::Ptr input_cloud) +{ + input_cloud_ = input_cloud; + + pcl::PointCloud point; + std::vector fields; + + int label_index = -1; + label_index = pcl::getFieldIndex ("label", fields); + + if (label_index != -1) + label_field_ = true; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::convertCloud (typename pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out) +{ + // resize points of output cloud + out->points.resize (in->points.size ()); + out->width = static_cast (out->points.size ()); + out->height = 1; + out->is_dense = false; + + for (std::size_t i = 0; i < in->points.size (); i++) + { + pcl::PointXYZ point; + // fill X Y Z + point.x = in->points[i].x; + point.y = in->points[i].y; + point.z = in->points[i].z; + out->points[i] = point; + } +} + +template void +pcl::UnaryClassifier::convertCloud (typename pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out) +{ + // TODO:: check if input cloud has RGBA information and insert into the cloud + + // resize points of output cloud + out->points.resize (in->points.size ()); + out->width = static_cast (out->points.size ()); + out->height = 1; + out->is_dense = false; + + for (std::size_t i = 0; i < in->points.size (); i++) + { + pcl::PointXYZRGBL point; + // X Y Z R G B L + point.x = in->points[i].x; + point.y = in->points[i].y; + point.z = in->points[i].z; + //point.rgba = in->points[i].rgba; + point.label = 1; + out->points[i] = point; + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::findClusters (typename pcl::PointCloud::Ptr in, + std::vector &cluster_numbers) +{ + // find the 'label' field index + std::vector fields; + int label_idx = -1; + pcl::PointCloud point; + label_idx = pcl::getFieldIndex ("label", fields); + + if (label_idx != -1) + { + for (std::size_t i = 0; i < in->points.size (); i++) + { + // get the 'label' field + std::uint32_t label; + memcpy (&label, reinterpret_cast (&in->points[i]) + fields[label_idx].offset, sizeof(std::uint32_t)); + + // check if label exist + bool exist = false; + for (const int &cluster_number : cluster_numbers) + { + if (static_cast (cluster_number) == label) + { + exist = true; + break; + } + } + if (!exist) + cluster_numbers.push_back (label); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::getCloudWithLabel (typename pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out, + int label_num) +{ + // find the 'label' field index + std::vector fields; + int label_idx = -1; + pcl::PointCloud point; + label_idx = pcl::getFieldIndex ("label", fields); + + if (label_idx != -1) + { + for (std::size_t i = 0; i < in->points.size (); i++) + { + // get the 'label' field + std::uint32_t label; + memcpy (&label, reinterpret_cast (&in->points[i]) + fields[label_idx].offset, sizeof(std::uint32_t)); + + if (static_cast (label) == label_num) + { + pcl::PointXYZ point; + // X Y Z + point.x = in->points[i].x; + point.y = in->points[i].y; + point.z = in->points[i].z; + out->points.push_back (point); + } + } + out->width = static_cast (out->points.size ()); + out->height = 1; + out->is_dense = false; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::computeFPFH (pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out, + float normal_radius_search, + float fpfh_radius_search) +{ + pcl::PointCloud::Ptr normals (new pcl::PointCloud ()); + pcl::search::KdTree::Ptr normals_tree (new pcl::search::KdTree); + pcl::NormalEstimation n3d; + + n3d.setRadiusSearch (normal_radius_search); + n3d.setSearchMethod (normals_tree); + // ---[ Estimate the point normals + n3d.setInputCloud (in); + n3d.compute (*normals); + + // Create the FPFH estimation class, and pass the input dataset+normals to it + pcl::FPFHEstimation fpfh; + fpfh.setInputCloud (in); + fpfh.setInputNormals (normals); + + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + fpfh.setSearchMethod (tree); + fpfh.setRadiusSearch (fpfh_radius_search); + // Compute the features + fpfh.compute (*out); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::kmeansClustering (pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out, + int k) +{ + pcl::Kmeans kmeans (static_cast (in->points.size ()), 33); + kmeans.setClusterSize (k); + + // add points to the clustering + for (const auto &point : in->points) + { + std::vector data (33); + for (int idx = 0; idx < 33; idx++) + data[idx] = point.histogram[idx]; + kmeans.addDataPoint (data); + } + + // k-means clustering + kmeans.kMeans (); + + // get the cluster centroids + pcl::Kmeans::Centroids centroids = kmeans.get_centroids (); + + // initialize output cloud + out->width = static_cast (centroids.size ()); + out->height = 1; + out->is_dense = false; + out->points.resize (static_cast (centroids.size ())); + // copy cluster centroids into feature cloud + for (std::size_t i = 0; i < centroids.size (); i++) + { + pcl::FPFHSignature33 point; + for (int idx = 0; idx < 33; idx++) + point.histogram[idx] = centroids[i][idx]; + out->points[i] = point; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::queryFeatureDistances (std::vector::Ptr> &trained_features, + pcl::PointCloud::Ptr query_features, + std::vector &indi, + std::vector &dist) +{ + // estimate the total number of row's needed + int n_row = 0; + for (const auto &trained_feature : trained_features) + n_row += static_cast (trained_feature->points.size ()); + + // Convert data into FLANN format + int n_col = 33; + flann::Matrix data (new float[n_row * n_col], n_row, n_col); + for (std::size_t k = 0; k < trained_features.size (); k++) + { + pcl::PointCloud::Ptr hist = trained_features[k]; + std::size_t c = hist->points.size (); + for (std::size_t i = 0; i < c; ++i) + for (std::size_t j = 0; j < data.cols; ++j) + data[(k * c) + i][j] = hist->points[i].histogram[j]; + } + + // build kd-tree given the training features + flann::Index > *index; + index = new flann::Index > (data, flann::LinearIndexParams ()); + //flann::Index > index (data, flann::LinearIndexParams ()); + //flann::Index > index (data, flann::KMeansIndexParams (5, -1)); + //flann::Index > index (data, flann::KDTreeIndexParams (4)); + index->buildIndex (); + + int k = 1; + indi.resize (query_features->points.size ()); + dist.resize (query_features->points.size ()); + // Query all points + for (std::size_t i = 0; i < query_features->points.size (); i++) + { + // Query point + flann::Matrix p = flann::Matrix(new float[n_col], 1, n_col); + memcpy (&p.ptr ()[0], query_features->points[i].histogram, p.cols * p.rows * sizeof (float)); + + flann::Matrix indices (new int[k], 1, k); + flann::Matrix distances (new float[k], 1, k); + index->knnSearch (p, indices, distances, k, flann::SearchParams (512)); + + indi[i] = indices[0][0]; + dist[i] = distances[0][0]; + + delete[] p.ptr (); + } + + //std::cout << "kdtree size: " << index->size () << std::endl; + + delete[] data.ptr (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::assignLabels (std::vector &indi, + std::vector &dist, + int n_feature_means, + float feature_threshold, + pcl::PointCloud::Ptr out) + +{ + float nfm = static_cast (n_feature_means); + for (std::size_t i = 0; i < out->points.size (); i++) + { + if (dist[i] < feature_threshold) + { + float l = static_cast (indi[i]) / nfm; + float intpart; + //float fractpart = std::modf (l , &intpart); + std::modf (l , &intpart); + int label = static_cast (intpart); + + out->points[i].label = label+2; + } + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::train (pcl::PointCloud::Ptr &output) +{ + // convert cloud into cloud with XYZ + pcl::PointCloud::Ptr tmp_cloud (new pcl::PointCloud); + convertCloud (input_cloud_, tmp_cloud); + + // compute FPFH feature histograms for all point of the input point cloud + pcl::PointCloud::Ptr feature (new pcl::PointCloud); + computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_); + + //PCL_INFO ("Number of input cloud features: %d\n", static_cast (feature->points.size ())); + + // use k-means to cluster the features + kmeansClustering (feature, output, cluster_size_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::trainWithLabel ( + std::vector, Eigen::aligned_allocator > > &output) +{ + // find clusters + std::vector cluster_numbers; + findClusters (input_cloud_, cluster_numbers); + std::cout << "cluster numbers: "; + for (const int &cluster_number : cluster_numbers) + std::cout << cluster_number << " "; + std::cout << std::endl; + + for (const int &cluster_number : cluster_numbers) + { + // extract all points with the same label number + pcl::PointCloud::Ptr label_cloud (new pcl::PointCloud); + getCloudWithLabel (input_cloud_, label_cloud, cluster_number); + + // compute FPFH feature histograms for all point of the input point cloud + pcl::PointCloud::Ptr feature (new pcl::PointCloud); + computeFPFH (label_cloud, feature, normal_radius_search_, fpfh_radius_search_); + + // use k-means to cluster the features + pcl::PointCloud::Ptr kmeans_feature (new pcl::PointCloud); + kmeansClustering (feature, kmeans_feature, cluster_size_); + + output.push_back (*kmeans_feature); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::UnaryClassifier::segment (pcl::PointCloud::Ptr &out) +{ + if (!trained_features_.empty ()) + { + // convert cloud into cloud with XYZ + pcl::PointCloud::Ptr tmp_cloud (new pcl::PointCloud); + convertCloud (input_cloud_, tmp_cloud); + + // compute FPFH feature histograms for all point of the input point cloud + pcl::PointCloud::Ptr input_cloud_features (new pcl::PointCloud); + computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_); + + // query the distances from the input data features to all trained features + std::vector indices; + std::vector distance; + queryFeatureDistances (trained_features_, input_cloud_features, indices, distance); + + // assign a label to each point of the input point cloud + int n_feature_means = static_cast (trained_features_[0]->points.size ()); + convertCloud (input_cloud_, out); + assignLabels (indices, distance, n_feature_means, feature_threshold_, out); + //std::cout << "Assign labels - DONE" << std::endl; + } + else + PCL_ERROR ("no training features set \n"); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +#define PCL_INSTANTIATE_UnaryClassifier(T) template class pcl::UnaryClassifier; + +#endif // PCL_UNARY_CLASSIFIER_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/lccp_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/lccp_segmentation.h new file mode 100755 index 0000000..5c66eb4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/lccp_segmentation.h @@ -0,0 +1,355 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +#define PCL_INSTANTIATE_LCCPSegmentation(T) template class PCL_EXPORTS pcl::LCCPSegmentation; + +namespace pcl +{ + /** \brief A simple segmentation algorithm partitioning a supervoxel graph into groups of locally convex connected supervoxels separated by concave borders. + * \note If you use this in a scientific work please cite the following paper: + * S. C. Stein, M. Schoeler, J. Papon, F. Woergoetter + * Object Partitioning using Local Convexity + * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2014 + * \author Simon Christoph Stein and Markus Schoeler (mschoeler@gwdg.de) + * \ingroup segmentation + */ + template + class LCCPSegmentation + { + /** \brief Edge Properties stored in the adjacency graph.*/ + struct EdgeProperties + { + /** \brief Describes the difference of normals of the two supervoxels being connected*/ + float normal_difference; + + /** \brief Describes if a connection is convex or concave*/ + bool is_convex; + + /** \brief Describes if a connection is valid for the segment growing. Usually convex connections are and concave connection are not. Due to k-concavity a convex connection can be invalidated*/ + bool is_valid; + + /** \brief Additional member used for the CPC algorithm. If edge has already induced a cut, it should be ignored for further cutting.*/ + bool used_for_cutting; + + EdgeProperties () : + normal_difference (0), is_convex (false), is_valid (false), used_for_cutting (false) + { + } + }; + + public: + + // Adjacency list with nodes holding labels (std::uint32_t) and edges holding EdgeProperties. + using SupervoxelAdjacencyList = boost::adjacency_list; + using VertexIterator = typename boost::graph_traits::vertex_iterator; + using AdjacencyIterator = typename boost::graph_traits::adjacency_iterator; + + using VertexID = typename boost::graph_traits::vertex_descriptor; + using EdgeIterator = typename boost::graph_traits::edge_iterator; + using OutEdgeIterator = typename boost::graph_traits::out_edge_iterator; + using EdgeID = typename boost::graph_traits::edge_descriptor; + + LCCPSegmentation (); + virtual + ~LCCPSegmentation (); + + /** \brief Reset internal memory. */ + void + reset (); + + + /** \brief Set the supervoxel clusters as well as the adjacency graph for the segmentation.Those parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref segment method. + * \param[in] supervoxel_clusters_arg Map of < supervoxel labels, supervoxels > + * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations + * \note Implicitly calls \ref reset */ + inline void + setInputSupervoxels (const std::map::Ptr> &supervoxel_clusters_arg, + const std::multimap &label_adjacency_arg) + { + // Initialization + prepareSegmentation (supervoxel_clusters_arg, label_adjacency_arg); // after this, sv_adjacency_list_ can be used to access adjacency list + supervoxels_set_ = true; + } + + /** \brief Merge supervoxels using local convexity. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method. + * \note There are three ways to retrieve the segmentation afterwards: \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/ + void + segment (); + + /** \brief Relabels cloud with supervoxel labels with the computed segment labels. labeled_cloud_arg should be created using the \ref getLabeledCloud method of the \ref SupervoxelClustering class. + * \param[in,out] labeled_cloud_arg Cloud to relabel */ + void + relabelCloud (pcl::PointCloud &labeled_cloud_arg); + + /** \brief Get map > + * \param[out] segment_supervoxel_map_arg The output container. On error the map is empty. */ + inline void + getSegmentToSupervoxelMap (std::map >& segment_supervoxel_map_arg) const + { + if (grouping_data_valid_) + { + segment_supervoxel_map_arg = seg_label_to_sv_list_map_; + } + else + { + PCL_WARN ("[pcl::LCCPSegmentation::getSegmentMap] WARNING: Call function segment first. Nothing has been done. \n"); + segment_supervoxel_map_arg = std::map > (); + } + } + + /** \brief Get map + * \param[out] supervoxel_segment_map_arg The output container. On error the map is empty. */ + inline void + getSupervoxelToSegmentMap (std::map& supervoxel_segment_map_arg) const + { + if (grouping_data_valid_) + { + supervoxel_segment_map_arg = sv_label_to_seg_label_map_; + } + else + { + PCL_WARN ("[pcl::LCCPSegmentation::getSegmentMap] WARNING: Call function segment first. Nothing has been done. \n"); + supervoxel_segment_map_arg = std::map (); + } + } + + /** \brief Get map > + * \param[out] segment_adjacency_map_arg map < SegmentID, std::set< Neighboring SegmentIDs> >. On error the map is empty. */ + inline void + getSegmentAdjacencyMap (std::map >& segment_adjacency_map_arg) + { + if (grouping_data_valid_) + { + if (seg_label_to_neighbor_set_map_.empty ()) + computeSegmentAdjacency (); + segment_adjacency_map_arg = seg_label_to_neighbor_set_map_; + } + else + { + PCL_WARN ("[pcl::LCCPSegmentation::getSegmentAdjacencyMap] WARNING: Call function segment first. Nothing has been done. \n"); + segment_adjacency_map_arg = std::map > (); + } + } + + /** \brief Get normal threshold + * \return The concavity tolerance angle in [deg] that is currently set */ + inline float + getConcavityToleranceThreshold () const + { + return (concavity_tolerance_threshold_); + } + + /** \brief Get the supervoxel adjacency graph with classified edges (boost::adjacency_list). + * \param[out] adjacency_list_arg The supervoxel adjacency list with classified (convex/concave) edges. On error the list is empty. */ + inline void + getSVAdjacencyList (SupervoxelAdjacencyList& adjacency_list_arg) const + { + if (grouping_data_valid_) + { + adjacency_list_arg = sv_adjacency_list_; + } + else + { + PCL_WARN ("[pcl::LCCPSegmentation::getSVAdjacencyList] WARNING: Call function segment first. Nothing has been done. \n"); + adjacency_list_arg = pcl::LCCPSegmentation::SupervoxelAdjacencyList (); + } + } + + /** \brief Set normal threshold + * \param[in] concavity_tolerance_threshold_arg the concavity tolerance angle in [deg] to set */ + inline void + setConcavityToleranceThreshold (float concavity_tolerance_threshold_arg) + { + concavity_tolerance_threshold_ = concavity_tolerance_threshold_arg; + } + + /** \brief Determines if a smoothness check is done during segmentation, trying to invalidate edges of non-smooth connected edges (steps). Two supervoxels are unsmooth if their plane-to-plane distance DIST > (expected_distance + smoothness_threshold_*voxel_resolution_). For parallel supervoxels, the expected_distance is zero. + * \param[in] use_smoothness_check_arg Determines if the smoothness check is used + * \param[in] voxel_res_arg The voxel resolution used for the supervoxels that are segmented + * \param[in] seed_res_arg The seed resolution used for the supervoxels that are segmented + * \param[in] smoothness_threshold_arg Threshold (/fudging factor) for smoothness constraint according to the above formula. */ + inline void + setSmoothnessCheck (bool use_smoothness_check_arg, + float voxel_res_arg, + float seed_res_arg, + float smoothness_threshold_arg = 0.1) + { + use_smoothness_check_ = use_smoothness_check_arg; + voxel_resolution_ = voxel_res_arg; + seed_resolution_ = seed_res_arg; + smoothness_threshold_ = smoothness_threshold_arg; + } + + /** \brief Determines if we want to use the sanity criterion to invalidate singular connected patches + * \param[in] use_sanity_criterion_arg Determines if the sanity check is performed */ + inline void + setSanityCheck (const bool use_sanity_criterion_arg) + { + use_sanity_check_ = use_sanity_criterion_arg; + } + + /** \brief Set the value used for k convexity. For k>0 convex connections between p_i and p_j require k common neighbors of these patches that have a convex connection to both. + * \param[in] k_factor_arg factor used for extended convexity check */ + inline void + setKFactor (const std::uint32_t k_factor_arg) + { + k_factor_ = k_factor_arg; + } + + /** \brief Set the value \ref min_segment_size_ used in \ref mergeSmallSegments + * \param[in] min_segment_size_arg Segments smaller than this size will be merged */ + inline void + setMinSegmentSize (const std::uint32_t min_segment_size_arg) + { + min_segment_size_ = min_segment_size_arg; + } + + protected: + + /** \brief Segments smaller than \ref min_segment_size_ are merged to the label of largest neighbor */ + void + mergeSmallSegments (); + + /** \brief Compute the adjacency of the segments */ + void + computeSegmentAdjacency (); + + /** \brief Is called within \ref setInputSupervoxels mainly to reserve required memory. + * \param[in] supervoxel_clusters_arg map of < supervoxel labels, supervoxels > + * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations */ + void + prepareSegmentation (const std::map::Ptr> &supervoxel_clusters_arg, + const std::multimap &label_adjacency_arg); + + + /** Perform depth search on the graph and recursively group all supervoxels with convex connections + * \note The vertices in the supervoxel adjacency list are the supervoxel centroids */ + void + doGrouping (); + + /** \brief Assigns neighbors of the query point to the same group as the query point. Recursive part of \ref doGrouping. Grouping is done by a depth-search of nodes in the adjacency-graph. + * \param[in] queryPointID ID of point whose neighbors will be considered for grouping + * \param[in] group_label ID of the group/segment the queried point belongs to */ + void + recursiveSegmentGrowing (const VertexID &queryPointID, + const unsigned int group_label); + + /** \brief Calculates convexity of edges and saves this to the adjacency graph. + * \param[in,out] adjacency_list_arg The supervoxel adjacency list*/ + void + calculateConvexConnections (SupervoxelAdjacencyList& adjacency_list_arg); + + /** \brief Connections are only convex if this is true for at least k_arg common neighbors of the two patches. Call \ref setKFactor before \ref segment to use this. + * \param[in] k_arg Factor used for extended convexity check */ + void + applyKconvexity (const unsigned int k_arg); + + /** \brief Returns true if the connection between source and target is convex. + * \param[in] source_label_arg Label of one supervoxel connected to the edge that should be checked + * \param[in] target_label_arg Label of the other supervoxel connected to the edge that should be checked + * \param[out] normal_angle The angle between source and target + * \return True if connection is convex */ + bool + connIsConvex (const std::uint32_t source_label_arg, + const std::uint32_t target_label_arg, + float &normal_angle); + + /// *** Parameters *** /// + + /** \brief Normal Threshold in degrees [0,180] used for merging */ + float concavity_tolerance_threshold_; + + /** \brief Marks if valid grouping data (\ref sv_adjacency_list_, \ref sv_label_to_seg_label_map_, \ref processed_) is available */ + bool grouping_data_valid_; + + /** \brief Marks if supervoxels have been set by calling \ref setInputSupervoxels */ + bool supervoxels_set_; + + /** \brief Determines if the smoothness check is used during segmentation*/ + bool use_smoothness_check_; + + /** \brief Two supervoxels are unsmooth if their plane-to-plane distance DIST > (expected_distance + smoothness_threshold_*voxel_resolution_). For parallel supervoxels, the expected_distance is zero. */ + float smoothness_threshold_; + + /** \brief Determines if we use the sanity check which tries to find and invalidate singular connected patches*/ + bool use_sanity_check_; + + /** \brief Seed resolution of the supervoxels (used only for smoothness check) */ + float seed_resolution_; + + /** \brief Voxel resolution used to build the supervoxels (used only for smoothness check)*/ + float voxel_resolution_; + + /** \brief Factor used for k-convexity */ + std::uint32_t k_factor_; + + /** \brief Minimum segment size */ + std::uint32_t min_segment_size_; + + /** \brief Stores which supervoxel labels were already visited during recursive grouping. + * \note processed_[sv_Label] = false (default)/true (already processed) */ + std::map processed_; + + /** \brief Adjacency graph with the supervoxel labels as nodes and edges between adjacent supervoxels */ + SupervoxelAdjacencyList sv_adjacency_list_; + + /** \brief map from the supervoxel labels to the supervoxel objects */ + std::map::Ptr> sv_label_to_supervoxel_map_; + + /** \brief Storing relation between original SuperVoxel Labels and new segmantion labels. + * \note sv_label_to_seg_label_map_[old_labelID] = new_labelID */ + std::map sv_label_to_seg_label_map_; + + /** \brief map > */ + std::map > seg_label_to_sv_list_map_; + + /** \brief map < SegmentID, std::set< Neighboring segment labels> > */ + std::map > seg_label_to_neighbor_set_map_; + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/min_cut_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/min_cut_segmentation.h new file mode 100755 index 0000000..5307e32 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/min_cut_segmentation.h @@ -0,0 +1,328 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id:$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief This class implements the segmentation algorithm based on minimal cut of the graph. + * The description can be found in the article: + * "Min-Cut Based Segmentation of Point Clouds" + * \author: Aleksey Golovinskiy and Thomas Funkhouser. + */ + template + class PCL_EXPORTS MinCutSegmentation : public pcl::PCLBase + { + public: + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + public: + + using Traits = boost::adjacency_list_traits< boost::vecS, boost::vecS, boost::directedS >; + + using mGraph = boost::adjacency_list< boost::vecS, boost::vecS, boost::directedS, + boost::property< boost::vertex_name_t, std::string, + boost::property< boost::vertex_index_t, long, + boost::property< boost::vertex_color_t, boost::default_color_type, + boost::property< boost::vertex_distance_t, long, + boost::property< boost::vertex_predecessor_t, Traits::edge_descriptor > > > > >, + boost::property< boost::edge_capacity_t, double, + boost::property< boost::edge_residual_capacity_t, double, + boost::property< boost::edge_reverse_t, Traits::edge_descriptor > > > >; + + using CapacityMap = boost::property_map< mGraph, boost::edge_capacity_t >::type; + + using ReverseEdgeMap = boost::property_map< mGraph, boost::edge_reverse_t>::type; + + using VertexDescriptor = Traits::vertex_descriptor; + + using EdgeDescriptor = boost::graph_traits::edge_descriptor; + + using OutEdgeIterator = boost::graph_traits::out_edge_iterator; + + using VertexIterator = boost::graph_traits::vertex_iterator; + + using ResidualCapacityMap = boost::property_map< mGraph, boost::edge_residual_capacity_t >::type; + + using IndexMap = boost::property_map< mGraph, boost::vertex_index_t >::type; + + using InEdgeIterator = boost::graph_traits::in_edge_iterator; + + using mGraphPtr = shared_ptr; + + public: + + /** \brief Constructor that sets default values for member variables. */ + MinCutSegmentation (); + + /** \brief Destructor that frees memory. */ + + ~MinCutSegmentation (); + + /** \brief This method simply sets the input point cloud. + * \param[in] cloud the const boost shared pointer to a PointCloud + */ + void + setInputCloud (const PointCloudConstPtr &cloud) override; + + /** \brief Returns normalization value for binary potentials. For more information see the article. */ + double + getSigma () const; + + /** \brief Allows to set the normalization value for the binary potentials as described in the article. + * \param[in] sigma new normalization value + */ + void + setSigma (double sigma); + + /** \brief Returns radius to the background. */ + double + getRadius () const; + + /** \brief Allows to set the radius to the background. + * \param[in] radius new radius to the background + */ + void + setRadius (double radius); + + /** \brief Returns weight that every edge from the source point has. */ + double + getSourceWeight () const; + + /** \brief Allows to set weight for source edges. Every edge that comes from the source point will have that weight. + * \param[in] weight new weight + */ + void + setSourceWeight (double weight); + + /** \brief Returns search method that is used for finding KNN. + * The graph is build such way that it contains the edges that connect point and its KNN. + */ + KdTreePtr + getSearchMethod () const; + + /** \brief Allows to set search method for finding KNN. + * The graph is build such way that it contains the edges that connect point and its KNN. + * \param[in] search search method that will be used for finding KNN. + */ + void + setSearchMethod (const KdTreePtr& tree); + + /** \brief Returns the number of neighbours to find. */ + unsigned int + getNumberOfNeighbours () const; + + /** \brief Allows to set the number of neighbours to find. + * \param[in] number_of_neighbours new number of neighbours + */ + void + setNumberOfNeighbours (unsigned int neighbour_number); + + /** \brief Returns the points that must belong to foreground. */ + std::vector > + getForegroundPoints () const; + + /** \brief Allows to specify points which are known to be the points of the object. + * \param[in] foreground_points point cloud that contains foreground points. At least one point must be specified. + */ + void + setForegroundPoints (typename pcl::PointCloud::Ptr foreground_points); + + /** \brief Returns the points that must belong to background. */ + std::vector > + getBackgroundPoints () const; + + /** \brief Allows to specify points which are known to be the points of the background. + * \param[in] background_points point cloud that contains background points. + */ + void + setBackgroundPoints (typename pcl::PointCloud::Ptr background_points); + + /** \brief This method launches the segmentation algorithm and returns the clusters that were + * obtained during the segmentation. The indices of points that belong to the object will be stored + * in the cluster with index 1, other indices will be stored in the cluster with index 0. + * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices. + */ + void + extract (std::vector & clusters); + + /** \brief Returns that flow value that was calculated during the segmentation. */ + double + getMaxFlow () const; + + /** \brief Returns the graph that was build for finding the minimum cut. */ + mGraphPtr + getGraph () const; + + /** \brief Returns the colored cloud. Points that belong to the object have the same color. */ + pcl::PointCloud::Ptr + getColoredCloud (); + + protected: + + /** \brief This method simply builds the graph that will be used during the segmentation. */ + bool + buildGraph (); + + /** \brief Returns unary potential(data cost) for the given point index. + * In other words it calculates weights for (source, point) and (point, sink) edges. + * \param[in] point index of the point for which weights will be calculated + * \param[out] source_weight calculated weight for the (source, point) edge + * \param[out] sink_weight calculated weight for the (point, sink) edge + */ + void + calculateUnaryPotential (int point, double& source_weight, double& sink_weight) const; + + /** \brief This method simply adds the edge from the source point to the target point with a given weight. + * \param[in] source index of the source point of the edge + * \param[in] target index of the target point of the edge + * \param[in] weight weight that will be assigned to the (source, target) edge + */ + bool + addEdge (int source, int target, double weight); + + /** \brief Returns the binary potential(smooth cost) for the given indices of points. + * In other words it returns weight that must be assigned to the edge from source to target point. + * \param[in] source index of the source point of the edge + * \param[in] target index of the target point of the edge + */ + double + calculateBinaryPotential (int source, int target) const; + + /** \brief This method recalculates unary potentials(data cost) if some changes were made, instead of creating new graph. */ + bool + recalculateUnaryPotentials (); + + /** \brief This method recalculates binary potentials(smooth cost) if some changes were made, instead of creating new graph. */ + bool + recalculateBinaryPotentials (); + + /** \brief This method analyzes the residual network and assigns a label to every point in the cloud. + * \param[in] residual_capacity residual network that was obtained during the segmentation + */ + void + assembleLabels (ResidualCapacityMap& residual_capacity); + + protected: + + /** \brief Stores the sigma coefficient. It is used for finding smooth costs. More information can be found in the article. */ + double inverse_sigma_; + + /** \brief Signalizes if the binary potentials are valid. */ + bool binary_potentials_are_valid_; + + /** \brief Used for comparison of the floating point numbers. */ + double epsilon_; + + /** \brief Stores the distance to the background. */ + double radius_; + + /** \brief Signalizes if the unary potentials are valid. */ + bool unary_potentials_are_valid_; + + /** \brief Stores the weight for every edge that comes from source point. */ + double source_weight_; + + /** \brief Stores the search method that will be used for finding K nearest neighbors. Neighbours are used for building the graph. */ + KdTreePtr search_; + + /** \brief Stores the number of neighbors to find. */ + unsigned int number_of_neighbours_; + + /** \brief Signalizes if the graph is valid. */ + bool graph_is_valid_; + + /** \brief Stores the points that are known to be in the foreground. */ + std::vector > foreground_points_; + + /** \brief Stores the points that are known to be in the background. */ + std::vector > background_points_; + + /** \brief After the segmentation it will contain the segments. */ + std::vector clusters_; + + /** \brief Stores the graph for finding the maximum flow. */ + mGraphPtr graph_; + + /** \brief Stores the capacity of every edge in the graph. */ + std::shared_ptr capacity_; + + /** \brief Stores reverse edges for every edge in the graph. */ + std::shared_ptr reverse_edges_; + + /** \brief Stores the vertices of the graph. */ + std::vector< VertexDescriptor > vertices_; + + /** \brief Stores the information about the edges that were added to the graph. It is used to avoid the duplicate edges. */ + std::vector< std::set > edge_marker_; + + /** \brief Stores the vertex that serves as source. */ + VertexDescriptor source_; + + /** \brief Stores the vertex that serves as sink. */ + VertexDescriptor sink_; + + /** \brief Stores the maximum flow value that was calculated during the segmentation. */ + double max_flow_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/organized_connected_component_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/organized_connected_component_segmentation.h new file mode 100755 index 0000000..99c4524 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/organized_connected_component_segmentation.h @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief OrganizedConnectedComponentSegmentation allows connected + * components to be found within organized point cloud data, given a + * comparison function. Given an input cloud and a comparator, it will + * output a PointCloud of labels, giving each connected component a unique + * id, along with a vector of PointIndices corresponding to each component. + * See OrganizedMultiPlaneSegmentation for an example application. + * + * \author Alex Trevor, Suat Gedikli + */ + template + class OrganizedConnectedComponentSegmentation : public PCLBase + { + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + using Comparator = pcl::Comparator; + using ComparatorPtr = typename Comparator::Ptr; + using ComparatorConstPtr = typename Comparator::ConstPtr; + + /** \brief Constructor for OrganizedConnectedComponentSegmentation + * \param[in] compare A pointer to the comparator to be used for segmentation. Must be an instance of pcl::Comparator. + */ + OrganizedConnectedComponentSegmentation (const ComparatorConstPtr& compare) + : compare_ (compare) + { + } + + /** \brief Destructor for OrganizedConnectedComponentSegmentation. */ + + ~OrganizedConnectedComponentSegmentation () + { + } + + /** \brief Provide a pointer to the comparator to be used for segmentation. + * \param[in] compare the comparator + */ + void + setComparator (const ComparatorConstPtr& compare) + { + compare_ = compare; + } + + /** \brief Get the comparator.*/ + ComparatorConstPtr + getComparator () const { return (compare_); } + + /** \brief Perform the connected component segmentation. + * \param[out] labels a PointCloud of labels: each connected component will have a unique id. + * \param[out] label_indices a vector of PointIndices corresponding to each label / component id. + */ + void + segment (pcl::PointCloud& labels, std::vector& label_indices) const; + + /** \brief Find the boundary points / contour of a connected component + * \param[in] start_idx the first (lowest) index of the connected component for which a boundary should be returned + * \param[in] labels the Label cloud produced by segmentation + * \param[out] boundary_indices the indices of the boundary points for the label corresponding to start_idx + */ + static void + findLabeledRegionBoundary (int start_idx, PointCloudLPtr labels, pcl::PointIndices& boundary_indices); + + + protected: + ComparatorConstPtr compare_; + + inline unsigned + findRoot (const std::vector& runs, unsigned index) const + { + unsigned idx = index; + while (runs[idx] != idx) + idx = runs[idx]; + + return (idx); + } + + private: + struct Neighbor + { + Neighbor (int dx, int dy, int didx) + : d_x (dx) + , d_y (dy) + , d_index (didx) + {} + + int d_x; + int d_y; + int d_index; // = dy * width + dx: pre-calculated + }; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/organized_multi_plane_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/organized_multi_plane_segmentation.h new file mode 100755 index 0000000..e73b7a2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/organized_multi_plane_segmentation.h @@ -0,0 +1,338 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief OrganizedMultiPlaneSegmentation finds all planes present in the + * input cloud, and outputs a vector of plane equations, as well as a vector + * of point clouds corresponding to the inliers of each detected plane. Only + * planes with more than min_inliers points are detected. + * Templated on point type, normal type, and label type + * + * \author Alex Trevor, Suat Gedikli + */ + template + class OrganizedMultiPlaneSegmentation : public PCLBase + { + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + using PlaneComparator = pcl::PlaneCoefficientComparator; + using PlaneComparatorPtr = typename PlaneComparator::Ptr; + using PlaneComparatorConstPtr = typename PlaneComparator::ConstPtr; + + using PlaneRefinementComparator = pcl::PlaneRefinementComparator; + using PlaneRefinementComparatorPtr = typename PlaneRefinementComparator::Ptr; + using PlaneRefinementComparatorConstPtr = typename PlaneRefinementComparator::ConstPtr; + + /** \brief Constructor for OrganizedMultiPlaneSegmentation. */ + OrganizedMultiPlaneSegmentation () : + normals_ (), + min_inliers_ (1000), + angular_threshold_ (pcl::deg2rad (3.0)), + distance_threshold_ (0.02), + maximum_curvature_ (0.001), + project_points_ (false), + compare_ (new PlaneComparator ()), refinement_compare_ (new PlaneRefinementComparator ()) + { + } + + /** \brief Destructor for OrganizedMultiPlaneSegmentation. */ + + ~OrganizedMultiPlaneSegmentation () + { + } + + /** \brief Provide a pointer to the input normals. + * \param[in] normals the input normal cloud + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) + { + normals_ = normals; + } + + /** \brief Get the input normals. */ + inline PointCloudNConstPtr + getInputNormals () const + { + return (normals_); + } + + /** \brief Set the minimum number of inliers required for a plane + * \param[in] min_inliers the minimum number of inliers required per plane + */ + inline void + setMinInliers (unsigned min_inliers) + { + min_inliers_ = min_inliers; + } + + /** \brief Get the minimum number of inliers required per plane. */ + inline unsigned + getMinInliers () const + { + return (min_inliers_); + } + + /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. + * \param[in] angular_threshold the tolerance in radians + */ + inline void + setAngularThreshold (double angular_threshold) + { + angular_threshold_ = angular_threshold; + } + + /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ + inline double + getAngularThreshold () const + { + return (angular_threshold_); + } + + /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. + * \param[in] distance_threshold the tolerance in meters + */ + inline void + setDistanceThreshold (double distance_threshold) + { + distance_threshold_ = distance_threshold; + } + + /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */ + inline double + getDistanceThreshold () const + { + return (distance_threshold_); + } + + /** \brief Set the maximum curvature allowed for a planar region. + * \param[in] maximum_curvature the maximum curvature + */ + inline void + setMaximumCurvature (double maximum_curvature) + { + maximum_curvature_ = maximum_curvature; + } + + /** \brief Get the maximum curvature allowed for a planar region. */ + inline double + getMaximumCurvature () const + { + return (maximum_curvature_); + } + + /** \brief Provide a pointer to the comparator to be used for segmentation. + * \param[in] compare A pointer to the comparator to be used for segmentation. + */ + void + setComparator (const PlaneComparatorPtr& compare) + { + compare_ = compare; + } + + /** \brief Provide a pointer to the comparator to be used for refinement. + * \param[in] compare A pointer to the comparator to be used for refinement. + */ + void + setRefinementComparator (const PlaneRefinementComparatorPtr& compare) + { + refinement_compare_ = compare; + } + + /** \brief Set whether or not to project boundary points to the plane, or leave them in the original 3D space. + * \param[in] project_points true if points should be projected, false if not. + */ + void + setProjectPoints (bool project_points) + { + project_points_ = project_points; + } + + /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices() + * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud + * \param[out] inlier_indices a vector of inliers for each detected plane + * \param[out] centroids a vector of centroids for each plane + * \param[out] covariances a vector of covariance matricies for the inliers of each plane + * \param[out] labels a point cloud for the connected component labels of each pixel + * \param[out] label_indices a vector of PointIndices for each labeled component + */ + void + segment (std::vector& model_coefficients, + std::vector& inlier_indices, + std::vector >& centroids, + std::vector >& covariances, + pcl::PointCloud& labels, + std::vector& label_indices); + + /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices() + * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud + * \param[out] inlier_indices a vector of inliers for each detected plane + */ + void + segment (std::vector& model_coefficients, + std::vector& inlier_indices); + + /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices() + * \param[out] regions a list of resultant planar polygonal regions + */ + void + segment (std::vector, Eigen::aligned_allocator > >& regions); + + /** \brief Perform a segmentation, as well as an additional refinement step. This helps with including points whose normals may not match neighboring points well, but may match the planar model well. + * \param[out] regions A list of regions generated by segmentation and refinement. + */ + void + segmentAndRefine (std::vector, Eigen::aligned_allocator > >& regions); + + /** \brief Perform a segmentation, as well as additional refinement step. Returns intermediate data structures for use in + * subsequent processing. + * \param[out] regions A vector of PlanarRegions generated by segmentation + * \param[out] model_coefficients A vector of model coefficients for each segmented plane + * \param[out] inlier_indices A vector of PointIndices, indicating the inliers to each segmented plane + * \param[out] labels A PointCloud corresponding to the resulting segmentation. + * \param[out] label_indices A vector of PointIndices for each label + * \param[out] boundary_indices A vector of PointIndices corresponding to the outer boundary / contour of each label + */ + void + segmentAndRefine (std::vector, Eigen::aligned_allocator > >& regions, + std::vector& model_coefficients, + std::vector& inlier_indices, + PointCloudLPtr& labels, + std::vector& label_indices, + std::vector& boundary_indices); + + /** \brief Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by the initial segmentation. + * \param [in] model_coefficients The list of segmented model coefficients + * \param [in] inlier_indices The list of segmented inlier indices, corresponding to each model + * \param [in] labels The labels produced by the initial segmentation + * \param [in] label_indices The list of indices corresponding to each label + */ + void + refine (std::vector& model_coefficients, + std::vector& inlier_indices, + PointCloudLPtr& labels, + std::vector& label_indices); + + /** \brief Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by the initial segmentation. + * \param [in] model_coefficients The list of segmented model coefficients + * \param [in] inlier_indices The list of segmented inlier indices, corresponding to each model + * \param [in] centroids The list of centroids corresponding to each segmented plane + * \param [in] covariances The list of covariances corresponding to each segemented plane + * \param [in] labels The labels produced by the initial segmentation + * \param [in] label_indices The list of indices corresponding to each label + */ + PCL_DEPRECATED("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases") + void + refine (std::vector& model_coefficients, + std::vector& inlier_indices, + std::vector >& /*centroids*/, + std::vector >& /*covariances*/, + PointCloudLPtr& labels, + std::vector& label_indices) + { + refine(model_coefficients, inlier_indices, labels, label_indices); + } + + protected: + + /** \brief A pointer to the input normals */ + PointCloudNConstPtr normals_; + + /** \brief The minimum number of inliers required for each plane. */ + unsigned min_inliers_; + + /** \brief The tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ + double angular_threshold_; + + /** \brief The tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. */ + double distance_threshold_; + + /** \brief The tolerance for maximum curvature after fitting a plane. Used to remove smooth, but non-planar regions. */ + double maximum_curvature_; + + /** \brief Whether or not points should be projected to the plane, or left in the original 3D space. */ + bool project_points_; + + /** \brief A comparator for comparing neighboring pixels' plane equations. */ + PlaneComparatorPtr compare_; + + /** \brief A comparator for use on the refinement step. Compares points to regions segmented in the first pass. */ + PlaneRefinementComparatorPtr refinement_compare_; + + /** \brief Class getName method. */ + virtual std::string + getClassName () const + { + return ("OrganizedMultiPlaneSegmentation"); + } + }; + +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/planar_polygon_fusion.h b/deps_install/include/pcl-1.10/pcl/segmentation/planar_polygon_fusion.h new file mode 100755 index 0000000..977ec8e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/planar_polygon_fusion.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief PlanarPolygonFusion takes a list of 2D planar polygons and + * attempts to reduce them to a minimum set that best represents the scene, + * based on various given comparators. + */ + template + class PlanarPolygonFusion + { + public: + /** \brief Constructor */ + PlanarPolygonFusion () : regions_ () {} + + /** \brief Destructor */ + virtual ~PlanarPolygonFusion () {} + + /** \brief Reset the state (clean the list of planar models). */ + void + reset () + { + regions_.clear (); + } + + /** \brief Set the list of 2D planar polygons to refine. + * \param[in] input the list of 2D planar polygons to refine + */ + void + addInputPolygons (const std::vector, Eigen::aligned_allocator > > &input) + { + int start = static_cast (regions_.size ()); + regions_.resize (regions_.size () + input.size ()); + for(std::size_t i = 0; i < input.size (); i++) + regions_[start+i] = input[i]; + } + + protected: + /** \brief Internal list of planar states. */ + std::vector, Eigen::aligned_allocator > > regions_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/planar_region.h b/deps_install/include/pcl-1.10/pcl/segmentation/planar_region.h new file mode 100755 index 0000000..69f1ec2 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/planar_region.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief PlanarRegion represents a set of points that lie in a plane. Inherits summary statistics about these points from Region3D, and summary statistics of a 3D collection of points. + * \author Alex Trevor + */ + template + class PlanarRegion : public pcl::Region3D, public pcl::PlanarPolygon + { + protected: + using Region3D::centroid_; + using Region3D::covariance_; + using Region3D::count_; + using PlanarPolygon::contour_; + using PlanarPolygon::coefficients_; + + public: + /** \brief Empty constructor for PlanarRegion. */ + PlanarRegion () + {} + + /** \brief Constructor for Planar region from a Region3D and a PlanarPolygon. + * \param[in] region a Region3D for the input data + * \param[in] polygon a PlanarPolygon for the input region + */ + PlanarRegion (const pcl::Region3D& region, const pcl::PlanarPolygon& polygon) + { + centroid_ = region.centroid; + covariance_ = region.covariance; + count_ = region.count; + contour_ = polygon.contour; + coefficients_ = polygon.coefficients; + } + + /** \brief Destructor. */ + ~PlanarRegion () {} + + /** \brief Constructor for PlanarRegion. + * \param[in] centroid the centroid of the region. + * \param[in] covariance the covariance of the region. + * \param[in] count the number of points in the region. + * \param[in] contour the contour / boudnary for the region + * \param[in] coefficients the model coefficients (a,b,c,d) for the plane + */ + PlanarRegion (const Eigen::Vector3f& centroid, const Eigen::Matrix3f& covariance, unsigned count, + const typename pcl::PointCloud::VectorType& contour, + const Eigen::Vector4f& coefficients) + { + centroid_ = centroid; + covariance_ = covariance; + count_ = count; + contour_ = contour; + coefficients_ = coefficients; + } + + private: + /** \brief The labels (good=true, bad=false) for whether or not this boundary was observed, + * or was due to edge of frame / occlusion boundary. + */ + std::vector contour_labels_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/plane_coefficient_comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/plane_coefficient_comparator.h new file mode 100755 index 0000000..d793397 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/plane_coefficient_comparator.h @@ -0,0 +1,212 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar segmentation. + * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. + * + * \author Alex Trevor + */ + template + class PlaneCoefficientComparator: public Comparator + { + public: + using PointCloud = typename Comparator::PointCloud; + using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::Comparator::input_; + + /** \brief Empty constructor for PlaneCoefficientComparator. */ + PlaneCoefficientComparator () + : normals_ () + , angular_threshold_ (pcl::deg2rad (2.0f)) + , distance_threshold_ (0.02f) + , depth_dependent_ (true) + , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) ) + { + } + + /** \brief Constructor for PlaneCoefficientComparator. + * \param[in] plane_coeff_d a reference to a vector of d coefficients of plane equations. Must be the same size as the input cloud and input normals. a, b, and c coefficients are in the input normals. + */ + PlaneCoefficientComparator (shared_ptr >& plane_coeff_d) + : normals_ () + , plane_coeff_d_ (plane_coeff_d) + , angular_threshold_ (pcl::deg2rad (2.0f)) + , distance_threshold_ (0.02f) + , depth_dependent_ (true) + , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f) ) + { + } + + /** \brief Destructor for PlaneCoefficientComparator. */ + + ~PlaneCoefficientComparator () + { + } + + void + setInputCloud (const PointCloudConstPtr& cloud) override + { + input_ = cloud; + } + + /** \brief Provide a pointer to the input normals. + * \param[in] normals the input normal cloud + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) + { + normals_ = normals; + } + + /** \brief Get the input normals. */ + inline PointCloudNConstPtr + getInputNormals () const + { + return (normals_); + } + + /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud. + * \param[in] plane_coeff_d a pointer to the plane coefficients. + */ + void + setPlaneCoeffD (shared_ptr >& plane_coeff_d) + { + plane_coeff_d_ = plane_coeff_d; + } + + /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form. a, b, and c are provided by the normal cloud. + * \param[in] plane_coeff_d a pointer to the plane coefficients. + */ + void + setPlaneCoeffD (std::vector& plane_coeff_d) + { + plane_coeff_d_ = boost::make_shared >(plane_coeff_d); + } + + /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */ + const std::vector& + getPlaneCoeffD () const + { + return (*plane_coeff_d_); + } + + /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. + * \param[in] angular_threshold the tolerance in radians + */ + virtual void + setAngularThreshold (float angular_threshold) + { + angular_threshold_ = std::cos (angular_threshold); + } + + /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ + inline float + getAngularThreshold () const + { + return (std::acos (angular_threshold_) ); + } + + /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. + * \param[in] distance_threshold the tolerance in meters (at 1m) + * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false) + */ + void + setDistanceThreshold (float distance_threshold, + bool depth_dependent = false) + { + distance_threshold_ = distance_threshold; + depth_dependent_ = depth_dependent; + } + + /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */ + inline float + getDistanceThreshold () const + { + return (distance_threshold_); + } + + /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, + * and the difference between the d component of the normals is less than distance threshold, else false + * \param idx1 The first index for the comparison + * \param idx2 The second index for the comparison + */ + bool + compare (int idx1, int idx2) const override + { + float threshold = distance_threshold_; + if (depth_dependent_) + { + Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); + + float z = vec.dot (z_axis_); + threshold *= z * z; + } + return ( (std::fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < threshold) + && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) ); + } + + protected: + PointCloudNConstPtr normals_; + shared_ptr > plane_coeff_d_; + float angular_threshold_; + float distance_threshold_; + bool depth_dependent_; + Eigen::Vector3f z_axis_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/plane_refinement_comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/plane_refinement_comparator.h new file mode 100755 index 0000000..7c4bb0e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/plane_refinement_comparator.h @@ -0,0 +1,216 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: extract_clusters.h 5027 2012-03-12 03:10:45Z rusu $ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief PlaneRefinementComparator is a Comparator that operates on plane coefficients, + * for use in planar segmentation. + * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. + * + * \author Alex Trevor, Suat Gedikli + */ + template + class PlaneRefinementComparator: public PlaneCoefficientComparator + { + public: + using PointCloud = typename Comparator::PointCloud; + using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using PointCloudL = pcl::PointCloud; + using PointCloudLPtr = typename PointCloudL::Ptr; + using PointCloudLConstPtr = typename PointCloudL::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::PlaneCoefficientComparator::input_; + using pcl::PlaneCoefficientComparator::normals_; + using pcl::PlaneCoefficientComparator::distance_threshold_; + using pcl::PlaneCoefficientComparator::plane_coeff_d_; + + + /** \brief Empty constructor for PlaneCoefficientComparator. */ + PlaneRefinementComparator () + : labels_ () + , depth_dependent_ (false) + { + } + + /** \brief Empty constructor for PlaneCoefficientComparator. + * \param[in] models + * \param[in] refine_labels + */ + PlaneRefinementComparator (shared_ptr >& models, + shared_ptr >& refine_labels) + : models_ (models) + , labels_ () + , refine_labels_ (refine_labels) + , depth_dependent_ (false) + { + } + + /** \brief Destructor for PlaneCoefficientComparator. */ + + ~PlaneRefinementComparator () + { + } + + /** \brief Set the vector of model coefficients to which we will compare. + * \param[in] models a vector of model coefficients produced by the initial segmentation step. + */ + void + setModelCoefficients (shared_ptr >& models) + { + models_ = models; + } + + /** \brief Set the vector of model coefficients to which we will compare. + * \param[in] models a vector of model coefficients produced by the initial segmentation step. + */ + void + setModelCoefficients (std::vector& models) + { + models_ = boost::make_shared >(models); + } + + /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined. + * \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined. + */ + void + setRefineLabels (shared_ptr >& refine_labels) + { + refine_labels_ = refine_labels; + } + + /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined. + * \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined. + */ + void + setRefineLabels (std::vector& refine_labels) + { + refine_labels_ = boost::make_shared >(refine_labels); + } + + /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed. + * \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models + */ + inline void + setLabelToModel (shared_ptr >& label_to_model) + { + label_to_model_ = label_to_model; + } + + /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed. + * \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models + */ + inline void + setLabelToModel (std::vector& label_to_model) + { + label_to_model_ = boost::make_shared >(label_to_model); + } + + /** \brief Get the vector of model coefficients to which we will compare. */ + inline shared_ptr > + getModelCoefficients () const + { + return (models_); + } + + /** \brief ... + * \param[in] labels + */ + inline void + setLabels (PointCloudLPtr& labels) + { + labels_ = labels; + } + + /** \brief Compare two neighboring points + * \param[in] idx1 The index of the first point. + * \param[in] idx2 The index of the second point. + */ + bool + compare (int idx1, int idx2) const override + { + int current_label = labels_->points[idx1].label; + int next_label = labels_->points[idx2].label; + + if (!((*refine_labels_)[current_label] && !(*refine_labels_)[next_label])) + return (false); + + const pcl::ModelCoefficients& model_coeff = (*models_)[(*label_to_model_)[current_label]]; + + PointT pt = input_->points[idx2]; + double ptp_dist = std::fabs (model_coeff.values[0] * pt.x + + model_coeff.values[1] * pt.y + + model_coeff.values[2] * pt.z + + model_coeff.values[3]); + + // depth dependent + float threshold = distance_threshold_; + if (depth_dependent_) + { + //Eigen::Vector4f origin = input_->sensor_origin_; + Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();// - origin.head<3> (); + + float z = vec.dot (z_axis_); + threshold *= z * z; + } + + return (ptp_dist < threshold); + } + + protected: + shared_ptr > models_; + PointCloudLPtr labels_; + shared_ptr > refine_labels_; + shared_ptr > label_to_model_; + bool depth_dependent_; + using PlaneCoefficientComparator::z_axis_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/progressive_morphological_filter.h b/deps_install/include/pcl-1.10/pcl/segmentation/progressive_morphological_filter.h new file mode 100755 index 0000000..d31b9f3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/progressive_morphological_filter.h @@ -0,0 +1,165 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * Implements the Progressive Morphological Filter for segmentation of ground points. + * Description can be found in the article + * "A Progressive Morphological Filter for Removing Nonground Measurements from + * Airborne LIDAR Data" + * by K. Zhang, S. Chen, D. Whitman, M. Shyu, J. Yan, and C. Zhang. + */ + template + class PCL_EXPORTS ProgressiveMorphologicalFilter : public pcl::PCLBase + { + public: + + using PointCloud = pcl::PointCloud; + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + public: + + /** \brief Constructor that sets default values for member variables. */ + ProgressiveMorphologicalFilter (); + + + ~ProgressiveMorphologicalFilter (); + + /** \brief Get the maximum window size to be used in filtering ground returns. */ + inline int + getMaxWindowSize () const { return (max_window_size_); } + + /** \brief Set the maximum window size to be used in filtering ground returns. */ + inline void + setMaxWindowSize (int max_window_size) { max_window_size_ = max_window_size; } + + /** \brief Get the slope value to be used in computing the height threshold. */ + inline float + getSlope () const { return (slope_); } + + /** \brief Set the slope value to be used in computing the height threshold. */ + inline void + setSlope (float slope) { slope_ = slope; } + + /** \brief Get the maximum height above the parameterized ground surface to be considered a ground return. */ + inline float + getMaxDistance () const { return (max_distance_); } + + /** \brief Set the maximum height above the parameterized ground surface to be considered a ground return. */ + inline void + setMaxDistance (float max_distance) { max_distance_ = max_distance; } + + /** \brief Get the initial height above the parameterized ground surface to be considered a ground return. */ + inline float + getInitialDistance () const { return (initial_distance_); } + + /** \brief Set the initial height above the parameterized ground surface to be considered a ground return. */ + inline void + setInitialDistance (float initial_distance) { initial_distance_ = initial_distance; } + + /** \brief Get the cell size. */ + inline float + getCellSize () const { return (cell_size_); } + + /** \brief Set the cell size. */ + inline void + setCellSize (float cell_size) { cell_size_ = cell_size; } + + /** \brief Get the base to be used in computing progressive window sizes. */ + inline float + getBase () const { return (base_); } + + /** \brief Set the base to be used in computing progressive window sizes. */ + inline void + setBase (float base) { base_ = base; } + + /** \brief Get flag indicating whether or not to exponentially grow window sizes? */ + inline bool + getExponential () const { return (exponential_); } + + /** \brief Set flag indicating whether or not to exponentially grow window sizes? */ + inline void + setExponential (bool exponential) { exponential_ = exponential; } + + /** \brief This method launches the segmentation algorithm and returns indices of + * points determined to be ground returns. + * \param[out] ground indices of points determined to be ground returns. + */ + virtual void + extract (std::vector& ground); + + protected: + + /** \brief Maximum window size to be used in filtering ground returns. */ + int max_window_size_; + + /** \brief Slope value to be used in computing the height threshold. */ + float slope_; + + /** \brief Maximum height above the parameterized ground surface to be considered a ground return. */ + float max_distance_; + + /** \brief Initial height above the parameterized ground surface to be considered a ground return. */ + float initial_distance_; + + /** \brief Cell size. */ + float cell_size_; + + /** \brief Base to be used in computing progressive window sizes. */ + float base_; + + /** \brief Exponentially grow window sizes? */ + bool exponential_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/random_walker.h b/deps_install/include/pcl-1.10/pcl/segmentation/random_walker.h new file mode 100755 index 0000000..351e969 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/random_walker.h @@ -0,0 +1,140 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +#include + +namespace pcl +{ + + namespace segmentation + { + + /** \brief Multilabel graph segmentation using random walks. + * + * This is an implementation of the algorithm described in "Random Walks + * for Image Segmentation" by Leo Grady. + * + * Given a weighted undirected graph and a small number of user-defined + * labels this algorithm analytically determines the probability that a + * random walker starting at each unlabeled vertex will first reach one + * of the prelabeled vertices. The unlabeled vertices are then assigned + * to the label for which the greatest probability is calculated. + * + * The input is a BGL graph, a property map that associates a weight to + * each edge of the graph, and a property map that contains initial + * vertex colors (the term "color" is used interchangeably with "label"). + * + * \note The colors of unlabeled vertices should be set to 0, the colors + * of labeled vetices could be any positive numbers. + * + * \note This is the responsibility of the user to make sure that every + * connected component of the graph has at least one colored vertex. If + * the user failed to do so, then the behavior of the algorithm is + * undefined, i.e. it may or may not succeed, and also may or may not + * report failure. + * + * The output of the algorithm (i.e. label assignment) is written back + * to the color map. + * + * \param[in] graph an undirected graph with internal edge weight and + * vertex color property maps + * + * Several overloads of randomWalker() function are provided for + * convenience. + * + * \sa randomWalker(Graph&, EdgeWeightMap, VertexColorMap) + * \sa randomWalker(Graph&, EdgeWeightMap, VertexColorMap, Eigen::Matrix ::value_type, Eigen::Dynamic, Eigen::Dynamic>&, std::map::value_type, std::size_t>&) + * + * \author Sergey Alexandrov + * \ingroup segmentation + */ + + template bool + randomWalker (Graph& graph); + + /** \brief Multilabel graph segmentation using random walks. + * + * This is an overloaded function provided for convenience. See the + * documentation for randomWalker(). + * + * \param[in] graph an undirected graph + * \param[in] weights an external edge weight property map + * \param[in,out] colors an external vertex color property map + * + * \author Sergey Alexandrov + * \ingroup segmentation + */ + template bool + randomWalker (Graph& graph, + EdgeWeightMap weights, + VertexColorMap colors); + + /** \brief Multilabel graph segmentation using random walks. + * + * This is an overloaded function provided for convenience. See the + * documentation for randomWalker(). + * + * \param[in] graph an undirected graph + * \param[in] weights an external edge weight property map + * \param[in,out] colors an external vertex color property map + * \param[out] potentials a matrix with calculated probabilities, + * where rows correspond to vertices, and columns + * correspond to colors + * \param[out] colors_to_columns_map a mapping between colors and + * columns in \a potentials matrix + * + * \author Sergey Alexandrov + * \ingroup segmentation + */ + template bool + randomWalker (Graph& graph, + EdgeWeightMap weights, + VertexColorMap colors, + Eigen::Matrix::value_type, Eigen::Dynamic, Eigen::Dynamic>& potentials, + std::map::value_type, std::size_t>& colors_to_columns_map); + + } + +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/region_3d.h b/deps_install/include/pcl-1.10/pcl/segmentation/region_3d.h new file mode 100755 index 0000000..99a7439 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/region_3d.h @@ -0,0 +1,123 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include + +#include + +namespace pcl +{ + /** \brief Region3D represents summary statistics of a 3D collection of points. + * \author Alex Trevor + */ + template + class Region3D + { + public: + /** \brief Empty constructor for Region3D. */ + Region3D () : centroid_ (Eigen::Vector3f::Zero ()), covariance_ (Eigen::Matrix3f::Identity ()), count_ (0) + { + } + + /** \brief Constructor for Region3D. + * \param[in] centroid The centroid of the region. + * \param[in] covariance The covariance of the region. + * \param[in] count The number of points in the region. + */ + Region3D (Eigen::Vector3f& centroid, Eigen::Matrix3f& covariance, unsigned count) + : centroid_ (centroid), covariance_ (covariance), count_ (count) + { + } + + /** \brief Destructor. */ + virtual ~Region3D () {} + + /** \brief Get the centroid of the region. */ + inline Eigen::Vector3f + getCentroid () const + { + return (centroid_); + } + + /** \brief Get the covariance of the region. */ + inline Eigen::Matrix3f + getCovariance () const + { + return (covariance_); + } + + /** \brief Get the number of points in the region. */ + unsigned + getCount () const + { + return (count_); + } + + /** \brief Get the curvature of the region. */ + float + getCurvature () const + { + return (curvature_); + } + + /** \brief Set the curvature of the region. */ + void + setCurvature (float curvature) + { + curvature_ = curvature; + } + + protected: + /** \brief The centroid of the region. */ + Eigen::Vector3f centroid_; + + /** \brief The covariance of the region. */ + Eigen::Matrix3f covariance_; + + /** \brief The number of points in the region. */ + unsigned count_; + + /** \brief The mean curvature of the region. */ + float curvature_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/region_growing.h b/deps_install/include/pcl-1.10/pcl/segmentation/region_growing.h new file mode 100755 index 0000000..c61e2f3 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/region_growing.h @@ -0,0 +1,349 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : mine_all_mine@bk.ru + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief + * Implements the well known Region Growing algorithm used for segmentation. + * Description can be found in the article + * "Segmentation of point clouds using smoothness constraint" + * by T. Rabbania, F. A. van den Heuvelb, G. Vosselmanc. + * In addition to residual test, the possibility to test curvature is added. + */ + template + class PCL_EXPORTS RegionGrowing : public pcl::PCLBase + { + public: + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + using Normal = pcl::PointCloud; + using NormalPtr = typename Normal::Ptr; + using PointCloud = pcl::PointCloud; + + using PCLBase ::input_; + using PCLBase ::indices_; + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + + public: + + /** \brief Constructor that sets default values for member variables. */ + RegionGrowing (); + + /** \brief This destructor destroys the cloud, normals and search method used for + * finding KNN. In other words it frees memory. + */ + + ~RegionGrowing (); + + /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */ + int + getMinClusterSize (); + + /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. */ + void + setMinClusterSize (int min_cluster_size); + + /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */ + int + getMaxClusterSize (); + + /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. */ + void + setMaxClusterSize (int max_cluster_size); + + /** \brief Returns the flag value. This flag signalizes which mode of algorithm will be used. + * If it is set to true than it will work as said in the article. This means that + * it will be testing the angle between normal of the current point and it's neighbours normal. + * Otherwise, it will be testing the angle between normal of the current point + * and normal of the initial point that was chosen for growing new segment. + */ + bool + getSmoothModeFlag () const; + + /** \brief This function allows to turn on/off the smoothness constraint. + * \param[in] value new mode value, if set to true then the smooth version will be used. + */ + void + setSmoothModeFlag (bool value); + + /** \brief Returns the flag that signalize if the curvature test is turned on/off. */ + bool + getCurvatureTestFlag () const; + + /** \brief Allows to turn on/off the curvature test. Note that at least one test + * (residual or curvature) must be turned on. If you are turning curvature test off + * then residual test will be turned on automatically. + * \param[in] value new value for curvature test. If set to true then the test will be turned on + */ + virtual void + setCurvatureTestFlag (bool value); + + /** \brief Returns the flag that signalize if the residual test is turned on/off. */ + bool + getResidualTestFlag () const; + + /** \brief + * Allows to turn on/off the residual test. Note that at least one test + * (residual or curvature) must be turned on. If you are turning residual test off + * then curvature test will be turned on automatically. + * \param[in] value new value for residual test. If set to true then the test will be turned on + */ + virtual void + setResidualTestFlag (bool value); + + /** \brief Returns smoothness threshold. */ + float + getSmoothnessThreshold () const; + + /** \brief Allows to set smoothness threshold used for testing the points. + * \param[in] theta new threshold value for the angle between normals + */ + void + setSmoothnessThreshold (float theta); + + /** \brief Returns residual threshold. */ + float + getResidualThreshold () const; + + /** \brief Allows to set residual threshold used for testing the points. + * \param[in] residual new threshold value for residual testing + */ + void + setResidualThreshold (float residual); + + /** \brief Returns curvature threshold. */ + float + getCurvatureThreshold () const; + + /** \brief Allows to set curvature threshold used for testing the points. + * \param[in] curvature new threshold value for curvature testing + */ + void + setCurvatureThreshold (float curvature); + + /** \brief Returns the number of nearest neighbours used for KNN. */ + unsigned int + getNumberOfNeighbours () const; + + /** \brief Allows to set the number of neighbours. For more information check the article. + * \param[in] neighbour_number number of neighbours to use + */ + void + setNumberOfNeighbours (unsigned int neighbour_number); + + /** \brief Returns the pointer to the search method that is used for KNN. */ + KdTreePtr + getSearchMethod () const; + + /** \brief Allows to set search method that will be used for finding KNN. + * \param[in] tree pointer to a KdTree + */ + void + setSearchMethod (const KdTreePtr& tree); + + /** \brief Returns normals. */ + NormalPtr + getInputNormals () const; + + /** \brief This method sets the normals. They are needed for the algorithm, so if + * no normals will be set, the algorithm would not be able to segment the points. + * \param[in] norm normals that will be used in the algorithm + */ + void + setInputNormals (const NormalPtr& norm); + + /** \brief This method launches the segmentation algorithm and returns the clusters that were + * obtained during the segmentation. + * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices. + */ + virtual void + extract (std::vector & clusters); + + /** \brief For a given point this function builds a segment to which it belongs and returns this segment. + * \param[in] index index of the initial point which will be the seed for growing a segment. + * \param[out] cluster cluster to which the point belongs. + */ + virtual void + getSegmentFromPoint (int index, pcl::PointIndices& cluster); + + /** \brief If the cloud was successfully segmented, then function + * returns colored cloud. Otherwise it returns an empty pointer. + * Points that belong to the same segment have the same color. + * But this function doesn't guarantee that different segments will have different + * color(it all depends on RNG). Points that were not listed in the indices array will have red color. + */ + pcl::PointCloud::Ptr + getColoredCloud (); + + /** \brief If the cloud was successfully segmented, then function + * returns colored cloud. Otherwise it returns an empty pointer. + * Points that belong to the same segment have the same color. + * But this function doesn't guarantee that different segments will have different + * color(it all depends on RNG). Points that were not listed in the indices array will have red color. + */ + pcl::PointCloud::Ptr + getColoredCloudRGBA (); + + protected: + + /** \brief This method simply checks if it is possible to execute the segmentation algorithm with + * the current settings. If it is possible then it returns true. + */ + virtual bool + prepareForSegmentation (); + + /** \brief This method finds KNN for each point and saves them to the array + * because the algorithm needs to find KNN a few times. + */ + virtual void + findPointNeighbours (); + + /** \brief This function implements the algorithm described in the article + * "Segmentation of point clouds using smoothness constraint" + * by T. Rabbania, F. A. van den Heuvelb, G. Vosselmanc. + */ + void + applySmoothRegionGrowingAlgorithm (); + + /** \brief This method grows a segment for the given seed point. And returns the number of its points. + * \param[in] initial_seed index of the point that will serve as the seed point + * \param[in] segment_number indicates which number this segment will have + */ + int + growRegion (int initial_seed, int segment_number); + + /** \brief This function is checking if the point with index 'nghbr' belongs to the segment. + * If so, then it returns true. It also checks if this point can serve as the seed. + * \param[in] initial_seed index of the initial point that was passed to the growRegion() function + * \param[in] point index of the current seed point + * \param[in] nghbr index of the point that is neighbour of the current seed + * \param[out] is_a_seed this value is set to true if the point with index 'nghbr' can serve as the seed + */ + virtual bool + validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const; + + /** \brief This function simply assembles the regions from list of point labels. + * Each cluster is an array of point indices. + */ + void + assembleRegions (); + + protected: + + /** \brief Stores the minimum number of points that a cluster needs to contain in order to be considered valid. */ + int min_pts_per_cluster_; + + /** \brief Stores the maximum number of points that a cluster needs to contain in order to be considered valid. */ + int max_pts_per_cluster_; + + /** \brief Flag that signalizes if the smoothness constraint will be used. */ + bool smooth_mode_flag_; + + /** \brief If set to true then curvature test will be done during segmentation. */ + bool curvature_flag_; + + /** \brief If set to true then residual test will be done during segmentation. */ + bool residual_flag_; + + /** \brief Thershold used for testing the smoothness between points. */ + float theta_threshold_; + + /** \brief Thershold used in residual test. */ + float residual_threshold_; + + /** \brief Thershold used in curvature test. */ + float curvature_threshold_; + + /** \brief Number of neighbours to find. */ + unsigned int neighbour_number_; + + /** \brief Serch method that will be used for KNN. */ + KdTreePtr search_; + + /** \brief Contains normals of the points that will be segmented. */ + NormalPtr normals_; + + /** \brief Contains neighbours of each point. */ + std::vector > point_neighbours_; + + /** \brief Point labels that tells to which segment each point belongs. */ + std::vector point_labels_; + + /** \brief If set to true then normal/smoothness test will be done during segmentation. + * It is always set to true for the usual region growing algorithm. It is used for turning on/off the test + * for smoothness in the child class RegionGrowingRGB.*/ + bool normal_flag_; + + /** \brief Tells how much points each segment contains. Used for reserving memory. */ + std::vector num_pts_in_segment_; + + /** \brief After the segmentation it will contain the segments. */ + std::vector clusters_; + + /** \brief Stores the number of segments. */ + int number_of_segments_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief This function is used as a comparator for sorting. */ + inline bool + comparePair (std::pair i, std::pair j) + { + return (i.first < j.first); + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/region_growing_rgb.h b/deps_install/include/pcl-1.10/pcl/segmentation/region_growing_rgb.h new file mode 100755 index 0000000..03f0665 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/region_growing_rgb.h @@ -0,0 +1,283 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Sergey Ushakov + * Email : mine_all_mine@bk.ru + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief + * Implements the well known Region Growing algorithm used for segmentation based on color of points. + * Description can be found in the article + * "Color-based segmentation of point clouds" + * by Qingming Zhan, Yubin Liang, Yinghui Xiao + */ + template + class PCL_EXPORTS RegionGrowingRGB : public RegionGrowing + { + public: + + using RegionGrowing::input_; + using RegionGrowing::indices_; + using RegionGrowing::initCompute; + using RegionGrowing::deinitCompute; + using RegionGrowing::normals_; + using RegionGrowing::normal_flag_; + using RegionGrowing::curvature_flag_; + using RegionGrowing::residual_flag_; + using RegionGrowing::residual_threshold_; + using RegionGrowing::neighbour_number_; + using RegionGrowing::search_; + using RegionGrowing::min_pts_per_cluster_; + using RegionGrowing::max_pts_per_cluster_; + using RegionGrowing::smooth_mode_flag_; + using RegionGrowing::theta_threshold_; + using RegionGrowing::curvature_threshold_; + using RegionGrowing::point_neighbours_; + using RegionGrowing::point_labels_; + using RegionGrowing::num_pts_in_segment_; + using RegionGrowing::clusters_; + using RegionGrowing::number_of_segments_; + using RegionGrowing::applySmoothRegionGrowingAlgorithm; + using RegionGrowing::assembleRegions; + + public: + + /** \brief Constructor that sets default values for member variables. */ + RegionGrowingRGB (); + + /** \brief Destructor that frees memory. */ + + ~RegionGrowingRGB (); + + /** \brief Returns the color threshold value used for testing if points belong to the same region. */ + float + getPointColorThreshold () const; + + /** \brief This method specifies the threshold value for color test between the points. + * This kind of testing is made at the first stage of the algorithm(region growing). + * If the difference between points color is less than threshold value, then they are considered + * to be in the same region. + * \param[in] thresh new threshold value for color test + */ + void + setPointColorThreshold (float thresh); + + /** \brief Returns the color threshold value used for testing if regions can be merged. */ + float + getRegionColorThreshold () const; + + /** \brief This method specifies the threshold value for color test between the regions. + * This kind of testing is made at the second stage of the algorithm(region merging). + * If the difference between segments color is less than threshold value, then they are merged together. + * \param[in] thresh new threshold value for color test + */ + void + setRegionColorThreshold (float thresh); + + /** \brief Returns the distance threshold. If the distance between two points is less or equal to + * distance threshold value, then those points assumed to be neighbouring points. + */ + float + getDistanceThreshold () const; + + /** \brief Allows to set distance threshold. + * \param[in] thresh new threshold value for neighbour test + */ + void + setDistanceThreshold (float thresh); + + /** \brief Returns the number of nearest neighbours used for searching K nearest segments. + * Note that here it refers to the segments(not the points). + */ + unsigned int + getNumberOfRegionNeighbours () const; + + /** \brief This method allows to set the number of neighbours that is used for finding + * neighbouring segments. Neighbouring segments are needed for the merging process. + * \param[in] nghbr_number the number of neighbouring segments to find + */ + void + setNumberOfRegionNeighbours (unsigned int nghbr_number); + + /** \brief Returns the flag that signalize if the smoothness test is turned on/off. */ + bool + getNormalTestFlag () const; + + /** \brief + * Allows to turn on/off the smoothness test. + * \param[in] value new value for normal/smoothness test. If set to true then the test will be turned on + */ + void + setNormalTestFlag (bool value); + + /** \brief Allows to turn on/off the curvature test. + * \param[in] value new value for curvature test. If set to true then the test will be turned on + */ + void + setCurvatureTestFlag (bool value) override; + + /** \brief + * Allows to turn on/off the residual test. + * \param[in] value new value for residual test. If set to true then the test will be turned on + */ + void + setResidualTestFlag (bool value) override; + + /** \brief This method launches the segmentation algorithm and returns the clusters that were + * obtained during the segmentation. + * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices. + */ + void + extract (std::vector & clusters) override; + + /** \brief For a given point this function builds a segment to which it belongs and returns this segment. + * \param[in] index index of the initial point which will be the seed for growing a segment. + * \param cluster + */ + void + getSegmentFromPoint (int index, pcl::PointIndices& cluster) override; + + protected: + + /** \brief This method simply checks if it is possible to execute the segmentation algorithm with + * the current settings. If it is possible then it returns true. + */ + bool + prepareForSegmentation () override; + + /** \brief This method finds KNN for each point and saves them to the array + * because the algorithm needs to find KNN a few times. + */ + void + findPointNeighbours () override; + + /** \brief This method simply calls the findRegionsKNN for each segment and + * saves the results for later use. + */ + void + findSegmentNeighbours (); + + /** \brief This method finds K nearest neighbours of the given segment. + * \param[in] index index of the segment for which neighbours will be found + * \param[in] nghbr_number the number of neighbours to find + * \param[out] nghbrs the array of indices of the neighbours that were found + * \param[out] dist the array of distances to the corresponding neighbours + */ + void + findRegionsKNN (int index, int nghbr_number, std::vector& nghbrs, std::vector& dist); + + /** \brief This function implements the merging algorithm described in the article + * "Color-based segmentation of point clouds" + * by Qingming Zhan, Yubin Liang, Yinghui Xiao + */ + void + applyRegionMergingAlgorithm (); + + /** \brief This method calculates the colorimetrical difference between two points. + * In this case it simply returns the euclidean distance between two colors. + * \param[in] first_color the color of the first point + * \param[in] second_color the color of the second point + */ + float + calculateColorimetricalDifference (std::vector& first_color, std::vector& second_color) const; + + /** \brief This method assembles the array containing neighbours of each homogeneous region. + * Homogeneous region is the union of some segments. This array is used when the regions + * with a few points need to be merged with the neighbouring region. + * \param[out] neighbours_out vector of lists of neighbours for every homogeneous region + * \param[in] regions_in vector of lists, each list contains indices of segments that belong + * to the corresponding homogeneous region. + */ + void + findRegionNeighbours (std::vector< std::vector< std::pair > >& neighbours_out, std::vector< std::vector >& regions_in); + + /** \brief This function simply assembles the regions from list of point labels. + * \param[in] num_pts_in_region for each final region it stores the corresponding number of points in it + * \param[in] num_regions number of regions to assemble + */ + void + assembleRegions (std::vector& num_pts_in_region, int num_regions); + + /** \brief This function is checking if the point with index 'nghbr' belongs to the segment. + * If so, then it returns true. It also checks if this point can serve as the seed. + * \param[in] initial_seed index of the initial point that was passed to the growRegion() function + * \param[in] point index of the current seed point + * \param[in] nghbr index of the point that is neighbour of the current seed + * \param[out] is_a_seed this value is set to true if the point with index 'nghbr' can serve as the seed + */ + bool + validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const override; + + protected: + + /** \brief Thershold used in color test for points. */ + float color_p2p_threshold_; + + /** \brief Thershold used in color test for regions. */ + float color_r2r_threshold_; + + /** \brief Threshold that tells which points we need to assume neighbouring. */ + float distance_threshold_; + + /** \brief Number of neighbouring segments to find. */ + unsigned int region_neighbour_number_; + + /** \brief Stores distances for the point neighbours from point_neighbours_ */ + std::vector< std::vector > point_distances_; + + /** \brief Stores the neighboures for the corresponding segments. */ + std::vector< std::vector > segment_neighbours_; + + /** \brief Stores distances for the segment neighbours from segment_neighbours_ */ + std::vector< std::vector > segment_distances_; + + /** \brief Stores new indices for segments that were obtained at the region growing stage. */ + std::vector segment_labels_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/rgb_plane_coefficient_comparator.h b/deps_install/include/pcl-1.10/pcl/segmentation/rgb_plane_coefficient_comparator.h new file mode 100755 index 0000000..51d6060 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/rgb_plane_coefficient_comparator.h @@ -0,0 +1,132 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + /** \brief RGBPlaneCoefficientComparator is a Comparator that operates on plane coefficients, + * for use in planar segmentation. Also takes into account RGB, so we can segmented different colored co-planar regions. + * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. + * + * \author Alex Trevor + */ + template + class RGBPlaneCoefficientComparator: public PlaneCoefficientComparator + { + public: + using PointCloud = typename Comparator::PointCloud; + using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using pcl::Comparator::input_; + using pcl::PlaneCoefficientComparator::normals_; + using pcl::PlaneCoefficientComparator::angular_threshold_; + using pcl::PlaneCoefficientComparator::distance_threshold_; + + /** \brief Empty constructor for RGBPlaneCoefficientComparator. */ + RGBPlaneCoefficientComparator () + : color_threshold_ (50.0f) + { + } + + /** \brief Constructor for RGBPlaneCoefficientComparator. + * \param[in] plane_coeff_d a reference to a vector of d coefficients of plane equations. Must be the same size as the input cloud and input normals. a, b, and c coefficients are in the input normals. + */ + RGBPlaneCoefficientComparator (shared_ptr >& plane_coeff_d) + : PlaneCoefficientComparator (plane_coeff_d), color_threshold_ (50.0f) + { + } + + /** \brief Destructor for RGBPlaneCoefficientComparator. */ + + ~RGBPlaneCoefficientComparator () + { + } + + /** \brief Set the tolerance in color space between neighboring points, to be considered part of the same plane. + * \param[in] color_threshold The distance in color space + */ + inline void + setColorThreshold (float color_threshold) + { + color_threshold_ = color_threshold * color_threshold; + } + + /** \brief Get the color threshold between neighboring points, to be considered part of the same plane. */ + inline float + getColorThreshold () const + { + return (color_threshold_); + } + + /** \brief Compare two neighboring points, by using normal information, euclidean distance, and color information. + * \param[in] idx1 The index of the first point. + * \param[in] idx2 The index of the second point. + */ + bool + compare (int idx1, int idx2) const override + { + float dx = input_->points[idx1].x - input_->points[idx2].x; + float dy = input_->points[idx1].y - input_->points[idx2].y; + float dz = input_->points[idx1].z - input_->points[idx2].z; + float dist = std::sqrt (dx*dx + dy*dy + dz*dz); + int dr = input_->points[idx1].r - input_->points[idx2].r; + int dg = input_->points[idx1].g - input_->points[idx2].g; + int db = input_->points[idx1].b - input_->points[idx2].b; + //Note: This is not the best metric for color comparisons, we should probably use HSV space. + float color_dist = static_cast (dr*dr + dg*dg + db*db); + return ( (dist < distance_threshold_) + && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) + && (color_dist < color_threshold_)); + } + + protected: + float color_threshold_; + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/sac_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/sac_segmentation.h new file mode 100755 index 0000000..754f7af --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/sac_segmentation.h @@ -0,0 +1,441 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +// Sample Consensus methods +#include +#include +// Sample Consensus models +#include +#include + +#include + +namespace pcl +{ + /** \brief @b SACSegmentation represents the Nodelet segmentation class for + * Sample Consensus methods and models, in the sense that it just creates a + * Nodelet wrapper for generic-purpose SAC-based segmentation. + * \author Radu Bogdan Rusu + * \ingroup segmentation + */ + template + class SACSegmentation : public PCLBase + { + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + using PCLBase::input_; + using PCLBase::indices_; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using SearchPtr = typename pcl::search::Search::Ptr; + + using SampleConsensusPtr = typename SampleConsensus::Ptr; + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + + /** \brief Empty constructor. + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SACSegmentation (bool random = false) + : model_ () + , sac_ () + , model_type_ (-1) + , method_type_ (0) + , threshold_ (0) + , optimize_coefficients_ (true) + , radius_min_ (-std::numeric_limits::max ()) + , radius_max_ (std::numeric_limits::max ()) + , samples_radius_ (0.0) + , samples_radius_search_ () + , eps_angle_ (0.0) + , axis_ (Eigen::Vector3f::Zero ()) + , max_iterations_ (50) + , threads_ (-1) + , probability_ (0.99) + , random_ (random) + { + } + + /** \brief Empty destructor. */ + ~SACSegmentation () { /*srv_.reset ();*/ }; + + /** \brief The type of model to use (user given parameter). + * \param[in] model the model type (check \a model_types.h) + */ + inline void + setModelType (int model) { model_type_ = model; } + + /** \brief Get the type of SAC model used. */ + inline int + getModelType () const { return (model_type_); } + + /** \brief Get a pointer to the SAC method used. */ + inline SampleConsensusPtr + getMethod () const { return (sac_); } + + /** \brief Get a pointer to the SAC model used. */ + inline SampleConsensusModelPtr + getModel () const { return (model_); } + + /** \brief The type of sample consensus method to use (user given parameter). + * \param[in] method the method type (check \a method_types.h) + */ + inline void + setMethodType (int method) { method_type_ = method; } + + /** \brief Get the type of sample consensus method used. */ + inline int + getMethodType () const { return (method_type_); } + + /** \brief Distance to the model threshold (user given parameter). + * \param[in] threshold the distance threshold to use + */ + inline void + setDistanceThreshold (double threshold) { threshold_ = threshold; } + + /** \brief Get the distance to the model threshold. */ + inline double + getDistanceThreshold () const { return (threshold_); } + + /** \brief Set the maximum number of iterations before giving up. + * \param[in] max_iterations the maximum number of iterations the sample consensus method will run + */ + inline void + setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; } + + /** \brief Get maximum number of iterations before giving up. */ + inline int + getMaxIterations () const { return (max_iterations_); } + + /** \brief Set the probability of choosing at least one sample free from outliers. + * \param[in] probability the model fitting probability + */ + inline void + setProbability (double probability) { probability_ = probability; } + + /** \brief Get the probability of choosing at least one sample free from outliers. */ + inline double + getProbability () const { return (probability_); } + + /** \brief Set the number of threads to use or turn off parallelization. + * \param[in] nr_threads the number of hardware threads to use (0 sets the value automatically, a negative number turns parallelization off) + * \note Not all SAC methods have a parallel implementation. Some will ignore this setting. + */ + inline void + setNumberOfThreads (const int nr_threads = -1) { threads_ = nr_threads; } + + /** \brief Set to true if a coefficient refinement is required. + * \param[in] optimize true for enabling model coefficient refinement, false otherwise + */ + inline void + setOptimizeCoefficients (bool optimize) { optimize_coefficients_ = optimize; } + + /** \brief Get the coefficient refinement internal flag. */ + inline bool + getOptimizeCoefficients () const { return (optimize_coefficients_); } + + /** \brief Set the minimum and maximum allowable radius limits for the model (applicable to models that estimate + * a radius) + * \param[in] min_radius the minimum radius model + * \param[in] max_radius the maximum radius model + */ + inline void + setRadiusLimits (const double &min_radius, const double &max_radius) + { + radius_min_ = min_radius; + radius_max_ = max_radius; + } + + /** \brief Get the minimum and maximum allowable radius limits for the model as set by the user. + * \param[out] min_radius the resultant minimum radius model + * \param[out] max_radius the resultant maximum radius model + */ + inline void + getRadiusLimits (double &min_radius, double &max_radius) + { + min_radius = radius_min_; + max_radius = radius_max_; + } + + /** \brief Set the maximum distance allowed when drawing random samples + * \param[in] radius the maximum distance (L2 norm) + * \param search + */ + inline void + setSamplesMaxDist (const double &radius, SearchPtr search) + { + samples_radius_ = radius; + samples_radius_search_ = search; + } + + /** \brief Get maximum distance allowed when drawing random samples + * + * \param[out] radius the maximum distance (L2 norm) + */ + inline void + getSamplesMaxDist (double &radius) + { + radius = samples_radius_; + } + + /** \brief Set the axis along which we need to search for a model perpendicular to. + * \param[in] ax the axis along which we need to search for a model perpendicular to + */ + inline void + setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } + + /** \brief Get the axis along which we need to search for a model perpendicular to. */ + inline Eigen::Vector3f + getAxis () const { return (axis_); } + + /** \brief Set the angle epsilon (delta) threshold. + * \param[in] ea the maximum allowed difference between the model normal and the given axis in radians. + */ + inline void + setEpsAngle (double ea) { eps_angle_ = ea; } + + /** \brief Get the epsilon (delta) model angle threshold in radians. */ + inline double + getEpsAngle () const { return (eps_angle_); } + + /** \brief Base method for segmentation of a model in a PointCloud given by + * \param[out] inliers the resultant point indices that support the model found (inliers) + * \param[out] model_coefficients the resultant model coefficients + */ + virtual void + segment (PointIndices &inliers, ModelCoefficients &model_coefficients); + + protected: + /** \brief Initialize the Sample Consensus model and set its parameters. + * \param[in] model_type the type of SAC model that is to be used + */ + virtual bool + initSACModel (const int model_type); + + /** \brief Initialize the Sample Consensus method and set its parameters. + * \param[in] method_type the type of SAC method to be used + */ + virtual void + initSAC (const int method_type); + + /** \brief The model that needs to be segmented. */ + SampleConsensusModelPtr model_; + + /** \brief The sample consensus segmentation method. */ + SampleConsensusPtr sac_; + + /** \brief The type of model to use (user given parameter). */ + int model_type_; + + /** \brief The type of sample consensus method to use (user given parameter). */ + int method_type_; + + /** \brief Distance to the model threshold (user given parameter). */ + double threshold_; + + /** \brief Set to true if a coefficient refinement is required. */ + bool optimize_coefficients_; + + /** \brief The minimum and maximum radius limits for the model. Applicable to all models that estimate a radius. */ + double radius_min_, radius_max_; + + /** \brief The maximum distance of subsequent samples from the first (radius search) */ + double samples_radius_; + + /** \brief The search object for picking subsequent samples using radius search */ + SearchPtr samples_radius_search_; + + /** \brief The maximum allowed difference between the model normal and the given axis. */ + double eps_angle_; + + /** \brief The axis along which we need to search for a model perpendicular to. */ + Eigen::Vector3f axis_; + + /** \brief Maximum number of iterations before giving up (user given parameter). */ + int max_iterations_; + + /** \brief The number of threads the scheduler should use, or a negative number if no parallelization is wanted. */ + int threads_; + + /** \brief Desired probability of choosing at least one sample free from outliers (user given parameter). */ + double probability_; + + /** \brief Set to true if we need a random seed. */ + bool random_; + + /** \brief Class get name method. */ + virtual std::string + getClassName () const { return ("SACSegmentation"); } + }; + + /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and + * models that require the use of surface normals for estimation. + * \ingroup segmentation + */ + template + class SACSegmentationFromNormals: public SACSegmentation + { + using SACSegmentation::model_; + using SACSegmentation::model_type_; + using SACSegmentation::radius_min_; + using SACSegmentation::radius_max_; + using SACSegmentation::eps_angle_; + using SACSegmentation::axis_; + using SACSegmentation::random_; + + public: + using PCLBase::input_; + using PCLBase::indices_; + + using PointCloud = typename SACSegmentation::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointCloudN = pcl::PointCloud; + using PointCloudNPtr = typename PointCloudN::Ptr; + using PointCloudNConstPtr = typename PointCloudN::ConstPtr; + + using SampleConsensusPtr = typename SampleConsensus::Ptr; + using SampleConsensusModelPtr = typename SampleConsensusModel::Ptr; + using SampleConsensusModelFromNormalsPtr = typename SampleConsensusModelFromNormals::Ptr; + + /** \brief Empty constructor. + * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) + */ + SACSegmentationFromNormals (bool random = false) + : SACSegmentation (random) + , normals_ () + , distance_weight_ (0.1) + , distance_from_origin_ (0) + , min_angle_ (0.0) + , max_angle_ (M_PI_2) + {}; + + /** \brief Provide a pointer to the input dataset that contains the point normals of + * the XYZ dataset. + * \param[in] normals the const boost shared pointer to a PointCloud message + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; } + + /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */ + inline PointCloudNConstPtr + getInputNormals () const { return (normals_); } + + /** \brief Set the relative weight (between 0 and 1) to give to the angular + * distance (0 to pi/2) between point normals and the plane normal. + * \param[in] distance_weight the distance/angular weight + */ + inline void + setNormalDistanceWeight (double distance_weight) { distance_weight_ = distance_weight; } + + /** \brief Get the relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point + * normals and the plane normal. */ + inline double + getNormalDistanceWeight () const { return (distance_weight_); } + + /** \brief Set the minimum opning angle for a cone model. + * \param min_angle the opening angle which we need minimum to validate a cone model. + * \param max_angle the opening angle which we need maximum to validate a cone model. + */ + inline void + setMinMaxOpeningAngle (const double &min_angle, const double &max_angle) + { + min_angle_ = min_angle; + max_angle_ = max_angle; + } + + /** \brief Get the opening angle which we need minimum to validate a cone model. */ + inline void + getMinMaxOpeningAngle (double &min_angle, double &max_angle) + { + min_angle = min_angle_; + max_angle = max_angle_; + } + + /** \brief Set the distance we expect a plane model to be from the origin + * \param[in] d distance from the template plane model to the origin + */ + inline void + setDistanceFromOrigin (const double d) { distance_from_origin_ = d; } + + /** \brief Get the distance of a plane model from the origin. */ + inline double + getDistanceFromOrigin () const { return (distance_from_origin_); } + + protected: + /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */ + PointCloudNConstPtr normals_; + + /** \brief The relative weight (between 0 and 1) to give to the angular + * distance (0 to pi/2) between point normals and the plane normal. + */ + double distance_weight_; + + /** \brief The distance from the template plane to the origin. */ + double distance_from_origin_; + + /** \brief The minimum and maximum allowed opening angle of valid cone model. */ + double min_angle_; + double max_angle_; + + /** \brief Initialize the Sample Consensus model and set its parameters. + * \param[in] model_type the type of SAC model that is to be used + */ + bool + initSACModel (const int model_type) override; + + /** \brief Class get name method. */ + std::string + getClassName () const override { return ("SACSegmentationFromNormals"); } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/seeded_hue_segmentation.h b/deps_install/include/pcl-1.10/pcl/segmentation/seeded_hue_segmentation.h new file mode 100755 index 0000000..e8cfa42 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/seeded_hue_segmentation.h @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $id: $ + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param[in] cloud the point cloud message + * \param[in] tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud + * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param[in] indices_in the cluster containing the seed point indices (as a vector of PointIndices) + * \param[out] indices_out + * \param[in] delta_hue + * \todo look how to make this templated! + * \ingroup segmentation + */ + void + seededHueSegmentation (const PointCloud &cloud, + const search::Search::Ptr &tree, + float tolerance, + PointIndices &indices_in, + PointIndices &indices_out, + float delta_hue = 0.0); + + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param[in] cloud the point cloud message + * \param[in] tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud + * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param[in] indices_in the cluster containing the seed point indices (as a vector of PointIndices) + * \param[out] indices_out + * \param[in] delta_hue + * \todo look how to make this templated! + * \ingroup segmentation + */ + void + seededHueSegmentation (const PointCloud &cloud, + const search::Search::Ptr &tree, + float tolerance, + PointIndices &indices_in, + PointIndices &indices_out, + float delta_hue = 0.0); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief SeededHueSegmentation + * \author Koen Buys + * \ingroup segmentation + */ + class SeededHueSegmentation: public PCLBase + { + using BasePCLBase = PCLBase; + + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = PointCloud::Ptr; + using PointCloudConstPtr = PointCloud::ConstPtr; + + using KdTree = pcl::search::Search; + using KdTreePtr = pcl::search::Search::Ptr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Empty constructor. */ + SeededHueSegmentation () : cluster_tolerance_ (0), delta_hue_ (0.0) + {}; + + /** \brief Provide a pointer to the search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () const { return (tree_); } + + /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space + * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space + */ + inline void + setClusterTolerance (double tolerance) { cluster_tolerance_ = tolerance; } + + /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */ + inline double + getClusterTolerance () const { return (cluster_tolerance_); } + + /** \brief Set the tollerance on the hue + * \param[in] delta_hue the new delta hue + */ + inline void + setDeltaHue (float delta_hue) { delta_hue_ = delta_hue; } + + /** \brief Get the tolerance on the hue */ + inline float + getDeltaHue () const { return (delta_hue_); } + + /** \brief Cluster extraction in a PointCloud given by + * \param[in] indices_in + * \param[out] indices_out + */ + void + segment (PointIndices &indices_in, PointIndices &indices_out); + + protected: + // Members derived from the base class + using BasePCLBase::input_; + using BasePCLBase::indices_; + using BasePCLBase::initCompute; + using BasePCLBase::deinitCompute; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */ + double cluster_tolerance_; + + /** \brief The allowed difference on the hue*/ + float delta_hue_; + + /** \brief Class getName method. */ + virtual std::string getClassName () const { return ("seededHueSegmentation"); } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/segment_differences.h b/deps_install/include/pcl-1.10/pcl/segmentation/segment_differences.h new file mode 100755 index 0000000..ecd181b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/segment_differences.h @@ -0,0 +1,172 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Obtain the difference between two aligned point clouds as another point cloud, given a distance threshold. + * \param src the input point cloud source + * \param threshold the distance threshold (tolerance) for point correspondences. (e.g., check if f a point p1 from + * src has a correspondence > threshold than a point p2 from tgt) + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching built over the target cloud + * \param output the resultant output point cloud difference + * \ingroup segmentation + */ + template + void getPointCloudDifference ( + const pcl::PointCloud &src, + double threshold, + const typename pcl::search::Search::Ptr &tree, + pcl::PointCloud &output); + + template + PCL_DEPRECATED("tgt parameter is not used; it is deprecated and will be removed in future releases") + inline void getPointCloudDifference ( + const pcl::PointCloud &src, + const pcl::PointCloud & /* tgt */, + double threshold, + const typename pcl::search::Search::Ptr &tree, + pcl::PointCloud &output) + { + getPointCloudDifference (src, threshold, tree, output); + } + + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b SegmentDifferences obtains the difference between two spatially + * aligned point clouds and returns the difference between them for a maximum + * given distance threshold. + * \author Radu Bogdan Rusu + * \ingroup segmentation + */ + template + class SegmentDifferences: public PCLBase + { + using BasePCLBase = PCLBase; + + public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + + using PointIndicesPtr = PointIndices::Ptr; + using PointIndicesConstPtr = PointIndices::ConstPtr; + + /** \brief Empty constructor. */ + SegmentDifferences () : + tree_ (), target_ (), distance_threshold_ (0) + {}; + + /** \brief Provide a pointer to the target dataset against which we + * compare the input cloud given in setInputCloud + * + * \param cloud the target PointCloud dataset + */ + inline void + setTargetCloud (const PointCloudConstPtr &cloud) { target_ = cloud; } + + /** \brief Get a pointer to the input target point cloud dataset. */ + inline PointCloudConstPtr const + getTargetCloud () { return (target_); } + + /** \brief Provide a pointer to the search object. + * \param tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () { return (tree_); } + + /** \brief Set the maximum distance tolerance (squared) between corresponding + * points in the two input datasets. + * + * \param sqr_threshold the squared distance tolerance as a measure in L2 Euclidean space + */ + inline void + setDistanceThreshold (double sqr_threshold) { distance_threshold_ = sqr_threshold; } + + /** \brief Get the squared distance tolerance between corresponding points as a + * measure in the L2 Euclidean space. + */ + inline double + getDistanceThreshold () { return (distance_threshold_); } + + /** \brief Segment differences between two input point clouds. + * \param output the resultant difference between the two point clouds as a PointCloud + */ + void + segment (PointCloud &output); + + protected: + // Members derived from the base class + using BasePCLBase::input_; + using BasePCLBase::indices_; + using BasePCLBase::initCompute; + using BasePCLBase::deinitCompute; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The input target point cloud dataset. */ + PointCloudConstPtr target_; + + /** \brief The distance tolerance (squared) as a measure in the L2 + * Euclidean space between corresponding points. + */ + double distance_threshold_; + + /** \brief Class getName method. */ + virtual std::string + getClassName () const { return ("SegmentDifferences"); } + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/supervoxel_clustering.h b/deps_install/include/pcl-1.10/pcl/segmentation/supervoxel_clustering.h new file mode 100755 index 0000000..5ce354c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/supervoxel_clustering.h @@ -0,0 +1,537 @@ + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : jpapon@gmail.com + * Email : jpapon@gmail.com + * + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +//DEBUG TODO REMOVE +#include + + +namespace pcl +{ + /** \brief Supervoxel container class - stores a cluster extracted using supervoxel clustering + */ + template + class Supervoxel + { + public: + Supervoxel () : + voxels_ (new pcl::PointCloud ()), + normals_ (new pcl::PointCloud ()) + { } + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Gets the centroid of the supervoxel + * \param[out] centroid_arg centroid of the supervoxel + */ + void + getCentroidPoint (PointXYZRGBA ¢roid_arg) + { + centroid_arg = centroid_; + } + + /** \brief Gets the point normal for the supervoxel + * \param[out] normal_arg Point normal of the supervoxel + * \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support + */ + void + getCentroidPointNormal (PointNormal &normal_arg) + { + normal_arg.x = centroid_.x; + normal_arg.y = centroid_.y; + normal_arg.z = centroid_.z; + normal_arg.normal_x = normal_.normal_x; + normal_arg.normal_y = normal_.normal_y; + normal_arg.normal_z = normal_.normal_z; + normal_arg.curvature = normal_.curvature; + } + + /** \brief The normal calculated for the voxels contained in the supervoxel */ + pcl::Normal normal_; + /** \brief The centroid of the supervoxel - average voxel */ + pcl::PointXYZRGBA centroid_; + /** \brief A Pointcloud of the voxels in the supervoxel */ + typename pcl::PointCloud::Ptr voxels_; + /** \brief A Pointcloud of the normals for the points in the supervoxel */ + typename pcl::PointCloud::Ptr normals_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief Implements a supervoxel algorithm based on voxel structure, normals, and rgb values + * \note Supervoxels are oversegmented volumetric patches (usually surfaces) + * \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used + * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter + * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds + * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 + * \ingroup segmentation + * \author Jeremie Papon (jpapon@gmail.com) + */ + template + class PCL_EXPORTS SupervoxelClustering : public pcl::PCLBase + { + //Forward declaration of friended helper class + class SupervoxelHelper; + friend class SupervoxelHelper; + public: + /** \brief VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContainer + * \note It stores xyz, rgb, normal, distance, an index, and an owner. + */ + class VoxelData + { + public: + VoxelData (): + xyz_ (0.0f, 0.0f, 0.0f), + rgb_ (0.0f, 0.0f, 0.0f), + normal_ (0.0f, 0.0f, 0.0f, 0.0f), + curvature_ (0.0f), + distance_(0), + idx_(0), + owner_ (nullptr) + {} + + /** \brief Gets the data of in the form of a point + * \param[out] point_arg Will contain the point value of the voxeldata + */ + void + getPoint (PointT &point_arg) const; + + /** \brief Gets the data of in the form of a normal + * \param[out] normal_arg Will contain the normal value of the voxeldata + */ + void + getNormal (Normal &normal_arg) const; + + Eigen::Vector3f xyz_; + Eigen::Vector3f rgb_; + Eigen::Vector4f normal_; + float curvature_; + float distance_; + int idx_; + SupervoxelHelper* owner_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + using LeafContainerT = pcl::octree::OctreePointCloudAdjacencyContainer; + using LeafVectorT = std::vector; + + using PointCloudT = pcl::PointCloud; + using NormalCloudT = pcl::PointCloud; + using OctreeAdjacencyT = pcl::octree::OctreePointCloudAdjacency; + using OctreeSearchT = pcl::octree::OctreePointCloudSearch; + using KdTreeT = pcl::search::KdTree; + using IndicesPtr = pcl::IndicesPtr; + + using PCLBase ::initCompute; + using PCLBase ::deinitCompute; + using PCLBase ::input_; + + using VoxelAdjacencyList = boost::adjacency_list; + using VoxelID = VoxelAdjacencyList::vertex_descriptor; + using EdgeID = VoxelAdjacencyList::edge_descriptor; + + public: + + /** \brief Constructor that sets default values for member variables. + * \param[in] voxel_resolution The resolution (in meters) of voxels used + * \param[in] seed_resolution The average size (in meters) of resulting supervoxels + */ + SupervoxelClustering (float voxel_resolution, float seed_resolution); + + PCL_DEPRECATED("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.") + SupervoxelClustering (float voxel_resolution, float seed_resolution, bool) : SupervoxelClustering (voxel_resolution, seed_resolution) { } + + /** \brief This destructor destroys the cloud, normals and search method used for + * finding neighbors. In other words it frees memory. + */ + + ~SupervoxelClustering (); + + /** \brief Set the resolution of the octree voxels */ + void + setVoxelResolution (float resolution); + + /** \brief Get the resolution of the octree voxels */ + float + getVoxelResolution () const; + + /** \brief Set the resolution of the octree seed voxels */ + void + setSeedResolution (float seed_resolution); + + /** \brief Get the resolution of the octree seed voxels */ + float + getSeedResolution () const; + + /** \brief Set the importance of color for supervoxels */ + void + setColorImportance (float val); + + /** \brief Set the importance of spatial distance for supervoxels */ + void + setSpatialImportance (float val); + + /** \brief Set the importance of scalar normal product for supervoxels */ + void + setNormalImportance (float val); + + /** \brief Set whether or not to use the single camera transform + * \note By default it will be used for organized clouds, but not for unorganized - this parameter will override that behavior + * The single camera transform scales bin size so that it increases exponentially with depth (z dimension). + * This is done to account for the decreasing point density found with depth when using an RGB-D camera. + * Without the transform, beyond a certain depth adjacency of voxels breaks down unless the voxel size is set to a large value. + * Using the transform allows preserving detail up close, while allowing adjacency at distance. + * The specific transform used here is: + * x /= z; y /= z; z = ln(z); + * This transform is applied when calculating the octree bins in OctreePointCloudAdjacency + */ + void + setUseSingleCameraTransform (bool val); + + /** \brief This method launches the segmentation algorithm and returns the supervoxels that were + * obtained during the segmentation. + * \param[out] supervoxel_clusters A map of labels to pointers to supervoxel structures + */ + virtual void + extract (std::map::Ptr > &supervoxel_clusters); + + /** \brief This method sets the cloud to be supervoxelized + * \param[in] cloud The cloud to be supervoxelize + */ + void + setInputCloud (const typename pcl::PointCloud::ConstPtr& cloud) override; + + /** \brief This method sets the normals to be used for supervoxels (should be same size as input cloud) + * \param[in] normal_cloud The input normals + */ + virtual void + setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud); + + /** \brief This method refines the calculated supervoxels - may only be called after extract + * \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient) + * \param[out] supervoxel_clusters The resulting refined supervoxels + */ + virtual void + refineSupervoxels (int num_itr, std::map::Ptr > &supervoxel_clusters); + + //////////////////////////////////////////////////////////// + /** \brief Returns an RGB colorized cloud showing superpixels + * Otherwise it returns an empty pointer. + * Points that belong to the same supervoxel have the same color. + * But this function doesn't guarantee that different segments will have different + * color(it's random). Points that are unlabeled will be black + * \note This will expand the label_colors_ vector so that it can accommodate all labels + */ + PCL_DEPRECATED("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp") + typename pcl::PointCloud::Ptr + getColoredCloud () const + { + return pcl::PointCloud::Ptr (new pcl::PointCloud); + } + + /** \brief Returns a deep copy of the voxel centroid cloud */ + typename pcl::PointCloud::Ptr + getVoxelCentroidCloud () const; + + /** \brief Returns labeled cloud + * Points that belong to the same supervoxel have the same label. + * Labels for segments start from 1, unlabled points have label 0 + */ + typename pcl::PointCloud::Ptr + getLabeledCloud () const; + + /** \brief Returns labeled voxelized cloud + * Points that belong to the same supervoxel have the same label. + * Labels for segments start from 1, unlabled points have label 0 + */ + pcl::PointCloud::Ptr + getLabeledVoxelCloud () const; + + /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels + * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships + */ + void + getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const; + + /** \brief Get a multimap which gives supervoxel adjacency + * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels + */ + void + getSupervoxelAdjacency (std::multimap &label_adjacency) const; + + /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels + * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class + * \returns Cloud of PointNormals of the supervoxels + * + */ + static pcl::PointCloud::Ptr + makeSupervoxelNormalCloud (std::map::Ptr > &supervoxel_clusters); + + /** \brief Returns the current maximum (highest) label */ + int + getMaxLabel () const; + + private: + /** \brief This method simply checks if it is possible to execute the segmentation algorithm with + * the current settings. If it is possible then it returns true. + */ + virtual bool + prepareForSegmentation (); + + /** \brief This selects points to use as initial supervoxel centroids + * \param[out] seed_indices The selected leaf indices + */ + void + selectInitialSupervoxelSeeds (std::vector &seed_indices); + + /** \brief This method creates the internal supervoxel helpers based on the provided seed points + * \param[in] seed_indices Indices of the leaves to use as seeds + */ + void + createSupervoxelHelpers (std::vector &seed_indices); + + /** \brief This performs the superpixel evolution */ + void + expandSupervoxels (int depth); + + /** \brief This sets the data of the voxels in the tree */ + void + computeVoxelData (); + + /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */ + void + reseedSupervoxels (); + + /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */ + void + makeSupervoxels (std::map::Ptr > &supervoxel_clusters); + + /** \brief Stores the resolution used in the octree */ + float resolution_; + + /** \brief Stores the resolution used to seed the superpixels */ + float seed_resolution_; + + /** \brief Distance function used for comparing voxelDatas */ + float + voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const; + + /** \brief Transform function used to normalize voxel density versus distance from camera */ + void + transformFunction (PointT &p); + + /** \brief Contains a KDtree for the voxelized cloud */ + typename pcl::search::KdTree::Ptr voxel_kdtree_; + + /** \brief Octree Adjacency structure with leaves at voxel resolution */ + typename OctreeAdjacencyT::Ptr adjacency_octree_; + + /** \brief Contains the Voxelized centroid Cloud */ + typename PointCloudT::Ptr voxel_centroid_cloud_; + + /** \brief Contains the Voxelized centroid Cloud */ + typename NormalCloudT::ConstPtr input_normals_; + + /** \brief Importance of color in clustering */ + float color_importance_; + /** \brief Importance of distance from seed center in clustering */ + float spatial_importance_; + /** \brief Importance of similarity in normals for clustering */ + float normal_importance_; + + /** \brief Whether or not to use the transform compressing depth in Z + * This is only checked if it has been manually set by the user. + * The default behavior is to use the transform for organized, and not for unorganized. + */ + bool use_single_camera_transform_; + /** \brief Whether to use default transform behavior or not */ + bool use_default_transform_behaviour_; + + /** \brief Internal storage class for supervoxels + * \note Stores pointers to leaves of clustering internal octree, + * \note so should not be used outside of clustering class + */ + class SupervoxelHelper + { + public: + /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves + * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed. + */ + struct compareLeaves + { + bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const + { + const VoxelData& leaf_data_left = left->getData (); + const VoxelData& leaf_data_right = right->getData (); + return leaf_data_left.idx_ < leaf_data_right.idx_; + } + }; + using LeafSetT = std::set; + using iterator = typename LeafSetT::iterator; + using const_iterator = typename LeafSetT::const_iterator; + + SupervoxelHelper (std::uint32_t label, SupervoxelClustering* parent_arg): + label_ (label), + parent_ (parent_arg) + { } + + void + addLeaf (LeafContainerT* leaf_arg); + + void + removeLeaf (LeafContainerT* leaf_arg); + + void + removeAllLeaves (); + + void + expand (); + + void + refineNormals (); + + void + updateCentroid (); + + void + getVoxels (typename pcl::PointCloud::Ptr &voxels) const; + + void + getNormals (typename pcl::PointCloud::Ptr &normals) const; + + using DistFuncPtr = float (SupervoxelClustering::*)(const VoxelData &, const VoxelData &); + + std::uint32_t + getLabel () const + { return label_; } + + Eigen::Vector4f + getNormal () const + { return centroid_.normal_; } + + Eigen::Vector3f + getRGB () const + { return centroid_.rgb_; } + + Eigen::Vector3f + getXYZ () const + { return centroid_.xyz_;} + + void + getXYZ (float &x, float &y, float &z) const + { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; } + + void + getRGB (std::uint32_t &rgba) const + { + rgba = static_cast(centroid_.rgb_[0]) << 16 | + static_cast(centroid_.rgb_[1]) << 8 | + static_cast(centroid_.rgb_[2]); + } + + void + getNormal (pcl::Normal &normal_arg) const + { + normal_arg.normal_x = centroid_.normal_[0]; + normal_arg.normal_y = centroid_.normal_[1]; + normal_arg.normal_z = centroid_.normal_[2]; + normal_arg.curvature = centroid_.curvature_; + } + + void + getNeighborLabels (std::set &neighbor_labels) const; + + VoxelData + getCentroid () const + { return centroid_; } + + std::size_t + size () const { return leaves_.size (); } + private: + //Stores leaves + LeafSetT leaves_; + std::uint32_t label_; + VoxelData centroid_; + SupervoxelClustering* parent_; + public: + //Type VoxelData may have fixed-size Eigen objects inside + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + //Make boost::ptr_list can access the private class SupervoxelHelper +#if BOOST_VERSION >= 107000 + friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering::SupervoxelHelper *) BOOST_NOEXCEPT; +#else + friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering::SupervoxelHelper *); +#endif + + using HelperListT = boost::ptr_list; + HelperListT supervoxel_helpers_; + + //TODO DEBUG REMOVE + StopWatch timer_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/segmentation/unary_classifier.h b/deps_install/include/pcl-1.10/pcl/segmentation/unary_classifier.h new file mode 100755 index 0000000..2aa4bf4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/segmentation/unary_classifier.h @@ -0,0 +1,177 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author : Christian Potthast + * Email : potthast@usc.edu + * + */ + +#pragma once + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace pcl +{ + /** \brief + * + */ + template + class PCL_EXPORTS UnaryClassifier + { + public: + + /** \brief Constructor that sets default values for member variables. */ + UnaryClassifier (); + + /** \brief This destructor destroys the cloud... + * + */ + ~UnaryClassifier (); + + /** \brief This method sets the input cloud. + * \param[in] input_cloud input point cloud + */ + void + setInputCloud (typename pcl::PointCloud::Ptr input_cloud); + + void + train (pcl::PointCloud::Ptr &output); + + void + trainWithLabel (std::vector, Eigen::aligned_allocator > > &output); + + void + segment (pcl::PointCloud::Ptr &out); + + void + queryFeatureDistances (std::vector::Ptr> &trained_features, + pcl::PointCloud::Ptr query_features, + std::vector &indi, + std::vector &dist); + + void + assignLabels (std::vector &indi, + std::vector &dist, + int n_feature_means, + float feature_threshold, + pcl::PointCloud::Ptr out); + + void + setClusterSize (unsigned int k){cluster_size_ = k;}; + + void + setNormalRadiusSearch (float param){normal_radius_search_ = param;}; + + void + setFPFHRadiusSearch (float param){fpfh_radius_search_ = param;}; + + void + setLabelField (bool l){label_field_ = l;}; + + void + setTrainedFeatures (std::vector::Ptr> &features){trained_features_ = features;}; + + void + setFeatureThreshold (float threshold){feature_threshold_ = threshold;}; + + protected: + + void + convertCloud (typename pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out); + + void + convertCloud (typename pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out); + + void + findClusters (typename pcl::PointCloud::Ptr in, + std::vector &cluster_numbers); + + void + getCloudWithLabel (typename pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out, + int label_num); + + void + computeFPFH (pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out, + float normal_radius_search, + float fpfh_radius_search); + + void + kmeansClustering (pcl::PointCloud::Ptr in, + pcl::PointCloud::Ptr out, + int k); + + + + /** \brief Contains the input cloud */ + typename pcl::PointCloud::Ptr input_cloud_; + + bool label_field_; + + unsigned int cluster_size_; + + float normal_radius_search_; + float fpfh_radius_search_; + float feature_threshold_; + + + std::vector::Ptr> trained_features_; + + /** \brief Contains normals of the points that will be segmented. */ + //typename pcl::PointCloud::Ptr normals_; + + /** \brief Stores the cloud that will be segmented. */ + //typename pcl::PointCloud::Ptr cloud_for_segmentation_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/sse.h b/deps_install/include/pcl-1.10/pcl/sse.h new file mode 100755 index 0000000..54a139e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/sse.h @@ -0,0 +1,98 @@ +/* + * Software License Agreement (Simplified BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, Open Perception, Inc. + * Copyright (c) 2012, Piotr Dollar & Ron Appel.[pdollar-at-caltech.edu] + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + *list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + *this list of conditions and the following disclaimer in the documentation + *and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * The views and conclusions contained in the software and documentation are those + * of the authors and should not be interpreted as representing official policies, + * either expressed or implied, of the FreeBSD Project. + * + * Taken from Piotr Dollar's MATLAB Image&Video ToolboxVersion 3.00. + * + */ + +#pragma once + +#if defined(__SSE2__) +#include // SSE2:, SSE3:, SSE4: + +#define RETf inline __m128 +#define RETi inline __m128i + +namespace pcl { + +// set, load and store values +RETf sse_set( const float &x ) { return _mm_set1_ps(x); } +RETf sse_set( float x, float y, float z, float w ) { return _mm_set_ps(x,y,z,w); } +RETi sse_set( const int &x ) { return _mm_set1_epi32(x); } +RETf sse_ld( const float &x ) { return _mm_load_ps(&x); } +RETf sse_ldu( const float &x ) { return _mm_loadu_ps(&x); } +RETf sse_str( float &x, const __m128 y ) { _mm_store_ps(&x,y); return y; } +RETf sse_str1( float &x, const __m128 y ) { _mm_store_ss(&x,y); return y; } +RETf sse_stru( float &x, const __m128 y ) { _mm_storeu_ps(&x,y); return y; } +RETf sse_str( float &x, const float y ) { return sse_str(x,sse_set(y)); } + +// arithmetic operators +RETi sse_add( const __m128i x, const __m128i y ) { return _mm_add_epi32(x,y); } +RETf sse_add( const __m128 x, const __m128 y ) { return _mm_add_ps(x,y); } +RETf sse_add( const __m128 x, const __m128 y, const __m128 z ) { + return sse_add(sse_add(x,y),z); } +RETf sse_add( const __m128 a, const __m128 b, const __m128 c, const __m128 &d ) { + return sse_add(sse_add(sse_add(a,b),c),d); } +RETf sse_sub( const __m128 x, const __m128 y ) { return _mm_sub_ps(x,y); } +RETf sse_mul( const __m128 x, const __m128 y ) { return _mm_mul_ps(x,y); } +RETf sse_mul( const __m128 x, const float y ) { return sse_mul(x,sse_set(y)); } +RETf sse_mul( const float x, const __m128 y ) { return sse_mul(sse_set(x),y); } +RETf sse_inc( __m128 &x, const __m128 y ) { return x = sse_add(x,y); } +RETf sse_inc( float &x, const __m128 y ) { __m128 t=sse_add(sse_ld(x),y); return sse_str(x,t); } +RETf sse_dec( __m128 &x, const __m128 y ) { return x = sse_sub(x,y); } +RETf sse_dec( float &x, const __m128 y ) { __m128 t=sse_sub(sse_ld(x),y); return sse_str(x,t); } +RETf sse_min( const __m128 x, const __m128 y ) { return _mm_min_ps(x,y); } +RETf sse_rcp( const __m128 x ) { return _mm_rcp_ps(x); } +RETf sse_rcpsqrt( const __m128 x ) { return _mm_rsqrt_ps(x); } + +// logical operators +RETf sse_and( const __m128 x, const __m128 y ) { return _mm_and_ps(x,y); } +RETi sse_and( const __m128i x, const __m128i y ) { return _mm_and_si128(x,y); } +RETf sse_andnot( const __m128 x, const __m128 y ) { return _mm_andnot_ps(x,y); } +RETf sse_or( const __m128 x, const __m128 y ) { return _mm_or_ps(x,y); } +RETf sse_xor( const __m128 x, const __m128 y ) { return _mm_xor_ps(x,y); } + +// comparison operators +RETf sse_cmpgt( const __m128 x, const __m128 y ) { return _mm_cmpgt_ps(x,y); } +RETi sse_cmpgt( const __m128i x, const __m128i y ) { return _mm_cmpgt_epi32(x,y); } + +// conversion operators +RETf sse_cvt( const __m128i x ) { return _mm_cvtepi32_ps(x); } +RETi sse_cvt( const __m128 x ) { return _mm_cvttps_epi32(x); } + +} // namespace pcl + +#undef RETf +#undef RETi +#endif /* defined(__SSE2__) */ diff --git a/deps_install/include/pcl-1.10/pcl/stereo/digital_elevation_map.h b/deps_install/include/pcl-1.10/pcl/stereo/digital_elevation_map.h new file mode 100755 index 0000000..bf3d397 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/stereo/digital_elevation_map.h @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +namespace pcl { + +/** \brief Build a Digital Elevation Map in the column-disparity space from a disparity + * map and a color image of the scene. + * + * Example of usage: + * + * \code + * pcl::PointCloud::Ptr cloud (new + * pcl::PointCloud); + * pcl::PointCloud::Ptr left_image (new + * pcl::PointCloud); + * // Fill left image cloud. + * + * pcl::DigitalElevationMapBuilder demb; + * demb.setBaseline (0.8387445f); + * demb.setFocalLength (368.534700f); + * demb.setImageCenterX (318.112200f); + * demb.setImageCenterY (224.334900f); + * demb.setDisparityThresholdMin (15.0f); + * demb.setDisparityThresholdMax (80.0f); + * demb.setResolution (64, 32); + * + * // Left view of the scene. + * demb.loadImage (left_image); + * // Disparity map of the scene. + * demb.loadDisparityMap ("disparity_map.txt", 640, 480); + * + * demb.compute(*cloud); + * \endcode + * + * \author Timur Ibadov (ibadov.timur@gmail.com) + * \ingroup stereo + */ +class PCL_EXPORTS DigitalElevationMapBuilder : public DisparityMapConverter { +public: + using DisparityMapConverter::baseline_; + using DisparityMapConverter::translateCoordinates; + using DisparityMapConverter::image_; + using DisparityMapConverter::disparity_map_; + using DisparityMapConverter::disparity_map_width_; + using DisparityMapConverter::disparity_map_height_; + using DisparityMapConverter::disparity_threshold_min_; + using DisparityMapConverter::disparity_threshold_max_; + + /** \brief DigitalElevationMapBuilder constructor. */ + DigitalElevationMapBuilder(); + + /** \brief Empty destructor. */ + ~DigitalElevationMapBuilder(); + + /** \brief Set resolution of the DEM. + * \param[in] resolution_column the column resolution. + * \param[in] resolution_disparity the disparity resolution. + */ + void + setResolution(std::size_t resolution_column, std::size_t resolution_disparity); + + /** \brief Get column resolution of the DEM. + * \return column resolution of the DEM. + */ + std::size_t + getColumnResolution() const; + + /** \brief Get disparity resolution of the DEM. + * \return disparity resolution of the DEM. + */ + std::size_t + getDisparityResolution() const; + + /** \brief Set minimum amount of points in a DEM's cell. + * \param[in] min_points_in_cell minimum amount of points in a DEM's cell. + */ + void + setMinPointsInCell(std::size_t min_points_in_cell); + + /** \brief Get minimum amount of points in a DEM's cell. + * \return minimum amount of points in a DEM's cell. + */ + std::size_t + getMinPointsInCell() const; + + /** \brief Compute the Digital Elevation Map. + * \param[out] out_cloud the variable to return the resulting cloud. + */ + void + compute(pcl::PointCloud& out_cloud) override; + +protected: + /** \brief Column resolution of the DEM. */ + std::size_t resolution_column_; + /** \brief disparity resolution of the DEM. */ + std::size_t resolution_disparity_; + /** \brief Minimum amount of points in a DEM's cell. */ + std::size_t min_points_in_cell_; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/stereo/disparity_map_converter.h b/deps_install/include/pcl-1.10/pcl/stereo/disparity_map_converter.h new file mode 100755 index 0000000..a25e9eb --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/stereo/disparity_map_converter.h @@ -0,0 +1,258 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +#include +#include + +namespace pcl { + +/** \brief Compute point cloud from the disparity map. + * + * Example of usage: + * + * \code + * pcl::PointCloud::Ptr cloud (new + * pcl::PointCloud); + * pcl::PointCloud::Ptr left_image (new + * pcl::PointCloud); + * // Fill left image cloud. + * + * pcl::DisparityMapConverter dmc; + * dmc.setBaseline (0.8387445f); + * dmc.setFocalLength (368.534700f); + * dmc.setImageCenterX (318.112200f); + * dmc.setImageCenterY (224.334900f); + * dmc.setDisparityThresholdMin(15.0f); + * + * // Left view of the scene. + * dmc.setImage (left_image); + * // Disparity map of the scene. + * dmc.loadDisparityMap ("disparity_map.txt", 640, 480); + * + * dmc.compute(*cloud); + * \endcode + * + * \author Timur Ibadov (ibadov.timur@gmail.com) + * \ingroup stereo + */ +template +class DisparityMapConverter { +protected: + using PointCloud = pcl::PointCloud; + +public: + /** \brief DisparityMapConverter constructor. */ + DisparityMapConverter(); + + /** \brief Empty destructor. */ + virtual ~DisparityMapConverter(); + + /** \brief Set x-coordinate of the image center. + * \param[in] center_x x-coordinate of the image center. + */ + inline void + setImageCenterX(const float center_x); + + /** \brief Get x-coordinate of the image center. + * \return x-coordinate of the image center. + */ + inline float + getImageCenterX() const; + + /** \brief Set y-coordinate of the image center. + * \param[in] center_y y-coordinate of the image center. + */ + inline void + setImageCenterY(const float center_y); + + /** \brief Get y-coordinate of the image center. + * \return y-coordinate of the image center. + */ + inline float + getImageCenterY() const; + + /** \brief Set focal length. + * \param[in] focal_length the focal length. + */ + inline void + setFocalLength(const float focal_length); + + /** \brief Get focal length. + * \return the focal length. + */ + inline float + getFocalLength() const; + + /** \brief Set baseline. + * \param[in] baseline baseline. + */ + inline void + setBaseline(const float baseline); + + /** \brief Get baseline. + * \return the baseline. + */ + inline float + getBaseline() const; + + /** \brief Set min disparity threshold. + * \param[in] disparity_threshold_min min disparity threshold. + */ + inline void + setDisparityThresholdMin(const float disparity_threshold_min); + + /** \brief Get min disparity threshold. + * \return min disparity threshold. + */ + inline float + getDisparityThresholdMin() const; + + /** \brief Set max disparity threshold. + * \param[in] disparity_threshold_max max disparity threshold. + */ + inline void + setDisparityThresholdMax(const float disparity_threshold_max); + + /** \brief Get max disparity threshold. + * \return max disparity threshold. + */ + inline float + getDisparityThresholdMax() const; + + /** \brief Set an image, that will be used for coloring of the output cloud. + * \param[in] image the image. + */ + void + setImage(const pcl::PointCloud::ConstPtr& image); + + /** \brief Get the image, that is used for coloring of the output cloud. + * \return the image. + */ + pcl::PointCloud::Ptr + getImage(); + + /** \brief Load the disparity map. + * \param[in] file_name the name of the disparity map file. + * \return "true" if the disparity map was successfully loaded; "false" otherwise + */ + bool + loadDisparityMap(const std::string& file_name); + + /** \brief Load the disparity map and initialize it's size. + * \param[in] file_name the name of the disparity map file. + * \param[in] width width of the disparity map. + * \param[in] height height of the disparity map. + * \return "true" if the disparity map was successfully loaded; "false" otherwise + */ + bool + loadDisparityMap(const std::string& file_name, + const std::size_t width, + const std::size_t height); + + /** \brief Set the disparity map. + * \param[in] disparity_map the disparity map. + */ + void + setDisparityMap(const std::vector& disparity_map); + + /** \brief Set the disparity map and initialize it's size. + * \param[in] disparity_map the disparity map. + * \param[in] width width of the disparity map. + * \param[in] height height of the disparity map. + * \return "true" if the disparity map was successfully loaded; "false" otherwise + */ + void + setDisparityMap(const std::vector& disparity_map, + const std::size_t width, + const std::size_t height); + + /** \brief Get the disparity map. + * \return the disparity map. + */ + std::vector + getDisparityMap(); + + /** \brief Compute the output cloud. + * \param[out] out_cloud the variable to return the resulting cloud. + */ + virtual void + compute(PointCloud& out_cloud); + +protected: + /** \brief Translate point from image coordinates and disparity to 3D-coordinates + * \param[in] row + * \param[in] column + * \param[in] disparity + * \return the 3D point, that corresponds to the input parametres and the camera + * calibration. + */ + PointXYZ + translateCoordinates(std::size_t row, std::size_t column, float disparity) const; + + /** \brief X-coordinate of the image center. */ + float center_x_; + /** \brief Y-coordinate of the image center. */ + float center_y_; + /** \brief Focal length. */ + float focal_length_; + /** \brief Baseline. */ + float baseline_; + + /** \brief Is color image is set. */ + bool is_color_; + /** \brief Color image of the scene. */ + pcl::PointCloud::ConstPtr image_; + + /** \brief Vector for the disparity map. */ + std::vector disparity_map_; + /** \brief X-size of the disparity map. */ + std::size_t disparity_map_width_; + /** \brief Y-size of the disparity map. */ + std::size_t disparity_map_height_; + + /** \brief Thresholds of the disparity. */ + float disparity_threshold_min_; + float disparity_threshold_max_; +}; + +} // namespace pcl + +#include diff --git a/deps_install/include/pcl-1.10/pcl/stereo/impl/disparity_map_converter.hpp b/deps_install/include/pcl-1.10/pcl/stereo/impl/disparity_map_converter.hpp new file mode 100755 index 0000000..b9e3af6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/stereo/impl/disparity_map_converter.hpp @@ -0,0 +1,327 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ +#define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ + +#include + +#include +#include + +#include +#include +#include + +template +pcl::DisparityMapConverter::DisparityMapConverter() +: center_x_(0.0f) +, center_y_(0.0f) +, focal_length_(0.0f) +, baseline_(0.0f) +, is_color_(false) +, disparity_map_width_(640) +, disparity_map_height_(480) +, disparity_threshold_min_(0.0f) +, disparity_threshold_max_(std::numeric_limits::max()) +{} + +template +pcl::DisparityMapConverter::~DisparityMapConverter() +{} + +template +inline void +pcl::DisparityMapConverter::setImageCenterX(const float center_x) +{ + center_x_ = center_x; +} + +template +inline float +pcl::DisparityMapConverter::getImageCenterX() const +{ + return center_x_; +} + +template +inline void +pcl::DisparityMapConverter::setImageCenterY(const float center_y) +{ + center_y_ = center_y; +} + +template +inline float +pcl::DisparityMapConverter::getImageCenterY() const +{ + return center_y_; +} + +template +inline void +pcl::DisparityMapConverter::setFocalLength(const float focal_length) +{ + focal_length_ = focal_length; +} + +template +inline float +pcl::DisparityMapConverter::getFocalLength() const +{ + return focal_length_; +} + +template +inline void +pcl::DisparityMapConverter::setBaseline(const float baseline) +{ + baseline_ = baseline; +} + +template +inline float +pcl::DisparityMapConverter::getBaseline() const +{ + return baseline_; +} + +template +inline void +pcl::DisparityMapConverter::setDisparityThresholdMin( + const float disparity_threshold_min) +{ + disparity_threshold_min_ = disparity_threshold_min; +} + +template +inline float +pcl::DisparityMapConverter::getDisparityThresholdMin() const +{ + return disparity_threshold_min_; +} + +template +inline void +pcl::DisparityMapConverter::setDisparityThresholdMax( + const float disparity_threshold_max) +{ + disparity_threshold_max_ = disparity_threshold_max; +} + +template +inline float +pcl::DisparityMapConverter::getDisparityThresholdMax() const +{ + return disparity_threshold_max_; +} + +template +void +pcl::DisparityMapConverter::setImage( + const pcl::PointCloud::ConstPtr& image) +{ + image_ = image; + + // Set disparity map's size same with the image size. + disparity_map_width_ = image_->width; + disparity_map_height_ = image_->height; + + is_color_ = true; +} + +template +pcl::PointCloud::Ptr +pcl::DisparityMapConverter::getImage() +{ + pcl::PointCloud::Ptr image_pointer(new pcl::PointCloud); + *image_pointer = *image_; + return image_pointer; +} + +template +bool +pcl::DisparityMapConverter::loadDisparityMap(const std::string& file_name) +{ + std::fstream disparity_file; + + // Open the disparity file + disparity_file.open(file_name.c_str(), std::fstream::in); + if (!disparity_file.is_open()) { + PCL_ERROR("[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n"); + return false; + } + + // Allocate memory for the disparity map. + disparity_map_.resize(disparity_map_width_ * disparity_map_height_); + + // Reading the disparity map. + for (std::size_t row = 0; row < disparity_map_height_; ++row) { + for (std::size_t column = 0; column < disparity_map_width_; ++column) { + float disparity; + disparity_file >> disparity; + + disparity_map_[column + row * disparity_map_width_] = disparity; + } // column + } // row + + return true; +} + +template +bool +pcl::DisparityMapConverter::loadDisparityMap(const std::string& file_name, + const std::size_t width, + const std::size_t height) +{ + // Initialize disparity map's size. + disparity_map_width_ = width; + disparity_map_height_ = height; + + // Load the disparity map. + return loadDisparityMap(file_name); +} + +template +void +pcl::DisparityMapConverter::setDisparityMap( + const std::vector& disparity_map) +{ + disparity_map_ = disparity_map; +} + +template +void +pcl::DisparityMapConverter::setDisparityMap( + const std::vector& disparity_map, + const std::size_t width, + const std::size_t height) +{ + disparity_map_width_ = width; + disparity_map_height_ = height; + + disparity_map_ = disparity_map; +} + +template +std::vector +pcl::DisparityMapConverter::getDisparityMap() +{ + return disparity_map_; +} + +template +void +pcl::DisparityMapConverter::compute(PointCloud& out_cloud) +{ + // Initialize the output cloud. + out_cloud.clear(); + out_cloud.width = disparity_map_width_; + out_cloud.height = disparity_map_height_; + out_cloud.resize(out_cloud.width * out_cloud.height); + + if (is_color_ && !image_) { + PCL_ERROR("[pcl::DisparityMapConverter::compute] Memory for the image was not " + "allocated.\n"); + return; + } + + for (std::size_t row = 0; row < disparity_map_height_; ++row) { + for (std::size_t column = 0; column < disparity_map_width_; ++column) { + // ID of current disparity point. + std::size_t disparity_point = column + row * disparity_map_width_; + + // Disparity value. + float disparity = disparity_map_[disparity_point]; + + // New point for the output cloud. + PointT new_point; + + // Init color + if (is_color_) { + pcl::common::IntensityFieldAccessor intensity_accessor; + intensity_accessor.set(new_point, + static_cast(image_->points[disparity_point].r + + image_->points[disparity_point].g + + image_->points[disparity_point].b) / + 3.0f); + } + + // Init coordinates. + if (disparity_threshold_min_ < disparity && + disparity < disparity_threshold_max_) { + // Compute new coordinates. + PointXYZ point_3D(translateCoordinates(row, column, disparity)); + new_point.x = point_3D.x; + new_point.y = point_3D.y; + new_point.z = point_3D.z; + } + else { + new_point.x = std::numeric_limits::quiet_NaN(); + new_point.y = std::numeric_limits::quiet_NaN(); + new_point.z = std::numeric_limits::quiet_NaN(); + } + // Put the point to the output cloud. + out_cloud[disparity_point] = new_point; + } // column + } // row +} + +template +pcl::PointXYZ +pcl::DisparityMapConverter::translateCoordinates(std::size_t row, + std::size_t column, + float disparity) const +{ + // Returning point. + PointXYZ point_3D; + + if (disparity != 0.0f) { + // Compute 3D-coordinates based on the image coordinates, the disparity and the + // camera parameters. + float z_value = focal_length_ * baseline_ / disparity; + point_3D.z = z_value; + point_3D.x = (static_cast(column) - center_x_) * (z_value / focal_length_); + point_3D.y = (static_cast(row) - center_y_) * (z_value / focal_length_); + } + + return point_3D; +} + +#define PCL_INSTANTIATE_DisparityMapConverter(T) \ + template class PCL_EXPORTS pcl::DisparityMapConverter; + +#endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ diff --git a/deps_install/include/pcl-1.10/pcl/stereo/stereo_grabber.h b/deps_install/include/pcl-1.10/pcl/stereo/stereo_grabber.h new file mode 100755 index 0000000..02263d9 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/stereo/stereo_grabber.h @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl { + +/** \brief Base class for Stereo file grabber. + * \ingroup io + */ +class PCL_EXPORTS StereoGrabberBase : public Grabber { +public: + /** \brief Constructor taking just one Stereo pair. + * \param[in] pair_files the name of the the stereo (left + right) images. + * \param[in] frames_per_second frames per second. If 0, start() functions like a + * trigger, publishing the next pair in the list. + * \param[in] repeat whether to play files in an endless loop or not. + */ + StereoGrabberBase(const std::pair& pair_files, + float frames_per_second, + bool repeat); + + /** \brief Constructor taking a list of paths to Stereo pair files, that are played in + * the order they appear in the list. + * + * \param[in] files vector of paths to stereo (left+right) images. + * \param[in] frames_per_second frames per second. If 0, start() functions like a + * trigger, publishing the next pair in the list. + * \param[in] repeat whether to play files in an endless loop or not. + */ + StereoGrabberBase(const std::vector>& files, + float frames_per_second, + bool repeat); + + /** \brief Copy constructor. + * \param[in] src the Stereo Grabber base object to copy into this + */ + StereoGrabberBase(const StereoGrabberBase& src) : impl_() { *this = src; } + + /** \brief Copy operator. + * \param[in] src the Stereo Grabber base object to copy into this + */ + StereoGrabberBase& + operator=(const StereoGrabberBase& src) + { + impl_ = src.impl_; + return (*this); + } + + /** \brief Virtual destructor. */ + ~StereoGrabberBase() noexcept; + + /** \brief Starts playing the list of Stereo images if frames_per_second is > 0. + * Otherwise it works as a trigger: publishes only the next pair in the list. */ + void + start() override; + + /** \brief Stops playing the list of Stereo images if frames_per_second is > 0. + * Otherwise the method has no effect. */ + void + stop() override; + + /** \brief Triggers a callback with new data */ + virtual void + trigger(); + + /** \brief whether the grabber is started (publishing) or not. + * \return true only if publishing. + */ + bool + isRunning() const override; + + /** \return The name of the grabber */ + std::string + getName() const override; + + /** \brief Rewinds to the first pair of files in the list.*/ + virtual void + rewind(); + + /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */ + float + getFramesPerSecond() const override; + + /** \brief Returns whether the repeat flag is on */ + bool + isRepeatOn() const; + +private: + virtual void + publish(const pcl::PCLPointCloud2& blob, + const Eigen::Vector4f& origin, + const Eigen::Quaternionf& orientation) const = 0; + + // to separate and hide the implementation from interface: PIMPL + struct StereoGrabberImpl; + StereoGrabberImpl* impl_; +}; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class StereoGrabber : public StereoGrabberBase { +public: + StereoGrabber(const std::pair& pair_files, + float frames_per_second = 0, + bool repeat = false); + StereoGrabber(const std::vector>& files, + float frames_per_second = 0, + bool repeat = false); + +protected: + void + publish(const pcl::PCLPointCloud2& blob, + const Eigen::Vector4f& origin, + const Eigen::Quaternionf& orientation) const override; + + boost::signals2::signal::ConstPtr&)>* + signal_; +}; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +StereoGrabber::StereoGrabber( + const std::pair& pair_files, + float frames_per_second, + bool repeat) +: StereoGrabberBase(pair_files, frames_per_second, repeat) +{ + signal_ = createSignal::ConstPtr&)>(); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +StereoGrabber::StereoGrabber( + const std::vector>& files, + float frames_per_second, + bool repeat) +: StereoGrabberBase(files, frames_per_second, repeat), signal_() +{ + signal_ = createSignal::ConstPTr&)>(); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void +StereoGrabber::publish(const pcl::PCLPointCloud2& blob, + const Eigen::Vector4f& origin, + const Eigen::Quaternionf& orientation) const +{ + typename pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromPCLPointCloud2(blob, *cloud); + cloud->sensor_origin_ = origin; + cloud->sensor_orientation_ = orientation; + + signal_->operator()(cloud); +} + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/stereo/stereo_matching.h b/deps_install/include/pcl-1.10/pcl/stereo/stereo_matching.h new file mode 100755 index 0000000..20d85ae --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/stereo/stereo_matching.h @@ -0,0 +1,535 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include + +#include +#include + +namespace pcl { +template +short int +doStereoRatioFilter(const T* const acc, + short int dbest, + T sad_min, + int ratio_filter, + int maxdisp, + int precision = 100) +{ + const auto sad_min_1st_part_it = std::min_element(acc, acc + dbest - 1); + const auto sad_min_2nd_part_it = std::min_element(acc + dbest + 2, acc + maxdisp); + + const auto sad_second_min = std::min(*sad_min_1st_part_it, *sad_min_2nd_part_it); + + if ((sad_min * precision) > ((precision - ratio_filter) * sad_second_min)) { + return -2; + } + return dbest; +} + +template +inline short int +doStereoPeakFilter(const T* const acc, short int dbest, int peak_filter, int maxdisp) +{ + // da and db = acc[index] - acc[dbest], + // where index = (dbest + 2) or (dbest - 2) + // => index = dbest + 2 - (0 or 4) = dbest - 2 + (0 or 4) + // => index = dbest + 2 - (0 << 2 or 1 << 2) = dbest - 2 + (0 << 2 or 1 << 2) + // => index = dbest + 2 - (condition << 2) = dbest - 2 + (condition << 2) + const auto da_condition = (dbest > 1); + const auto db_condition = (dbest < maxdisp - 2); + const auto da_index = dbest + 2 - (da_condition << 2); + const auto db_index = dbest - 2 + (db_condition << 2); + + const auto da = acc[da_index] - acc[dbest]; + const auto db = acc[db_index] - acc[dbest]; + if ((da + db) < peak_filter) { + return -4; + } + return dbest; +} + +/** \brief Stereo Matching abstract class + * + * The class performs stereo matching on a rectified stereo pair. + * + * Includes the following functionalities: + * * preprocessing of the image pair, to improve robustness against photometric + * distortions (wrt. to a spatially constant additive photometric factor) + * * postprocessing: filtering of wrong disparities via Peak Filter (eliminating + * ambiguities due to low-textured regions) and Ratio Filter (eliminating generic + * matching ambiguities, similar to that present in OpenCV Block Matching Stereo) + * * postprocessing: Left-Right consistency check (eliminates wrong disparities at the + * cost of twice the stereo matching computation) + * * postprocessing: subpixel refinement of computed disparities, to reduce the depth + * quantization effect + * * postprocessing: smoothing of the disparity map via median filter + * * after stereo matching a PCL point cloud can be computed, given the stereo + * intrinsic (focal, principal point coordinates) and extrinsic (baseline) + * calibration parameters + * + * \author Federico Tombari (federico.tombari@unibo.it) + * \ingroup stereo + */ +class PCL_EXPORTS StereoMatching { +public: + StereoMatching(); + + virtual ~StereoMatching(); + + /** \brief setter for number of disparity candidates (disparity range) + * + * \param[in] max_disp number of disparity candidates (disparity range); has to be > 0 + */ + void + setMaxDisparity(int max_disp) + { + max_disp_ = max_disp; + }; + + /** \brief setter for horizontal offset, i.e. number of pixels to shift the disparity + * range over the target image + * + * \param[in] x_off horizontal offset value; has to be >= 0 + */ + void + setXOffset(int x_off) + { + x_off_ = x_off; + }; + + /** \brief setter for the value of the ratio filter + * + * \param[in] ratio_filter value of the ratio filter; it is a number in the range + * [0, 100] (0: no filtering action; 100: all disparities are + * filtered) + */ + void + setRatioFilter(int ratio_filter) + { + ratio_filter_ = ratio_filter; + }; + + /** \brief setter for the value of the peak filter + * + * \param[in] peak_filter value of the peak filter; it is a number in the range + * [0, inf] (0: no filtering action) + */ + void + setPeakFilter(int peak_filter) + { + peak_filter_ = peak_filter; + }; + + /** \brief setter for the pre processing step + * + * \param[in] is_pre_proc setting the boolean to true activates the pre-processing + * step for both stereo images + */ + void + setPreProcessing(bool is_pre_proc) + { + is_pre_proc_ = is_pre_proc; + }; + + /** \brief setter for the left-right consistency check stage, that eliminates + * inconsistent/wrong disparity values from the disparity map at approx. twice the + * processing cost of the selected stereo algorithm + * + * \param[in] is_lr_check setting the boolean to true activates the left-right + * consistency check + */ + void + setLeftRightCheck(bool is_lr_check) + { + is_lr_check_ = is_lr_check; + }; + + /** \brief setter for the left-right consistency check threshold + * + * \param[in] lr_check_th sets the value of the left-right consistency check threshold + * only has some influence if the left-right check is active typically has + * either the value 0 ("strong" consistency check, more points being + * filtered) or 1 ("weak" consistency check, less points being filtered) + */ + void + setLeftRightCheckThreshold(int lr_check_th) + { + lr_check_th_ = lr_check_th; + }; + + /** \brief stereo processing, it computes a disparity map stored internally by the + * class + * + * \param[in] ref_img reference array of image pixels (left image) + * \param[in] trg_img target array of image pixels (right image) + * \param[in] width number of elements per row for both input arrays + * \param[in] height number of elements per column for both input arrays + */ + virtual void + compute(unsigned char* ref_img, unsigned char* trg_img, int width, int height) = 0; + + /** \brief stereo processing, it computes a disparity map stored internally by the + * class + * + * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference + * image (left image) + * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target + * image (right image) + */ + virtual void + compute(pcl::PointCloud& ref, pcl::PointCloud& trg) = 0; + + /** \brief median filter applied on the previously computed disparity map + * + * \note The "compute" method must have been previously called at least once in order + * for this function to have any effect + * + * \param[in] radius radius of the squared window used to compute the median filter; + * the window side is equal to 2*radius + 1 + */ + void + medianFilter(int radius); + + /** \brief computation of the 3D point cloud from the previously computed disparity + * map without color information + * + * \note The "compute" method must have been previously called at least once in order + * for this function to have any effect + * + * \param[in] u_c horizontal coordinate of the principal point (calibration parameter) + * \param[in] v_c vertical coordinate of the principal point (calibration parameter) + * \param[in] focal focal length in pixels (calibration parameter) + * \param[in] baseline distance between the two cameras (calibration parameter); the + * measure unit used to specify this parameter will be the same as the 3D + * points in the output point cloud + * \param[out] cloud output 3D point cloud; it is organized and non-dense, with NaNs + * where 3D points are invalid + */ + virtual bool + getPointCloud(float u_c, + float v_c, + float focal, + float baseline, + pcl::PointCloud::Ptr cloud); + + /** \brief computation of the 3D point cloud from the previously computed disparity + * map including color information + * + * \note The "compute" method must have been previously called at least once in order + * for this function to have any effect + * + * \param[in] u_c horizontal coordinate of the principal point (calibration parameter) + * \param[in] v_c vertical coordinate of the principal point (calibration parameter) + * \param[in] focal focal length in pixels (calibration parameter) + * \param[in] baseline distance between the two cameras (calibration parameter); the + * measure unit used to specify this parameter will be the same as the 3D + * points in the output point cloud \param[out] cloud output 3D point + * cloud; it is organized and non-dense, with NaNs where 3D points are + * invalid + * \param[in] texture 3D cloud (same size of the output cloud) used to associate to + * each 3D point of the output cloud a color triplet + */ + virtual bool + getPointCloud(float u_c, + float v_c, + float focal, + float baseline, + pcl::PointCloud::Ptr cloud, + pcl::PointCloud::Ptr texture); + + /** \brief computation of a pcl::RGB cloud with scaled disparity values it can be used + * to display a rescaled version of the disparity map by means of the pcl::ImageViewer + * invalid disparity values are shown in green + * + * \note The "compute" method must have been previously called at least once in order + * for this function to have any effect + * + * \param[out] vMap output cloud + */ + void + getVisualMap(pcl::PointCloud::Ptr vMap); + +protected: + /** \brief The internal disparity map. */ + short int* disp_map_; + + /** \brief Local aligned copies of the cloud data. */ + unsigned char* ref_img_; + unsigned char* trg_img_; + + /** \brief Disparity map used for left-right check. */ + short int* disp_map_trg_; + + /** \brief Local aligned copies used for pre processing. */ + unsigned char* pp_ref_img_; + unsigned char* pp_trg_img_; + + /** \brief number of pixels per column of the input stereo pair . */ + int width_; + + /** \brief number of pixels per row of the input stereo pair . */ + int height_; + + /** \brief Disparity range used for stereo processing. */ + int max_disp_; + + /** \brief Horizontal displacemente (x offset) used for stereo processing */ + int x_off_; + + /** \brief Threshold for the ratio filter, \f$\in [0 100]\f$ */ + int ratio_filter_; + + /** \brief Threshold for the peak filter, \f$\in [0 \infty]\f$ */ + int peak_filter_; + + /** \brief toggle for the activation of the pre-processing stage */ + bool is_pre_proc_; + + /** \brief toggle for the activation of the left-right consistency check stage */ + bool is_lr_check_; + + /** \brief Threshold for the left-right consistency check, typically either 0 or 1 */ + int lr_check_th_; + + virtual void + preProcessing(unsigned char* img, unsigned char* pp_img) = 0; + + virtual void + imgFlip(unsigned char*& img) = 0; + + virtual void + compute_impl(unsigned char* ref_img, unsigned char* trg_img) = 0; + + void + leftRightCheck(); + + inline short int + computeStereoSubpixel(int dbest, int s1, int s2, int s3) + { + int den = (s1 + s3 - 2 * s2); + if (den != 0) + return (static_cast(16 * dbest + (((s1 - s3) * 8) / den))); + return (static_cast(dbest * 16)); + } + + inline short int + computeStereoSubpixel(int dbest, float s1, float s2, float s3) + { + float den = (s1 + s3 - 2 * s2); + if (den != 0) + return (static_cast(16 * dbest + + std::floor(.5 + (((s1 - s3) * 8) / den)))); + return (static_cast(dbest * 16)); + } +}; + +/** \brief Stereo Matching abstract class for Grayscale images + * + * The class implements some functionalities of pcl::StereoMatching specific for + * grayscale stereo processing, such as image pre-processing and left + * + * \author Federico Tombari (federico.tombari@unibo.it) + * \ingroup stereo + */ +class PCL_EXPORTS GrayStereoMatching : public StereoMatching { +public: + GrayStereoMatching(); + ~GrayStereoMatching(); + + /** \brief stereo processing, it computes a disparity map stored internally by the + * class + * + * \param[in] ref_img reference array of image pixels (left image), has to be + * grayscale single channel + * \param[in] trg_img target array of image pixels (right image), has to be grayscale + * single channel + * \param[in] width number of elements per row for both input arrays + * \param[in] height number of elements per column for both input arrays + */ + void + compute(unsigned char* ref_img, + unsigned char* trg_img, + int width, + int height) override; + + /** \brief stereo processing, it computes a disparity map stored internally by the + * class + * + * \param[in] ref point cloud of pcl::RGB type containing the pixels of the reference + * image (left image) the pcl::RGB triplets are automatically converted to + * grayscale upon call of the method + * \param[in] trg point cloud of pcl::RGB type containing the pixels of the target + * image (right image) the pcl::RGB triplets are automatically converted to + * grayscale upon call of the method + */ + void + compute(pcl::PointCloud& ref, pcl::PointCloud& trg) override; + +protected: + void + compute_impl(unsigned char* ref_img, unsigned char* trg_img) override = 0; + + void + preProcessing(unsigned char* img, unsigned char* pp_img) override; + + void + imgFlip(unsigned char*& img) override; +}; + +/** \brief Block based (or fixed window) Stereo Matching class + * + * This class implements the baseline Block-based - aka Fixed Window - stereo matching + * algorithm. The algorithm includes a running box filter so that the computational + * complexity is independent of the size of the window ( O(1) wrt. to the size of + * window) The algorithm is based on the Sum of Absolute Differences (SAD) matching + * function Only works with grayscale (single channel) rectified images + * + * \author Federico Tombari (federico.tombari@unibo.it) + * \ingroup stereo + */ + +class PCL_EXPORTS BlockBasedStereoMatching : public GrayStereoMatching { +public: + BlockBasedStereoMatching(); + ~BlockBasedStereoMatching(){}; + + /** \brief setter for the radius of the squared window + * \param[in] radius radius of the squared window used to compute the block-based + * stereo algorithm the window side is equal to 2*radius + 1 + */ + void + setRadius(int radius) + { + radius_ = radius; + }; + +private: + void + compute_impl(unsigned char* ref_img, unsigned char* trg_img) override; + + int radius_; +}; + +/** \brief Adaptive Cost 2-pass Scanline Optimization Stereo Matching class + * + * This class implements an adaptive-cost stereo matching algorithm based on 2-pass + * Scanline Optimization. The algorithm is inspired by the paper: [1] L. Wang et al., + * "High Quality Real-time Stereo using Adaptive Cost Aggregation and Dynamic + * Programming", 3DPVT 2006 Cost aggregation is performed using adaptive weigths + * computed on a single column as proposed in [1]. Instead of using Dynamic Programming + * as in [1], the optimization is performed via 2-pass Scanline Optimization. The + * algorithm is based on the Sum of Absolute Differences (SAD) matching function Only + * works with grayscale (single channel) rectified images + * + * \author Federico Tombari (federico.tombari@unibo.it) + * \ingroup stereo + */ +class PCL_EXPORTS AdaptiveCostSOStereoMatching : public GrayStereoMatching { +public: + AdaptiveCostSOStereoMatching(); + + ~AdaptiveCostSOStereoMatching(){}; + + /** \brief setter for the radius (half length) of the column used for cost aggregation + * \param[in] radius radius (half length) of the column used for cost aggregation; the + * total column length is equal to 2*radius + 1 + */ + void + setRadius(int radius) + { + radius_ = radius; + }; + + /** \brief setter for the spatial bandwidth used for cost aggregation based on + * adaptive weights + * \param[in] gamma_s spatial bandwidth used for cost aggregation based on adaptive + * weights + */ + void + setGammaS(int gamma_s) + { + gamma_s_ = gamma_s; + }; + + /** \brief setter for the color bandwidth used for cost aggregation based on adaptive + * weights + * \param[in] gamma_c color bandwidth used for cost aggregation based on + * adaptive weights + */ + void + setGammaC(int gamma_c) + { + gamma_c_ = gamma_c; + }; + + /** \brief "weak" smoothness penalty used within 2-pass Scanline Optimization + * \param[in] smoothness_weak "weak" smoothness penalty cost + */ + void + setSmoothWeak(int smoothness_weak) + { + smoothness_weak_ = smoothness_weak; + }; + + /** \brief "strong" smoothness penalty used within 2-pass Scanline Optimization + * \param[in] smoothness_strong "strong" smoothness penalty cost + */ + void + setSmoothStrong(int smoothness_strong) + { + smoothness_strong_ = smoothness_strong; + }; + +private: + void + compute_impl(unsigned char* ref_img, unsigned char* trg_img) override; + + int radius_; + + // parameters for adaptive weight cost aggregation + double gamma_c_; + double gamma_s_; + + // Parameters for 2-pass SO optimization + int smoothness_strong_; + int smoothness_weak_; +}; + +} // namespace pcl diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/allocator.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/allocator.h new file mode 100755 index 0000000..bf4a586 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/allocator.h @@ -0,0 +1,168 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef ALLOCATOR_INCLUDED +#define ALLOCATOR_INCLUDED +#include + +namespace pcl +{ + namespace poisson + { + class AllocatorState{ + public: + int index,remains; + }; + /** This templated class assists in memory allocation and is well suited for instances + * when it is known that the sequence of memory allocations is performed in a stack-based + * manner, so that memory allocated last is released first. It also preallocates memory + * in chunks so that multiple requests for small chunks of memory do not require separate + * system calls to the memory manager. + * The allocator is templated off of the class of objects that we would like it to allocate, + * ensuring that appropriate constructors and destructors are called as necessary. + */ + template + class Allocator{ + int blockSize; + int index,remains; + std::vector memory; + public: + Allocator(void){ + blockSize=index=remains=0; + } + ~Allocator(void){ + reset(); + } + + /** This method is the allocators destructor. It frees up any of the memory that + * it has allocated. */ + void reset(void){ + for(std::size_t i=0;iblockSize=blockSize; + index=-1; + remains=0; + } + + /** This method returns a pointer to an array of elements objects. If there is left over pre-allocated + * memory, this method simply returns a pointer to the next free piece of memory, otherwise it pre-allocates + * more memory. Note that if the number of objects requested is larger than the value blockSize with which + * the allocator was initialized, the request for memory will fail. + */ + T* newElements( int elements=1){ + T* mem; + if(!elements){return NULL;} + if(elements>blockSize){ + fprintf(stderr,"Allocator Error, elements bigger than block-size: %d>%d\n",elements,blockSize); + return NULL; + } + if(remains + class BinaryNode + { + public: + static inline int CenterCount( int depth ) { return 1<>= 1; + depth++; + } +#if MSVC_2010_FIX + depth--; +#endif // MSVC_2010_FIX + offset = ( idx+1 ) - (1< + class BSplineData + { + bool useDotRatios; + bool reflectBoundary; + public: + struct BSplineComponents + { + Polynomial< Degree > polys[Degree+1]; + Polynomial< Degree >& operator[] ( int idx ) { return polys[idx]; } + const Polynomial< Degree >& operator[] ( int idx ) const { return polys[idx]; } + void printnl( void ) const { for( int d=0 ; d<=Degree ; d++ ) polys[d].printnl(); } + BSplineComponents scale( double s ) const { BSplineComponents b ; for( int d=0 ; d<=Degree ; d++ ) b[d] = polys[d].scale(s) ; return b; } + BSplineComponents shift( double s ) const { BSplineComponents b ; for( int d=0 ; d<=Degree ; d++ ) b[d] = polys[d].shift(s) ; return b; } + }; + const static int VV_DOT_FLAG = 1; + const static int DV_DOT_FLAG = 2; + const static int DD_DOT_FLAG = 4; + const static int VALUE_FLAG = 1; + const static int D_VALUE_FLAG = 2; + + int depth , functionCount , sampleCount; + Real *vvDotTable , *dvDotTable , *ddDotTable; + Real *valueTables , *dValueTables; + PPolynomial< Degree > baseFunction , leftBaseFunction , rightBaseFunction; + PPolynomial< Degree-1 > dBaseFunction , dLeftBaseFunction , dRightBaseFunction; + BSplineComponents baseBSpline, leftBSpline , rightBSpline; + PPolynomial* baseFunctions; + BSplineComponents* baseBSplines; + + BSplineData(void); + ~BSplineData(void); + + virtual void setDotTables( int flags ); + virtual void clearDotTables( int flags ); + + virtual void setValueTables( int flags,double smooth=0); + virtual void setValueTables( int flags,double valueSmooth,double normalSmooth); + virtual void clearValueTables(void); + + void setSampleSpan( int idx , int& start , int& end , double smooth=0 ) const; + + /******************************************************** + * Sets the translates and scales of the basis function + * up to the prescribed depth + * the maximum depth + * specifies if dot-products of derivatives + * should be pre-divided by function integrals + * spcifies if function space should be + * forced to be reflectively symmetric across the boundary + ********************************************************/ + void set( int maxDepth , bool useDotRatios=true , bool reflectBoundary=false ); + + inline int Index( int i1 , int i2 ) const; + static inline int SymmetricIndex( int i1 , int i2 ); + static inline int SymmetricIndex( int i1 , int i2 , int& index ); + }; + + template< int Degree > + struct BSplineElementCoefficients + { + int coeffs[Degree+1]; + BSplineElementCoefficients( void ) { memset( coeffs , 0 , sizeof( int ) * ( Degree+1 ) ); } + int& operator[]( int idx ){ return coeffs[idx]; } + const int& operator[]( int idx ) const { return coeffs[idx]; } + }; + template< int Degree > + struct BSplineElements : public std::vector< BSplineElementCoefficients< Degree > > + { + static const int _off = (Degree+1)/2; + void _addLeft ( int offset , int boundary ); + void _addRight( int offset , int boundary ); + public: + enum + { + NONE = 0, + DIRICHLET = -1, + NEUMANN = 1 + }; + // Coefficients are ordered as "/" "-" "\" + int denominator; + + BSplineElements( void ) { denominator = 1; } + BSplineElements( int res , int offset , int boundary=NONE ); + + void upSample( BSplineElements& high ) const; + void differentiate( BSplineElements< Degree-1 >& d ) const; + + void print( FILE* fp=stdout ) const + { + for( int i=0 ; isize() ; i++ ) + { + printf( "%d]" , i ); + for( int j=0 ; j<=Degree ; j++ ) printf( " %d" , (*this)[i][j] ); + printf( " (%d)\n" , denominator ); + } + } + }; + template< int Degree1 , int Degree2 > void SetBSplineElementIntegrals( double integrals[Degree1+1][Degree2+1] ); + + + } +} + + +#include "bspline_data.hpp" + +#endif // BSPLINE_DATA_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/bspline_data.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/bspline_data.hpp new file mode 100755 index 0000000..85c0d60 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/bspline_data.hpp @@ -0,0 +1,535 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#include "poisson_exceptions.h" + +namespace pcl +{ + namespace poisson + { + + ///////////////// + // BSplineData // + ///////////////// + // Support[i]: + // Odd: i +/- 0.5 * ( 1 + Degree ) + // i - 0.5 * ( 1 + Degree ) < 0 + // <=> i < 0.5 * ( 1 + Degree ) + // i + 0.5 * ( 1 + Degree ) > 0 + // <=> i > - 0.5 * ( 1 + Degree ) + // i + 0.5 * ( 1 + Degree ) > r + // <=> i > r - 0.5 * ( 1 + Degree ) + // i - 0.5 * ( 1 + Degree ) < r + // <=> i < r + 0.5 * ( 1 + Degree ) + // Even: i + 0.5 +/- 0.5 * ( 1 + Degree ) + // i - 0.5 * Degree < 0 + // <=> i < 0.5 * Degree + // i + 1 + 0.5 * Degree > 0 + // <=> i > -1 - 0.5 * Degree + // i + 1 + 0.5 * Degree > r + // <=> i > r - 1 - 0.5 * Degree + // i - 0.5 * Degree < r + // <=> i < r + 0.5 * Degree + template< int Degree > inline bool LeftOverlap( unsigned int depth , int offset ) + { + offset <<= 1; + if( Degree & 1 ) return (offset < 1+Degree) && (offset > -1-Degree ); + else return (offset < Degree) && (offset > -2-Degree ); + } + template< int Degree > inline bool RightOverlap( unsigned int depth , int offset ) + { + offset <<= 1; + int r = 1<<(depth+1); + if( Degree & 1 ) return (offset > 2-1-Degree) && (offset < 2+1+Degree ); + else return (offset > 2-2-Degree) && (offset < 2+ Degree ); + } + template< int Degree > inline int ReflectLeft( unsigned int depth , int offset ) + { + if( Degree&1 ) return -offset; + else return -1-offset; + } + template< int Degree > inline int ReflectRight( unsigned int depth , int offset ) + { + int r = 1<<(depth+1); + if( Degree&1 ) return r -offset; + else return r-1-offset; + } + + template< int Degree , class Real > + BSplineData::BSplineData( void ) + { + vvDotTable = dvDotTable = ddDotTable = NULL; + valueTables = dValueTables = NULL; + baseFunctions = NULL; + baseBSplines = NULL; + functionCount = sampleCount = 0; + } + + template< int Degree , class Real > + BSplineData< Degree , Real >::~BSplineData(void) + { + if( functionCount ) + { + if( vvDotTable ) delete[] vvDotTable; + if( dvDotTable ) delete[] dvDotTable; + if( ddDotTable ) delete[] ddDotTable; + + if( valueTables ) delete[] valueTables; + if( dValueTables ) delete[] dValueTables; + + if( baseFunctions ) delete[] baseFunctions; + if( baseBSplines ) delete[] baseBSplines; + } + vvDotTable = dvDotTable = ddDotTable = NULL; + valueTables = dValueTables=NULL; + baseFunctions = NULL; + baseBSplines = NULL; + functionCount = 0; + } + + template + void BSplineData::set( int maxDepth , bool useDotRatios , bool reflectBoundary ) + { + this->useDotRatios = useDotRatios; + this->reflectBoundary = reflectBoundary; + + depth = maxDepth; + // [Warning] This assumes that the functions spacing is dual + functionCount = BinaryNode< double >::CumulativeCenterCount( depth ); + sampleCount = BinaryNode< double >::CenterCount( depth ) + BinaryNode< double >::CornerCount( depth ); + baseFunctions = new PPolynomial[functionCount]; + baseBSplines = new BSplineComponents[functionCount]; + + baseFunction = PPolynomial< Degree >::BSpline(); + for( int i=0 ; i<=Degree ; i++ ) baseBSpline[i] = Polynomial< Degree >::BSplineComponent( i ).shift( double(-(Degree+1)/2) + i - 0.5 ); + dBaseFunction = baseFunction.derivative(); + StartingPolynomial< Degree > sPolys[Degree+3]; + + for( int i=0 ; i=1 && i<=Degree+1 ) sPolys[i].p += baseBSpline[i-1]; + for( int j=0 ; j=1 && i<=Degree+1 ) sPolys[i].p += baseBSpline[i-1].shift( 1 ); + for( int j=0 ; j::CenterAndWidth( i , c , w ); + baseFunctions[i] = baseFunction.scale(w).shift(c); + baseBSplines[i] = baseBSpline.scale(w).shift(c); + if( reflectBoundary ) + { + int d , off , r; + BinaryNode< double >::DepthAndOffset( i , d , off ); + r = 1< + void BSplineData::setDotTables( int flags ) + { + clearDotTables( flags ); + int size = ( functionCount*functionCount + functionCount )>>1; + int fullSize = functionCount*functionCount; + if( flags & VV_DOT_FLAG ) + { + vvDotTable = new Real[size]; + memset( vvDotTable , 0 , sizeof(Real)*size ); + } + if( flags & DV_DOT_FLAG ) + { + dvDotTable = new Real[fullSize]; + memset( dvDotTable , 0 , sizeof(Real)*fullSize ); + } + if( flags & DD_DOT_FLAG ) + { + ddDotTable = new Real[size]; + memset( ddDotTable , 0 , sizeof(Real)*size ); + } + double vvIntegrals[Degree+1][Degree+1]; + double vdIntegrals[Degree+1][Degree ]; + double dvIntegrals[Degree ][Degree+1]; + double ddIntegrals[Degree ][Degree ]; + int vvSums[Degree+1][Degree+1]; + int vdSums[Degree+1][Degree ]; + int dvSums[Degree ][Degree+1]; + int ddSums[Degree ][Degree ]; + SetBSplineElementIntegrals< Degree , Degree >( vvIntegrals ); + SetBSplineElementIntegrals< Degree , Degree-1 >( vdIntegrals ); + SetBSplineElementIntegrals< Degree-1 , Degree >( dvIntegrals ); + SetBSplineElementIntegrals< Degree-1 , Degree-1 >( ddIntegrals ); + + for( int d1=0 ; d1<=depth ; d1++ ) + for( int off1=0 ; off1<(1<::CenterIndex( d1 , off1 ); + BSplineElements< Degree > b1( 1<::NEUMANN : BSplineElements< Degree>::NONE ); + BSplineElements< Degree-1 > db1; + b1.differentiate( db1 ); + + int start1 , end1; + + start1 = -1; + for( int i=0 ; i=end1 || start1>=end2 ) continue; + start2 = std::max< int >( start1 , start2 ); + end2 = std::min< int >( end1 , end2 ); + if( d1==d2 && off2::CenterIndex( d2 , off2 ); + BSplineElements< Degree > b2( 1<::NEUMANN : BSplineElements< Degree>::NONE ); + BSplineElements< Degree-1 > db2; + b2.differentiate( db2 ); + + int idx = SymmetricIndex( ii , jj ); + int idx1 = Index( ii , jj ) , idx2 = Index( jj , ii ); + + memset( vvSums , 0 , sizeof( int ) * ( Degree+1 ) * ( Degree+1 ) ); + memset( vdSums , 0 , sizeof( int ) * ( Degree+1 ) * ( Degree ) ); + memset( dvSums , 0 , sizeof( int ) * ( Degree ) * ( Degree+1 ) ); + memset( ddSums , 0 , sizeof( int ) * ( Degree ) * ( Degree ) ); + for( int i=start2 ; i b; + b = b1; + b.upSample( b1 ); + b1.differentiate( db1 ); + start1 = -1; + for( int i=0 ; i + void BSplineData::clearDotTables( int flags ) + { + if( (flags & VV_DOT_FLAG) && vvDotTable ) delete[] vvDotTable , vvDotTable = NULL; + if( (flags & DV_DOT_FLAG) && dvDotTable ) delete[] dvDotTable , dvDotTable = NULL; + if( (flags & DD_DOT_FLAG) && ddDotTable ) delete[] ddDotTable , ddDotTable = NULL; + } + template< int Degree , class Real > + void BSplineData< Degree , Real >::setSampleSpan( int idx , int& start , int& end , double smooth ) const + { + int d , off , res; + BinaryNode< double >::DepthAndOffset( idx , d , off ); + res = 1<_start && (start-1)/(sampleCount-1)<=_start + // => start > _start * (sampleCount-1 ) && start <= _start*(sampleCount-1) + 1 + // => _start * (sampleCount-1) + 1 >= start > _start * (sampleCount-1) + start = int( floor( _start * (sampleCount-1) + 1 ) ); + if( start<0 ) start = 0; + // (end)/(sampleCount-1)<_end && (end+1)/(sampleCount-1)>=_end + // => end < _end * (sampleCount-1 ) && end >= _end*(sampleCount-1) - 1 + // => _end * (sampleCount-1) > end >= _end * (sampleCount-1) - 1 + end = int( ceil( _end * (sampleCount-1) - 1 ) ); + if( end>=sampleCount ) end = sampleCount-1; + } + template + void BSplineData::setValueTables( int flags , double smooth ) + { + clearValueTables(); + if( flags & VALUE_FLAG ) valueTables = new Real[functionCount*sampleCount]; + if( flags & D_VALUE_FLAG ) dValueTables = new Real[functionCount*sampleCount]; + PPolynomial function; + PPolynomial dFunction; + for( int i=0 ; i0) + { + function = baseFunctions[i].MovingAverage(smooth); + dFunction = baseFunctions[i].derivative().MovingAverage(smooth); + } + else + { + function = baseFunctions[i]; + dFunction = baseFunctions[i].derivative(); + } + for( int j=0 ; j + void BSplineData::setValueTables(int flags,double valueSmooth,double normalSmooth){ + clearValueTables(); + if(flags & VALUE_FLAG){ valueTables=new Real[functionCount*sampleCount];} + if(flags & D_VALUE_FLAG){dValueTables=new Real[functionCount*sampleCount];} + PPolynomial function; + PPolynomial dFunction; + for(int i=0;i0) { function=baseFunctions[i].MovingAverage(valueSmooth);} + else { function=baseFunctions[i];} + if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);} + else {dFunction=baseFunctions[i].derivative();} + + for(int j=0;j + void BSplineData::clearValueTables(void){ + if( valueTables){delete[] valueTables;} + if(dValueTables){delete[] dValueTables;} + valueTables=dValueTables=NULL; + } + + template + inline int BSplineData::Index( int i1 , int i2 ) const { return i1*functionCount+i2; } + template + inline int BSplineData::SymmetricIndex( int i1 , int i2 ) + { + if( i1>i2 ) return ((i1*i1+i1)>>1)+i2; + else return ((i2*i2+i2)>>1)+i1; + } + template + inline int BSplineData::SymmetricIndex( int i1 , int i2 , int& index ) + { + if( i1>1)+i1; + return 1; + } + else + { + index = ((i1*i1+i1)>>1)+i2; + return 0; + } + } + + + //////////////////////// + // BSplineElementData // + //////////////////////// + template< int Degree > + BSplineElements< Degree >::BSplineElements( int res , int offset , int boundary ) + { + denominator = 1; + this->resize( res , BSplineElementCoefficients() ); + + for( int i=0 ; i<=Degree ; i++ ) + { + int idx = -_off + offset + i; + if( idx>=0 && idx + void BSplineElements< Degree >::_addLeft( int offset , int boundary ) + { + int res = int( this->size() ); + bool set = false; + for( int i=0 ; i<=Degree ; i++ ) + { + int idx = -_off + offset + i; + if( idx>=0 && idx + void BSplineElements< Degree >::_addRight( int offset , int boundary ) + { + int res = int( this->size() ); + bool set = false; + for( int i=0 ; i<=Degree ; i++ ) + { + int idx = -_off + offset + i; + if( idx>=0 && idx + void BSplineElements< Degree >::upSample( BSplineElements< Degree >& high ) const + { + POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "B-spline up-sampling not supported for degree " << Degree); + } + template<> + void BSplineElements< 1 >::upSample( BSplineElements< 1 >& high ) const + { + high.resize( size()*2 ); + high.assign( high.size() , BSplineElementCoefficients<1>() ); + for( int i=0 ; i + void BSplineElements< 2 >::upSample( BSplineElements< 2 >& high ) const + { + // /----\ + // / \ + // / \ = 1 /--\ +3 /--\ +3 /--\ +1 /--\ + // / \ / \ / \ / \ / \ + // |----------| |----------| |----------| |----------| |----------| + + high.resize( size()*2 ); + high.assign( high.size() , BSplineElementCoefficients<2>() ); + for( int i=0 ; i + void BSplineElements< Degree >::differentiate( BSplineElements< Degree-1 >& d ) const + { + d.resize( this->size() ); + d.assign( d.size() , BSplineElementCoefficients< Degree-1 >() ); + for( int i=0 ; isize()) ; i++ ) for( int j=0 ; j<=Degree ; j++ ) + { + if( j-1>=0 ) d[i][j-1] -= (*this)[i][j]; + if( j + void SetBSplineElementIntegrals( double integrals[Degree1+1][Degree2+1] ) + { + for( int i=0 ; i<=Degree1 ; i++ ) + { + Polynomial< Degree1 > p1 = Polynomial< Degree1 >::BSplineComponent( i ); + for( int j=0 ; j<=Degree2 ; j++ ) + { + Polynomial< Degree2 > p2 = Polynomial< Degree2 >::BSplineComponent( j ); + integrals[i][j] = ( p1 * p2 ).integral( 0 , 1 ); + } + } + } + + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/factor.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/factor.h new file mode 100755 index 0000000..747c8f0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/factor.h @@ -0,0 +1,60 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef FACTOR_INCLUDED +#define FACTOR_INCLUDED + +#include + +#define PI 3.1415926535897932384 +#define SQRT_3 1.7320508075688772935 + +namespace pcl +{ + namespace poisson + { + PCL_EXPORTS double ArcTan2(double y,double x); + PCL_EXPORTS double Angle(const double in[2]); + PCL_EXPORTS void Sqrt(const double in[2],double out[2]); + PCL_EXPORTS void Add(const double in1[2],const double in2[2],double out[2]); + PCL_EXPORTS void Subtract(const double in1[2],const double in2[2],double out[2]); + PCL_EXPORTS void Multiply(const double in1[2],const double in2[2],double out[2]); + PCL_EXPORTS void Divide(const double in1[2],const double in2[2],double out[2]); + + PCL_EXPORTS int Factor(double a1,double a0,double roots[1][2],double EPS); + PCL_EXPORTS int Factor(double a2,double a1,double a0,double roots[2][2],double EPS); + PCL_EXPORTS int Factor(double a3,double a2,double a1,double a0,double roots[3][2],double EPS); + PCL_EXPORTS int Factor(double a4,double a3,double a2,double a1,double a0,double roots[4][2],double EPS); + + PCL_EXPORTS int Solve(const double* eqns,const double* values,double* solutions, int dim); + + } +} + + +#endif // FACTOR_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/function_data.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/function_data.h new file mode 100755 index 0000000..fc1634e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/function_data.h @@ -0,0 +1,123 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef FUNCTION_DATA_INCLUDED +#define FUNCTION_DATA_INCLUDED + +#define BOUNDARY_CONDITIONS 1 + +#if defined __GNUC__ +# pragma GCC system_header +#endif + + +#include "ppolynomial.h" + +namespace pcl +{ + namespace poisson + { + + template + class FunctionData{ + bool useDotRatios; + int normalize; +#if BOUNDARY_CONDITIONS + bool reflectBoundary; +#endif // BOUNDARY_CONDITIONS + public: + const static int DOT_FLAG = 1; + const static int D_DOT_FLAG = 2; + const static int D2_DOT_FLAG = 4; + const static int VALUE_FLAG = 1; + const static int D_VALUE_FLAG = 2; + + int depth , res , res2; + Real *dotTable , *dDotTable , *d2DotTable; + Real *valueTables , *dValueTables; +#if BOUNDARY_CONDITIONS + PPolynomial baseFunction , leftBaseFunction , rightBaseFunction; + PPolynomial dBaseFunction , dLeftBaseFunction , dRightBaseFunction; +#else // !BOUNDARY_CONDITIONS + PPolynomial baseFunction; + PPolynomial dBaseFunction; +#endif // BOUNDARY_CONDITIONS + PPolynomial* baseFunctions; + + FunctionData(void); + ~FunctionData(void); + + virtual void setDotTables(const int& flags); + virtual void clearDotTables(const int& flags); + + virtual void setValueTables(const int& flags,const double& smooth=0); + virtual void setValueTables(const int& flags,const double& valueSmooth,const double& normalSmooth); + virtual void clearValueTables(void); + + /******************************************************** + * Sets the translates and scales of the basis function + * up to the prescribed depth + * the maximum depth + * the basis function + * how the functions should be scaled + * 0] Value at zero equals 1 + * 1] Integral equals 1 + * 2] L2-norm equals 1 + * specifies if dot-products of derivatives + * should be pre-divided by function integrals + * spcifies if function space should be + * forced to be reflectively symmetric across the boundary + ********************************************************/ +#if BOUNDARY_CONDITIONS + void set( const int& maxDepth , const PPolynomial& F , const int& normalize , bool useDotRatios=true , bool reflectBoundary=false ); +#else // !BOUNDARY_CONDITIONS + void set(const int& maxDepth,const PPolynomial& F,const int& normalize , bool useDotRatios=true ); +#endif // BOUNDARY_CONDITIONS + +#if BOUNDARY_CONDITIONS + Real dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const; + Real dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const; + Real d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const; +#else // !BOUNDARY_CONDITIONS + Real dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 ) const; + Real dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 ) const; + Real d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 ) const; +#endif // BOUNDARY_CONDITIONS + + static inline int SymmetricIndex( const int& i1 , const int& i2 ); + static inline int SymmetricIndex( const int& i1 , const int& i2 , int& index ); + }; + + + } +} + + +#include "function_data.hpp" + +#endif // FUNCTION_DATA_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/function_data.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/function_data.hpp new file mode 100755 index 0000000..8fa4cdc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/function_data.hpp @@ -0,0 +1,423 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +////////////////// +// FunctionData // +////////////////// + +namespace pcl +{ + namespace poisson + { + + template + FunctionData::FunctionData(void) + { + dotTable=dDotTable=d2DotTable=NULL; + valueTables=dValueTables=NULL; + res=0; + } + + template + FunctionData::~FunctionData(void) + { + if(res) + { + if( dotTable) delete[] dotTable; + if( dDotTable) delete[] dDotTable; + if(d2DotTable) delete[] d2DotTable; + if( valueTables) delete[] valueTables; + if(dValueTables) delete[] dValueTables; + } + dotTable=dDotTable=d2DotTable=NULL; + valueTables=dValueTables=NULL; + res=0; + } + + template +#if BOUNDARY_CONDITIONS + void FunctionData::set( const int& maxDepth , const PPolynomial& F , const int& normalize , bool useDotRatios , bool reflectBoundary ) +#else // !BOUNDARY_CONDITIONS + void FunctionData::set(const int& maxDepth,const PPolynomial& F,const int& normalize , bool useDotRatios ) +#endif // BOUNDARY_CONDITIONS + { + this->normalize = normalize; + this->useDotRatios = useDotRatios; +#if BOUNDARY_CONDITIONS + this->reflectBoundary = reflectBoundary; +#endif // BOUNDARY_CONDITIONS + + depth = maxDepth; + res = BinaryNode::CumulativeCenterCount( depth ); + res2 = (1<<(depth+1))+1; + baseFunctions = new PPolynomial[res]; + // Scale the function so that it has: + // 0] Value 1 at 0 + // 1] Integral equal to 1 + // 2] Square integral equal to 1 + switch( normalize ) + { + case 2: + baseFunction=F/sqrt((F*F).integral(F.polys[0].start,F.polys[F.polyCount-1].start)); + break; + case 1: + baseFunction=F/F.integral(F.polys[0].start,F.polys[F.polyCount-1].start); + break; + default: + baseFunction=F/F(0); + } + dBaseFunction = baseFunction.derivative(); +#if BOUNDARY_CONDITIONS + leftBaseFunction = baseFunction + baseFunction.shift( -1 ); + rightBaseFunction = baseFunction + baseFunction.shift( 1 ); + dLeftBaseFunction = leftBaseFunction.derivative(); + dRightBaseFunction = rightBaseFunction.derivative(); +#endif // BOUNDARY_CONDITIONS + double c1,w1; + for( int i=0 ; i::CenterAndWidth( i , c1 , w1 ); +#if BOUNDARY_CONDITIONS + if( reflectBoundary ) + { + int d , off; + BinaryNode< double >::DepthAndOffset( i , d , off ); + if ( off==0 ) baseFunctions[i] = leftBaseFunction.scale( w1 ).shift( c1 ); + else if( off==((1< + void FunctionData::setDotTables( const int& flags ) + { + clearDotTables( flags ); + int size; + size = ( res*res + res )>>1; + if( flags & DOT_FLAG ) + { + dotTable = new Real[size]; + memset( dotTable , 0 , sizeof(Real)*size ); + } + if( flags & D_DOT_FLAG ) + { + dDotTable = new Real[size]; + memset( dDotTable , 0 , sizeof(Real)*size ); + } + if( flags & D2_DOT_FLAG ) + { + d2DotTable = new Real[size]; + memset( d2DotTable , 0 , sizeof(Real)*size ); + } + double t1 , t2; + t1 = baseFunction.polys[0].start; + t2 = baseFunction.polys[baseFunction.polyCount-1].start; + for( int i=0 ; i::CenterAndWidth( i , c1 , w1 ); +#if BOUNDARY_CONDITIONS + int d1 , d2 , off1 , off2; + BinaryNode< double >::DepthAndOffset( i , d1 , off1 ); + int boundary1 = 0; + if ( reflectBoundary && off1==0 ) boundary1 = -1; + else if( reflectBoundary && off1==( (1<::CenterAndWidth( j , c2 , w2 ); +#if BOUNDARY_CONDITIONS + BinaryNode< double >::DepthAndOffset( j , d2 , off2 ); + int boundary2 = 0; + if ( reflectBoundary && off2==0 ) boundary2 = -1; + else if( reflectBoundary && off2==( (1<1 ) start = 1; + if( end <0 ) end = 0; + if( end >1 ) end = 1; + } +#endif // BOUNDARY_CONDITIONS + + if( start< start1 ) start = start1; + if( end > end1 ) end = end1; + if( start>= end ) continue; + +#if BOUNDARY_CONDITIONS + Real dot = dotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); +#else // !BOUNDARY_CONDITIONS + Real dot = dotProduct( c1 , w1 , c2 , w2 ); +#endif // BOUNDARY_CONDITIONS + if( fabs(dot)<1e-15 ) continue; + if( flags & DOT_FLAG ) dotTable[idx]=dot; + if( useDotRatios ) + { +#if BOUNDARY_CONDITIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot; + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ) / dot; +#else // !BOUNDARY_CONDITIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = -dDotProduct(c1,w1,c2,w2)/dot; + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2)/dot; +#endif // BOUNDARY_CONDITIONS + } + else + { +#if BOUNDARY_CONDITIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct( c1 , w1 , c2 , w2 , boundary1 , boundary2 ); +#else // !BOUNDARY_CONDTIONS + if( flags & D_DOT_FLAG ) dDotTable[idx] = dDotProduct(c1,w1,c2,w2); + if( flags & D2_DOT_FLAG ) d2DotTable[idx] = d2DotProduct(c1,w1,c2,w2); +#endif // BOUNDARY_CONDITIONS + } + } + } + } + template + void FunctionData::clearDotTables( const int& flags ) + { + if((flags & DOT_FLAG) && dotTable) + { + delete[] dotTable; + dotTable=NULL; + } + if((flags & D_DOT_FLAG) && dDotTable) + { + delete[] dDotTable; + dDotTable=NULL; + } + if((flags & D2_DOT_FLAG) && d2DotTable) + { + delete[] d2DotTable; + d2DotTable=NULL; + } + } + template + void FunctionData::setValueTables( const int& flags , const double& smooth ) + { + clearValueTables(); + if( flags & VALUE_FLAG ) valueTables = new Real[res*res2]; + if( flags & D_VALUE_FLAG ) dValueTables = new Real[res*res2]; + PPolynomial function; + PPolynomial dFunction; + for( int i=0 ; i0) + { + function=baseFunctions[i].MovingAverage(smooth); + dFunction=baseFunctions[i].derivative().MovingAverage(smooth); + } + else + { + function=baseFunctions[i]; + dFunction=baseFunctions[i].derivative(); + } + for( int j=0 ; j + void FunctionData::setValueTables(const int& flags,const double& valueSmooth,const double& normalSmooth){ + clearValueTables(); + if(flags & VALUE_FLAG){ valueTables=new Real[res*res2];} + if(flags & D_VALUE_FLAG){dValueTables=new Real[res*res2];} + PPolynomial function; + PPolynomial dFunction; + for(int i=0;i0) { function=baseFunctions[i].MovingAverage(valueSmooth);} + else { function=baseFunctions[i];} + if(normalSmooth>0) {dFunction=baseFunctions[i].derivative().MovingAverage(normalSmooth);} + else {dFunction=baseFunctions[i].derivative();} + + for(int j=0;j + void FunctionData::clearValueTables(void){ + if( valueTables){delete[] valueTables;} + if(dValueTables){delete[] dValueTables;} + valueTables=dValueTables=NULL; + } + +#if BOUNDARY_CONDITIONS + template + Real FunctionData::dotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const + { + const PPolynomial< Degree > *b1 , *b2; + if ( boundary1==-1 ) b1 = & leftBaseFunction; + else if( boundary1== 0 ) b1 = & baseFunction; + else if( boundary1== 1 ) b1 = &rightBaseFunction; + if ( boundary2==-1 ) b2 = & leftBaseFunction; + else if( boundary2== 0 ) b2 = & baseFunction; + else if( boundary2== 1 ) b2 = &rightBaseFunction; + double r=fabs( baseFunction.polys[0].start ); + switch( normalize ) + { + case 2: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2)); + case 1: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2)); + default: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1); + } + } + template + Real FunctionData::dDotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const + { + const PPolynomial< Degree-1 > *b1; + const PPolynomial< Degree > *b2; + if ( boundary1==-1 ) b1 = & dLeftBaseFunction; + else if( boundary1== 0 ) b1 = & dBaseFunction; + else if( boundary1== 1 ) b1 = &dRightBaseFunction; + if ( boundary2==-1 ) b2 = & leftBaseFunction; + else if( boundary2== 0 ) b2 = & baseFunction; + else if( boundary2== 1 ) b2 = & rightBaseFunction; + double r=fabs(baseFunction.polys[0].start); + switch(normalize){ + case 2: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2)); + case 1: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2)); + default: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)); + } + } + template + Real FunctionData::d2DotProduct( const double& center1 , const double& width1 , const double& center2 , const double& width2 , int boundary1 , int boundary2 ) const + { + const PPolynomial< Degree-1 > *b1 , *b2; + if ( boundary1==-1 ) b1 = & dLeftBaseFunction; + else if( boundary1== 0 ) b1 = & dBaseFunction; + else if( boundary1== 1 ) b1 = &dRightBaseFunction; + if ( boundary2==-1 ) b2 = & dLeftBaseFunction; + else if( boundary2== 0 ) b2 = & dBaseFunction; + else if( boundary2== 1 ) b2 = &dRightBaseFunction; + double r=fabs(baseFunction.polys[0].start); + switch( normalize ) + { + case 2: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2)); + case 1: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2)); + default: + return Real(((*b1)*b2->scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2); + } + } +#else // !BOUNDARY_CONDITIONS + template + Real FunctionData::dotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ + double r=fabs(baseFunction.polys[0].start); + switch( normalize ) + { + case 2: + return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/sqrt(width1*width2)); + case 1: + return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1/(width1*width2)); + default: + return Real((baseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)*width1); + } + } + template + Real FunctionData::dDotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ + double r=fabs(baseFunction.polys[0].start); + switch(normalize){ + case 2: + return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/sqrt(width1*width2)); + case 1: + return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/(width1*width2)); + default: + return Real((dBaseFunction*baseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)); + } + } + template + Real FunctionData::d2DotProduct(const double& center1,const double& width1,const double& center2,const double& width2) const{ + double r=fabs(baseFunction.polys[0].start); + switch(normalize){ + case 2: + return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/sqrt(width1*width2)); + case 1: + return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2/(width1*width2)); + default: + return Real((dBaseFunction*dBaseFunction.scale(width2/width1).shift((center2-center1)/width1)).integral(-2*r,2*r)/width2); + } + } +#endif // BOUNDARY_CONDITIONS + template + inline int FunctionData::SymmetricIndex( const int& i1 , const int& i2 ) + { + if( i1>i2 ) return ((i1*i1+i1)>>1)+i2; + else return ((i2*i2+i2)>>1)+i1; + } + template + inline int FunctionData::SymmetricIndex( const int& i1 , const int& i2 , int& index ) + { + if( i1>1)+i1; + return 1; + } + else{ + index = ((i1*i1+i1)>>1)+i2; + return 0; + } + } + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/geometry.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/geometry.h new file mode 100755 index 0000000..9f7882d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/geometry.h @@ -0,0 +1,333 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef GEOMETRY_INCLUDED +#define GEOMETRY_INCLUDED + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include +#include +#include + +namespace pcl +{ + namespace poisson + { + + template + Real Random(void); + + template< class Real > + struct Point3D + { + Real coords[3]; + Point3D( void ) { coords[0] = coords[1] = coords[2] = Real(0); } + inline Real& operator[] ( int i ) { return coords[i]; } + inline const Real& operator[] ( int i ) const { return coords[i]; } + inline Point3D& operator += ( Point3D p ){ coords[0] += p.coords[0] , coords[1] += p.coords[1] , coords[2] += p.coords[2] ; return *this; } + inline Point3D& operator -= ( Point3D p ){ coords[0] -= p.coords[0] , coords[1] -= p.coords[1] , coords[2] -= p.coords[2] ; return *this; } + inline Point3D& operator *= ( Real r ){ coords[0] *= r , coords[1] *= r , coords[2] *= r ; return *this; } + inline Point3D& operator /= ( Real r ){ coords[0] /= r , coords[1] /= r , coords[2] /= r ; return *this; } + inline Point3D operator + ( Point3D p ) const { Point3D q ; q.coords[0] = coords[0] + p.coords[0] , q.coords[1] = coords[1] + p.coords[1] , q.coords[2] = coords[2] + p.coords[2] ; return q; } + inline Point3D operator - ( Point3D p ) const { Point3D q ; q.coords[0] = coords[0] - p.coords[0] , q.coords[1] = coords[1] - p.coords[1] , q.coords[2] = coords[2] - p.coords[2] ; return q; } + inline Point3D operator * ( Real r ) const { Point3D q ; q.coords[0] = coords[0] * r , q.coords[1] = coords[1] * r , q.coords[2] = coords[2] * r ; return q; } + inline Point3D operator / ( Real r ) const { return (*this) * ( Real(1.)/r ); } + }; + + template + Point3D RandomBallPoint(void); + + template + Point3D RandomSpherePoint(void); + + template + double Length(const Point3D& p); + + template + double SquareLength(const Point3D& p); + + template + double Distance(const Point3D& p1,const Point3D& p2); + + template + double SquareDistance(const Point3D& p1,const Point3D& p2); + + template + void CrossProduct(const Point3D& p1,const Point3D& p2,Point3D& p); + + class Edge + { + public: + double p[2][2]; + double Length(void) const + { + double d[2]; + d[0]=p[0][0]-p[1][0]; + d[1]=p[0][1]-p[1][1]; + + return sqrt(d[0]*d[0]+d[1]*d[1]); + } + }; + class Triangle + { + public: + double p[3][3]; + double Area(void) const + { + double v1[3] , v2[3] , v[3]; + for( int d=0 ; d<3 ; d++ ) + { + v1[d] = p[1][d] - p[0][d]; + v2[d] = p[2][d] - p[0][d]; + } + v[0] = v1[1]*v2[2] - v1[2]*v2[1]; + v[1] = -v1[0]*v2[2] + v1[2]*v2[0]; + v[2] = v1[0]*v2[1] - v1[1]*v2[0]; + return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] ) / 2; + } + double AspectRatio(void) const + { + double d=0; + int i,j; + for(i=0;i<3;i++){ + for(i=0;i<3;i++) + for(j=0;j<3;j++){d+=(p[(i+1)%3][j]-p[i][j])*(p[(i+1)%3][j]-p[i][j]);} + } + return Area()/d; + } + + }; + class PCL_EXPORTS CoredPointIndex + { + public: + int index; + char inCore; + + int operator == (const CoredPointIndex& cpi) const {return (index==cpi.index) && (inCore==cpi.inCore);}; + int operator != (const CoredPointIndex& cpi) const {return (index!=cpi.index) || (inCore!=cpi.inCore);}; + }; + class EdgeIndex{ + public: + int idx[2]; + }; + class CoredEdgeIndex{ + public: + CoredPointIndex idx[2]; + }; + class TriangleIndex{ + public: + int idx[3]; + }; + + class TriangulationEdge + { + public: + TriangulationEdge(void); + int pIndex[2]; + int tIndex[2]; + }; + + class TriangulationTriangle + { + public: + TriangulationTriangle(void); + int eIndex[3]; + }; + + template + class Triangulation + { + public: + + std::vector > points; + std::vector edges; + std::vector triangles; + + int factor( int tIndex,int& p1,int& p2,int& p3); + double area(void); + double area( int tIndex ); + double area( int p1 , int p2 , int p3 ); + int flipMinimize( int eIndex); + int addTriangle( int p1 , int p2 , int p3 ); + + protected: + std::unordered_map edgeMap; + static long long EdgeIndex( int p1 , int p2 ); + double area(const Triangle& t); + }; + + + template + void EdgeCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector >* normals); + template + void TriangleCollapse(const Real& edgeRatio,std::vector& triangles,std::vector >& positions,std::vector >* normals); + + struct CoredVertexIndex + { + int idx; + bool inCore; + }; + class PCL_EXPORTS CoredMeshData + { + public: + std::vector > inCorePoints; + virtual void resetIterator( void ) = 0; + + virtual int addOutOfCorePoint( const Point3D& p ) = 0; + virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0; + + virtual int nextOutOfCorePoint( Point3D& p )=0; + virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0; + + virtual int outOfCorePointCount(void)=0; + virtual int polygonCount( void ) = 0; + }; + // Stores the iso-span of each vertex, rather than it's position + class PCL_EXPORTS CoredMeshData2 + { + public: + struct Vertex + { + Point3D< float > start , end; + float value; + Vertex( void ) { ; } + Vertex( Point3D< float > s , Point3D< float > e , float v ) { start = s , end = e , value = v; } + Vertex( Point3D< float > s , Point3D< float > e , Point3D< float > p ) + { + start = s , end = e; + // < p , e-s > = < s + v*(e-s) , e-s > + // < p , e-s > - < s , e-s > = v || e-s || ^2 + // v = < p-s , e-s > / || e-s ||^2 + Point3D< float > p1 = p-s , p2 = e-s; + value = ( p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2] ) / ( p2[0] * p2[0] + p2[1] * p2[1] + p2[2] * p2[2] ); + } + }; + std::vector< Vertex > inCorePoints; + virtual void resetIterator( void ) = 0; + + virtual int addOutOfCorePoint( const Vertex& v ) = 0; + virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0; + + virtual int nextOutOfCorePoint( Vertex& v ) = 0; + virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0; + + virtual int outOfCorePointCount( void )=0; + virtual int polygonCount( void ) = 0; + }; + + class PCL_EXPORTS CoredVectorMeshData : public CoredMeshData + { + std::vector > oocPoints; + std::vector< std::vector< int > > polygons; + int polygonIndex; + int oocPointIndex; + public: + CoredVectorMeshData(void); + + void resetIterator(void); + + int addOutOfCorePoint( const Point3D& p ); + int addPolygon( const std::vector< CoredVertexIndex >& vertices ); + + int nextOutOfCorePoint( Point3D& p ); + int nextPolygon( std::vector< CoredVertexIndex >& vertices ); + + int outOfCorePointCount(void); + int polygonCount( void ); + }; + class PCL_EXPORTS CoredVectorMeshData2 : public CoredMeshData2 + { + std::vector< CoredMeshData2::Vertex > oocPoints; + std::vector< std::vector< int > > polygons; + int polygonIndex; + int oocPointIndex; + public: + CoredVectorMeshData2( void ); + + void resetIterator(void); + + int addOutOfCorePoint( const CoredMeshData2::Vertex& v ); + int addPolygon( const std::vector< CoredVertexIndex >& vertices ); + + int nextOutOfCorePoint( CoredMeshData2::Vertex& v ); + int nextPolygon( std::vector< CoredVertexIndex >& vertices ); + + int outOfCorePointCount( void ); + int polygonCount( void ); + }; + class CoredFileMeshData : public CoredMeshData + { + FILE *oocPointFile , *polygonFile; + int oocPoints , polygons; + public: + CoredFileMeshData(void); + ~CoredFileMeshData(void); + + void resetIterator(void); + + int addOutOfCorePoint(const Point3D& p); + int addPolygon( const std::vector< CoredVertexIndex >& vertices ); + + int nextOutOfCorePoint(Point3D& p); + int nextPolygon( std::vector< CoredVertexIndex >& vertices ); + + int outOfCorePointCount(void); + int polygonCount( void ); + }; + class CoredFileMeshData2 : public CoredMeshData2 + { + FILE *oocPointFile , *polygonFile; + int oocPoints , polygons; + public: + CoredFileMeshData2( void ); + ~CoredFileMeshData2( void ); + + void resetIterator( void ); + + int addOutOfCorePoint( const CoredMeshData2::Vertex& v ); + int addPolygon( const std::vector< CoredVertexIndex >& vertices ); + + int nextOutOfCorePoint( CoredMeshData2::Vertex& v ); + int nextPolygon( std::vector< CoredVertexIndex >& vertices ); + + int outOfCorePointCount( void ); + int polygonCount( void ); + }; + } +} + +#include "geometry.hpp" + + + + +#endif // GEOMETRY_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/geometry.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/geometry.hpp new file mode 100755 index 0000000..9dfa260 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/geometry.hpp @@ -0,0 +1,431 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#include + +namespace pcl +{ + namespace poisson + { + + + template + Real Random(void){return Real(rand())/RAND_MAX;} + + template + Point3D RandomBallPoint(void){ + Point3D p; + while(1){ + p.coords[0]=Real(1.0-2.0*Random()); + p.coords[1]=Real(1.0-2.0*Random()); + p.coords[2]=Real(1.0-2.0*Random()); + double l=SquareLength(p); + if(l<=1){return p;} + } + } + template + Point3D RandomSpherePoint(void){ + Point3D p=RandomBallPoint(); + Real l=Real(Length(p)); + p.coords[0]/=l; + p.coords[1]/=l; + p.coords[2]/=l; + return p; + } + + template + double SquareLength(const Point3D& p){return p.coords[0]*p.coords[0]+p.coords[1]*p.coords[1]+p.coords[2]*p.coords[2];} + + template + double Length(const Point3D& p){return sqrt(SquareLength(p));} + + template + double SquareDistance(const Point3D& p1,const Point3D& p2){ + return (p1.coords[0]-p2.coords[0])*(p1.coords[0]-p2.coords[0])+(p1.coords[1]-p2.coords[1])*(p1.coords[1]-p2.coords[1])+(p1.coords[2]-p2.coords[2])*(p1.coords[2]-p2.coords[2]); + } + + template + double Distance(const Point3D& p1,const Point3D& p2){return sqrt(SquareDistance(p1,p2));} + + template + void CrossProduct(const Point3D& p1,const Point3D& p2,Point3D& p){ + p.coords[0]= p1.coords[1]*p2.coords[2]-p1.coords[2]*p2.coords[1]; + p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0]; + p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0]; + } + template + void EdgeCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector< Point3D >* normals){ + int i,j,*remapTable,*pointCount,idx[3]; + Point3D p[3],q[2],c; + double d[3],a; + double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle + + remapTable=new int[positions.size()]; + pointCount=new int[positions.size()]; + for(i=0;i=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] a*Ratio){ + // Find the smallest edge + j=0; + if(d[1]=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] + void TriangleCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector< Point3D >* normals){ + int i,j,*remapTable,*pointCount,idx[3]; + Point3D p[3],q[2],c; + double d[3],a; + double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle + + remapTable=new int[positions.size()]; + pointCount=new int[positions.size()]; + for(i=0;i=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] a*Ratio){ + // Find the smallest edge + j=0; + if(d[1]=0;i--){ + for(j=0;j<3;j++){ + idx[j]=triangles[i].idx[j]; + while(remapTable[idx[j]] + long long Triangulation::EdgeIndex( int p1 , int p2 ) + { + if(p1>p2) {return ((long long)(p1)<<32) | ((long long)(p2));} + else {return ((long long)(p2)<<32) | ((long long)(p1));} + } + + template + int Triangulation::factor(int tIndex,int& p1,int& p2,int & p3){ + if(triangles[tIndex].eIndex[0]<0 || triangles[tIndex].eIndex[1]<0 || triangles[tIndex].eIndex[2]<0){return 0;} + if(edges[triangles[tIndex].eIndex[0]].tIndex[0]==tIndex){p1=edges[triangles[tIndex].eIndex[0]].pIndex[0];} + else {p1=edges[triangles[tIndex].eIndex[0]].pIndex[1];} + if(edges[triangles[tIndex].eIndex[1]].tIndex[0]==tIndex){p2=edges[triangles[tIndex].eIndex[1]].pIndex[0];} + else {p2=edges[triangles[tIndex].eIndex[1]].pIndex[1];} + if(edges[triangles[tIndex].eIndex[2]].tIndex[0]==tIndex){p3=edges[triangles[tIndex].eIndex[2]].pIndex[0];} + else {p3=edges[triangles[tIndex].eIndex[2]].pIndex[1];} + return 1; + } + template + double Triangulation::area(int p1,int p2,int p3){ + Point3D q1,q2,q; + for(int i=0;i<3;i++){ + q1.coords[i]=points[p2].coords[i]-points[p1].coords[i]; + q2.coords[i]=points[p3].coords[i]-points[p1].coords[i]; + } + CrossProduct(q1,q2,q); + return Length(q); + } + template + double Triangulation::area(int tIndex){ + int p1,p2,p3; + factor(tIndex,p1,p2,p3); + return area(p1,p2,p3); + } + template + double Triangulation::area(void){ + double a=0; + for(int i=0;i + int Triangulation::addTriangle(int p1,int p2,int p3){ + int tIdx,eIdx,p[3]; + p[0]=p1; + p[1]=p2; + p[2]=p3; + triangles.push_back(TriangulationTriangle()); + tIdx=int(triangles.size())-1; + + for(int i=0;i<3;i++) + { + long long e = EdgeIndex(p[i],p[(i+1)%3]); + if(edgeMap.count(e) == 0) + { + TriangulationEdge edge; + edge.pIndex[0]=p[i]; + edge.pIndex[1]=p[(i+1)%3]; + edges.push_back(edge); + eIdx=int(edges.size())-1; + edgeMap[e]=eIdx; + edges[eIdx].tIndex[0]=tIdx; + } + else{ + eIdx=edgeMap[e]; + if(edges[eIdx].pIndex[0]==p[i]){ + if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;} + else{PCL_DEBUG("Edge Triangle in use 1\n");return 0;} + } + else{ + if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;} + else{PCL_DEBUG("Edge Triangle in use 2\n");return 0;} + } + + } + triangles[tIdx].eIndex[i]=eIdx; + } + return tIdx; + } + template + int Triangulation::flipMinimize(int eIndex){ + double oldArea,newArea; + int oldP[3],oldQ[3],newP[3],newQ[3]; + TriangulationEdge newEdge; + + if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){return 0;} + + if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){return 0;} + if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){return 0;} + + oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]); + int idxP,idxQ; + for(idxP=0;idxP<3;idxP++){ + int i; + for(i=0;i<3;i++){if(oldP[idxP]==oldQ[i]){break;}} + if(i==3){break;} + } + for(idxQ=0;idxQ<3;idxQ++){ + int i; + for(i=0;i<3;i++){if(oldP[i]==oldQ[idxQ]){break;}} + if(i==3){break;} + } + if(idxP==3 || idxQ==3){return 0;} + newP[0]=oldP[idxP]; + newP[1]=oldP[(idxP+1)%3]; + newP[2]=oldQ[idxQ]; + newQ[0]=oldQ[idxQ]; + newQ[1]=oldP[(idxP+2)%3]; + newQ[2]=oldP[idxP]; + + newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]); + if(oldArea<=newArea){return 0;} + + // Remove the entry in the hash_table for the old edge + edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1])); + // Set the new edge so that the zero-side is newQ + edges[eIndex].pIndex[0]=newP[0]; + edges[eIndex].pIndex[1]=newQ[0]; + // Insert the entry into the hash_table for the new edge + edgeMap[EdgeIndex(newP[0],newQ[0])]=eIndex; + // Update the triangle information + for(int i=0;i<3;i++){ + int idx; + idx=edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])]; + triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx; + if(idx!=eIndex){ + if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];} + if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];} + } + + idx=edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])]; + triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx; + if(idx!=eIndex){ + if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];} + if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];} + } + } + return 1; + } + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h new file mode 100755 index 0000000..26fedac --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/marching_cubes_poisson.h @@ -0,0 +1,144 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef MARCHING_CUBES_INCLUDED +#define MARCHING_CUBES_INCLUDED +#include +#include +#include "geometry.h" + + +namespace pcl +{ + namespace poisson + { + + class PCL_EXPORTS Square + { + public: + enum { CORNERS=4,EDGES=4,NEIGHBORS=4 }; + static int CornerIndex (int x,int y); + static int AntipodalCornerIndex(int idx); + static void FactorCornerIndex (int idx,int& x,int& y); + static int EdgeIndex (int orientation,int i); + static void FactorEdgeIndex (int idx,int& orientation,int& i); + + static int ReflectCornerIndex (int idx,int edgeIndex); + static int ReflectEdgeIndex (int idx,int edgeIndex); + + static void EdgeCorners(int idx,int& c1,int &c2); + }; + + class PCL_EXPORTS Cube + { + public: + enum { CORNERS=8,EDGES=12,NEIGHBORS=6 }; + + static int CornerIndex (int x,int y,int z); + static void FactorCornerIndex (int idx,int& x,int& y,int& z); + static int EdgeIndex (int orientation,int i,int j); + static void FactorEdgeIndex (int idx,int& orientation,int& i,int &j); + static int FaceIndex (int dir,int offSet); + static int FaceIndex (int x,int y,int z); + static void FactorFaceIndex (int idx,int& x,int &y,int& z); + static void FactorFaceIndex (int idx,int& dir,int& offSet); + + static int AntipodalCornerIndex (int idx); + static int FaceReflectCornerIndex (int idx,int faceIndex); + static int FaceReflectEdgeIndex (int idx,int faceIndex); + static int FaceReflectFaceIndex (int idx,int faceIndex); + static int EdgeReflectCornerIndex (int idx,int edgeIndex); + static int EdgeReflectEdgeIndex (int edgeIndex); + + static int FaceAdjacentToEdges (int eIndex1,int eIndex2); + static void FacesAdjacentToEdge (int eIndex,int& f1Index,int& f2Index); + + static void EdgeCorners(int idx,int& c1,int &c2); + static void FaceCorners(int idx,int& c1,int &c2,int& c3,int& c4); + }; + + class PCL_EXPORTS MarchingSquares + { + static double Interpolate(double v1,double v2); + static void SetVertex(int e,const double values[Square::CORNERS],double iso); + public: + enum { MAX_EDGES=2 }; + static const int* edgeMask(); + static int edges(int i, int j); + static double& vertexList(int i, int j); + + static int GetIndex(const double values[Square::CORNERS],double iso); + static int IsAmbiguous(const double v[Square::CORNERS],double isoValue); + static int AddEdges(const double v[Square::CORNERS],double isoValue,Edge* edges); + static int AddEdgeIndices(const double v[Square::CORNERS],double isoValue,int* edges); + }; + + class PCL_EXPORTS MarchingCubes + { + static void SetVertex(int e,const double values[Cube::CORNERS],double iso); + static int GetFaceIndex(const double values[Cube::CORNERS],double iso,int faceIndex); + + static void SetVertex(int e,const float values[Cube::CORNERS],float iso); + static int GetFaceIndex(const float values[Cube::CORNERS],float iso,int faceIndex); + + static int GetFaceIndex(int mcIndex,int faceIndex); + public: + static double Interpolate(double v1,double v2); + static float Interpolate(float v1,float v2); + enum { MAX_TRIANGLES=5 }; + static const int* edgeMask(); + static int triangles(int i, int j); + static const int* cornerMap(); + static double& vertexList(int i, int j); + + static int AddTriangleIndices(int mcIndex,int* triangles); + + static int GetIndex(const double values[Cube::CORNERS],double iso); + static int IsAmbiguous(const double v[Cube::CORNERS],double isoValue,int faceIndex); + static int HasRoots(const double v[Cube::CORNERS],double isoValue); + static int HasRoots(const double v[Cube::CORNERS],double isoValue,int faceIndex); + static int AddTriangles(const double v[Cube::CORNERS],double isoValue,Triangle* triangles); + static int AddTriangleIndices(const double v[Cube::CORNERS],double isoValue,int* triangles); + + static int GetIndex(const float values[Cube::CORNERS],float iso); + static int IsAmbiguous(const float v[Cube::CORNERS],float isoValue,int faceIndex); + static int HasRoots(const float v[Cube::CORNERS],float isoValue); + static int HasRoots(const float v[Cube::CORNERS],float isoValue,int faceIndex); + static int AddTriangles(const float v[Cube::CORNERS],float isoValue,Triangle* triangles); + static int AddTriangleIndices(const float v[Cube::CORNERS],float isoValue,int* triangles); + + static int IsAmbiguous(int mcIndex,int faceIndex); + static int HasRoots(int mcIndex); + static int HasFaceRoots(int mcIndex,int faceIndex); + static int HasEdgeRoots(int mcIndex,int edgeIndex); + }; + } +} + + +#endif //MARCHING_CUBES_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/mat.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/mat.h new file mode 100755 index 0000000..7ea5856 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/mat.h @@ -0,0 +1,59 @@ +/* +Copyright (c) 2007, Michael Kazhdan +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ +#ifndef MAT_INCLUDED +#define MAT_INCLUDED +#include "geometry.h" + + +namespace pcl +{ + namespace poisson + { + + template + class MinimalAreaTriangulation + { + Real* bestTriangulation; + int* midPoint; + Real GetArea(const std::size_t& i,const std::size_t& j,const std::vector >& vertices); + void GetTriangulation(const std::size_t& i,const std::size_t& j,const std::vector >& vertices,std::vector& triangles); + public: + MinimalAreaTriangulation(void); + ~MinimalAreaTriangulation(void); + Real GetArea(const std::vector >& vertices); + void GetTriangulation(const std::vector >& vertices,std::vector& triangles); + }; + + } +} + +#include "mat.hpp" + + + +#endif // MAT_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/mat.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/mat.hpp new file mode 100755 index 0000000..1a93693 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/mat.hpp @@ -0,0 +1,223 @@ +/* +Copyright (c) 2007, Michael Kazhdan +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ +////////////////////////////// +// MinimalAreaTriangulation // +////////////////////////////// + +namespace pcl +{ + namespace poisson + { + + template + MinimalAreaTriangulation::MinimalAreaTriangulation(void) + { + bestTriangulation=NULL; + midPoint=NULL; + } + template + MinimalAreaTriangulation::~MinimalAreaTriangulation(void) + { + if(bestTriangulation) + delete[] bestTriangulation; + bestTriangulation=NULL; + if(midPoint) + delete[] midPoint; + midPoint=NULL; + } + template + void MinimalAreaTriangulation::GetTriangulation(const std::vector >& vertices,std::vector& triangles) + { + if(vertices.size()==3) + { + triangles.resize(1); + triangles[0].idx[0]=0; + triangles[0].idx[1]=1; + triangles[0].idx[2]=2; + return; + } + else if(vertices.size()==4) + { + TriangleIndex tIndex[2][2]; + Real area[2]; + + area[0]=area[1]=0; + triangles.resize(2); + + tIndex[0][0].idx[0]=0; + tIndex[0][0].idx[1]=1; + tIndex[0][0].idx[2]=2; + tIndex[0][1].idx[0]=2; + tIndex[0][1].idx[1]=3; + tIndex[0][1].idx[2]=0; + + tIndex[1][0].idx[0]=0; + tIndex[1][0].idx[1]=1; + tIndex[1][0].idx[2]=3; + tIndex[1][1].idx[0]=3; + tIndex[1][1].idx[1]=1; + tIndex[1][1].idx[2]=2; + + Point3D n,p1,p2; + for(int i=0;i<2;i++) + for(int j=0;j<2;j++) + { + p1=vertices[tIndex[i][j].idx[1]]-vertices[tIndex[i][j].idx[0]]; + p2=vertices[tIndex[i][j].idx[2]]-vertices[tIndex[i][j].idx[0]]; + CrossProduct(p1,p2,n); + area[i] += Real( Length(n) ); + } + if(area[0]>area[1]) + { + triangles[0]=tIndex[1][0]; + triangles[1]=tIndex[1][1]; + } + else + { + triangles[0]=tIndex[0][0]; + triangles[1]=tIndex[0][1]; + } + return; + } + if(bestTriangulation) + delete[] bestTriangulation; + if(midPoint) + delete[] midPoint; + bestTriangulation=NULL; + midPoint=NULL; + std::size_t eCount=vertices.size(); + bestTriangulation=new Real[eCount*eCount]; + midPoint=new int[eCount*eCount]; + for(std::size_t i=0;i + Real MinimalAreaTriangulation::GetArea(const std::vector >& vertices) + { + if(bestTriangulation) + delete[] bestTriangulation; + if(midPoint) + delete[] midPoint; + bestTriangulation=NULL; + midPoint=NULL; + int eCount=vertices.size(); + bestTriangulation=new double[eCount*eCount]; + midPoint=new int[eCount*eCount]; + for(int i=0;i + void MinimalAreaTriangulation::GetTriangulation(const std::size_t& i,const std::size_t& j,const std::vector >& vertices,std::vector& triangles) + { + TriangleIndex tIndex; + std::size_t eCount=vertices.size(); + std::size_t ii=i; + if(i=ii) + return; + ii=midPoint[i*eCount+j]; + if(ii>=0) + { + tIndex.idx[0] = int( i ); + tIndex.idx[1] = int( j ); + tIndex.idx[2] = int( ii ); + triangles.push_back(tIndex); + GetTriangulation(i,ii,vertices,triangles); + GetTriangulation(ii,j,vertices,triangles); + } + } + + template + Real MinimalAreaTriangulation::GetArea(const std::size_t& i,const std::size_t& j,const std::vector >& vertices) + { + Real a=FLT_MAX,temp; + std::size_t eCount=vertices.size(); + std::size_t idx=i*eCount+j; + std::size_t ii=i; + if(i=ii) + { + bestTriangulation[idx]=0; + return 0; + } + if(midPoint[idx]!=-1) + return bestTriangulation[idx]; + int mid=-1; + for(std::size_t r=j+1;r p,p1,p2; + p1=vertices[i]-vertices[rr]; + p2=vertices[j]-vertices[rr]; + CrossProduct(p1,p2,p); + temp = Real( Length(p) ); + if(bestTriangulation[idx1]>=0) + { + temp+=bestTriangulation[idx1]; + if(temp>a) + continue; + if(bestTriangulation[idx2]>0) + temp+=bestTriangulation[idx2]; + else + temp+=GetArea(rr,j,vertices); + } + else + { + if(bestTriangulation[idx2]>=0) + temp+=bestTriangulation[idx2]; + else + temp+=GetArea(rr,j,vertices); + if(temp>a) + continue; + temp+=GetArea(i,rr,vertices); + } + + if(temp +// b_1[i] = < \nabla B_i(p) , V(p) > +// 2] Formulate this as a Poisson equation: +// \sum_i x_i \Delta B_i(p) = \nabla \cdot V(p) +// which is solved by the system A_2x = b_2 where: +// A_2[i,j] = - < \Delta B_i(p) , B_j(p) > +// b_2[i] = - < B_i(p) , \nabla \cdot V(p) > +// Although the two system matrices should be the same (assuming that the B_i satisfy dirichlet/neumann boundary conditions) +// the constraint vectors can differ when V does not satisfy the Neumann boundary conditions: +// A_1[i,j] = \int_R < \nabla B_i(p) , \nabla B_j(p) > +// = \int_R [ \nabla \cdot ( B_i(p) \nabla B_j(p) ) - B_i(p) \Delta B_j(p) ] +// = \int_dR < N(p) , B_i(p) \nabla B_j(p) > + A_2[i,j] +// and the first integral is zero if either f_i is zero on the boundary dR or the derivative of B_i across the boundary is zero. +// However, for the constraints we have: +// b_1(i) = \int_R < \nabla B_i(p) , V(p) > +// = \int_R [ \nabla \cdot ( B_i(p) V(p) ) - B_i(p) \nabla \cdot V(p) ] +// = \int_dR < N(p) , B_i(p) V(p) > + b_2[i] +// In particular, this implies that if the B_i satisfy the Neumann boundary conditions (rather than Dirichlet), +// and V is not zero across the boundary, then the two constraints are different. +// Forcing the < V(p) , N(p) > = 0 on the boundary, by killing off the component of the vector-field in the normal direction +// (FORCE_NEUMANN_FIELD), makes the two systems equal, and the value of this flag should be immaterial. +// Note that under interpretation 1, we have: +// \sum_i b_1(i) = < \nabla \sum_ i B_i(p) , V(p) > = 0 +// because the B_i's sum to one. However, in general, we could have +// \sum_i b_2(i) \neq 0. +// This could cause trouble because the constant functions are in the kernel of the matrix A, so CG will misbehave if the constraint +// has a non-zero DC term. (Again, forcing < V(p) , N(p) > = 0 along the boundary resolves this problem.) + +#define FORCE_NEUMANN_FIELD 1 // This flag forces the normal component across the boundary of the integration domain to be zero. +// This should be enabled if GRADIENT_DOMAIN_SOLUTION is not, so that CG doesn't run into trouble. + + +#include + +#include "bspline_data.h" + + +namespace pcl +{ + namespace poisson + { + + typedef float Real; + typedef float BSplineDataReal; + typedef pcl::poisson::OctNode< class TreeNodeData , Real > TreeOctNode; + + + + class RootInfo + { + public: + const TreeOctNode* node; + int edgeIndex; + long long key; + }; + + class VertexData + { + public: + static long long EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth , int index[DIMENSION] ); + static long long EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth ); + static long long FaceIndex( const TreeOctNode* node , int fIndex , int maxDepth,int index[DIMENSION] ); + static long long FaceIndex( const TreeOctNode* node , int fIndex , int maxDepth ); + static long long CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int index[DIMENSION] ); + static long long CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int index[DIMENSION] ); + static long long CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth ); + static long long CenterIndex( int depth , const int offSet[DIMENSION] , int maxDepth , int index[DIMENSION] ); + static long long CenterIndex( const TreeOctNode* node , int maxDepth , int index[DIMENSION] ); + static long long CenterIndex( const TreeOctNode* node , int maxDepth ); + static long long CornerIndexKey( const int index[DIMENSION] ); + }; + class SortedTreeNodes + { + public: + TreeOctNode** treeNodes; + int *nodeCount; + int maxDepth; + SortedTreeNodes( void ); + ~SortedTreeNodes( void ); + void set( TreeOctNode& root ); + struct CornerIndices + { + int idx[pcl::poisson::Cube::CORNERS]; + CornerIndices( void ) { memset( idx , -1 , sizeof( int ) * pcl::poisson::Cube::CORNERS ); } + int& operator[] ( int i ) { return idx[i]; } + const int& operator[] ( int i ) const { return idx[i]; } + }; + struct CornerTableData + { + CornerTableData( void ) { cCount=0; } + ~CornerTableData( void ) { clear(); } + void clear( void ) { cTable.clear() ; cCount = 0; } + CornerIndices& operator[] ( const TreeOctNode* node ); + const CornerIndices& operator[] ( const TreeOctNode* node ) const; + CornerIndices& cornerIndices( const TreeOctNode* node ); + const CornerIndices& cornerIndices( const TreeOctNode* node ) const; + int cCount; + std::vector< CornerIndices > cTable; + std::vector< int > offsets; + }; + void setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int depth , int threads ) const; + void setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int threads ) const { setCornerTable( cData , rootNode , maxDepth-1 , threads ); } + void setCornerTable( CornerTableData& cData , int threads ) const { setCornerTable( cData , treeNodes[0] , maxDepth-1 , threads ); } + int getMaxCornerCount( const TreeOctNode* rootNode , int depth , int maxDepth , int threads ) const ; + struct EdgeIndices + { + int idx[pcl::poisson::Cube::EDGES]; + EdgeIndices( void ) { memset( idx , -1 , sizeof( int ) * pcl::poisson::Cube::EDGES ); } + int& operator[] ( int i ) { return idx[i]; } + const int& operator[] ( int i ) const { return idx[i]; } + }; + struct EdgeTableData + { + EdgeTableData( void ) { eCount=0; } + ~EdgeTableData( void ) { clear(); } + void clear( void ) { eTable.clear() , eCount=0; } + EdgeIndices& operator[] ( const TreeOctNode* node ); + const EdgeIndices& operator[] ( const TreeOctNode* node ) const; + EdgeIndices& edgeIndices( const TreeOctNode* node ); + const EdgeIndices& edgeIndices( const TreeOctNode* node ) const; + int eCount; + std::vector< EdgeIndices > eTable; + std::vector< int > offsets; + }; + void setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int depth , int threads ); + void setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int threads ) { setEdgeTable( eData , rootNode , maxDepth-1 , threads ); } + void setEdgeTable( EdgeTableData& eData , int threads ) { setEdgeTable( eData , treeNodes[0] , maxDepth-1 , threads ); } + int getMaxEdgeCount( const TreeOctNode* rootNode , int depth , int threads ) const ; + }; + + class TreeNodeData + { + public: + static int UseIndex; + int nodeIndex; + union + { + int mcIndex; + struct + { + Real centerWeightContribution; + int normalIndex; + }; + }; + Real constraint , solution; + int pointIndex; + + TreeNodeData(void); + ~TreeNodeData(void); + }; + + template< int Degree > + class Octree + { + SortedTreeNodes _sNodes; + int _minDepth; + bool _constrainValues; + std::vector< int > _pointCount; + struct PointData + { + pcl::poisson::Point3D< Real > position; + Real weight; + Real value; + PointData( pcl::poisson::Point3D< Real > p , Real w , Real v=0 ) { position = p , weight = w , value = v; } + }; + std::vector< PointData > _points; + TreeOctNode::NeighborKey3 neighborKey; + TreeOctNode::ConstNeighborKey3 neighborKey2; + + Real radius; + int width; + Real GetLaplacian( const int index[DIMENSION] ) const; + // Note that this is a slight misnomer. We're only taking the diveregence/Laplacian in the weak sense, so there is a change of sign. + Real GetLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 ) const; + Real GetDivergence( const TreeOctNode* node1 , const TreeOctNode* node2 , const pcl::poisson::Point3D& normal1 ) const; + Real GetDivergenceMinusLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 , Real value1 , const pcl::poisson::Point3D& normal1 ) const; + struct PointInfo + { + float splineValues[3][3]; + float weightedValue; + }; + Real GetValue( const PointInfo points[3][3][3] , const bool hasPoints[3][3] , const int d[3] ) const; + + class AdjacencyCountFunction + { + public: + int adjacencyCount; + void Function(const TreeOctNode* node1,const TreeOctNode* node2); + }; + class AdjacencySetFunction{ + public: + int *adjacencies,adjacencyCount; + void Function(const TreeOctNode* node1,const TreeOctNode* node2); + }; + + class RefineFunction{ + public: + int depth; + void Function(TreeOctNode* node1,const TreeOctNode* node2); + }; + class FaceEdgesFunction + { + public: + int fIndex , maxDepth; + std::vector< std::pair< RootInfo , RootInfo > >* edges; + std::unordered_map< long long , std::pair< RootInfo , int > >* vertexCount; + void Function( const TreeOctNode* node1 , const TreeOctNode* node2 ); + }; + + int SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* subConstraints , bool showResidual , int minIters , double accuracy ); + int SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* subConstraints , int startingDepth , bool showResidual , int minIters , double accuracy ); + + void SetMatrixRowBounds( const TreeOctNode* node , int rDepth , const int rOff[3] , int& xStart , int& xEnd , int& yStart , int& yEnd , int& zStart , int& zEnd ) const; + int GetMatrixRowSize( const TreeOctNode::Neighbors5& neighbors5 ) const; + int GetMatrixRowSize( const TreeOctNode::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const; + int SetMatrixRow( const TreeOctNode::Neighbors5& neighbors5 , pcl::poisson::MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const; + int SetMatrixRow( const TreeOctNode::Neighbors5& neighbors5 , pcl::poisson::MatrixEntry< float >* row , int offset , const double stencil[5][5][5] , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const; + void SetDivergenceStencil( int depth , pcl::poisson::Point3D< double > *stencil , bool scatter ) const; + void SetLaplacianStencil( int depth , double stencil[5][5][5] ) const; + template< class C , int N > struct Stencil{ C values[N][N][N]; }; + void SetLaplacianStencils( int depth , Stencil< double , 5 > stencil[2][2][2] ) const; + void SetDivergenceStencils( int depth , Stencil< pcl::poisson::Point3D< double > , 5 > stencil[2][2][2] , bool scatter ) const; + void SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const; + + static void UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ ); + void UpdateConstraintsFromCoarser( const TreeOctNode::NeighborKey5& neighborKey5 , TreeOctNode* node , Real* metSolution , const Stencil< double , 5 >& stencil ) const; + void SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution ); + Real WeightedCoarserFunctionValue( const TreeOctNode::NeighborKey3& neighborKey3 , const TreeOctNode* node , Real* metSolution ) const; + void UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , pcl::poisson::Vector< Real >& solution ) const; + void DownSampleFinerConstraints( int depth , SortedTreeNodes& sNodes ) const; + template< class C > void DownSample( int depth , const SortedTreeNodes& sNodes , C* constraints ) const; + template< class C > void UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const; + int GetFixedDepthLaplacian( pcl::poisson::SparseSymmetricMatrix& matrix , int depth , const SortedTreeNodes& sNodes , Real* subConstraints ); + int GetRestrictedFixedDepthLaplacian( pcl::poisson::SparseSymmetricMatrix& matrix , int depth , const int* entries , int entryCount , const TreeOctNode* rNode, Real radius , const SortedTreeNodes& sNodes , Real* subConstraints ); + + void SetIsoCorners( Real isoValue , TreeOctNode* leaf , SortedTreeNodes::CornerTableData& cData , char* valuesSet , Real* values , TreeOctNode::ConstNeighborKey3& nKey , const Real* metSolution , const Stencil< double , 3 > stencil1[8] , const Stencil< double , 3 > stencil2[8][8] ); + static int IsBoundaryFace( const TreeOctNode* node , int faceIndex , int subdivideDepth ); + static int IsBoundaryEdge( const TreeOctNode* node , int edgeIndex , int subdivideDepth ); + static int IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subidivideDepth ); + + // For computing the iso-surface there is a lot of re-computation of information across shared geometry. + // For function values we don't care so much. + // For edges we need to be careful so that the mesh remains water-tight + struct RootData : public SortedTreeNodes::CornerTableData , public SortedTreeNodes::EdgeTableData + { + // Edge to iso-vertex map + std::unordered_map< long long , int > boundaryRoots; + // Vertex to ( value , normal ) map + std::unordered_map< long long , std::pair< Real , pcl::poisson::Point3D< Real > > > *boundaryValues; + int* interiorRoots; + Real *cornerValues; + pcl::poisson::Point3D< Real >* cornerNormals; + char *cornerValuesSet , *cornerNormalsSet , *edgesSet; + }; + + int SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , pcl::poisson::CoredMeshData* mesh , int nonLinearFit ); + int SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData , + std::vector< pcl::poisson::Point3D< float > >* interiorPositions , pcl::poisson::CoredMeshData* mesh , const Real* metSolution , int nonLinearFit ); +#if MISHA_DEBUG + int GetMCIsoTriangles( TreeOctNode* node , pcl::poisson::CoredMeshData* mesh , RootData& rootData , + std::vector< pcl::poisson::Point3D< float > >* interiorPositions , int offSet , int sDepth , bool polygonMesh , std::vector< pcl::poisson::Point3D< float > >* barycenters ); + static int AddTriangles( pcl::poisson::CoredMeshData* mesh , std::vector& edges , std::vector >* interiorPositions , int offSet , bool polygonMesh , std::vector< pcl::poisson::Point3D< float > >* barycenters ); +#else // !MISHA_DEBUG + int GetMCIsoTriangles( TreeOctNode* node , pcl::poisson::CoredMeshData* mesh , RootData& rootData , + std::vector< pcl::poisson::Point3D< float > >* interiorPositions , int offSet , int sDepth , bool addBarycenter , bool polygonMesh ); + static int AddTriangles( pcl::poisson::CoredMeshData* mesh , std::vector& edges , std::vector >* interiorPositions , int offSet , bool addBarycenter , bool polygonMesh ); +#endif // MISHA_DEBUG + + + void GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges ); + static int GetEdgeLoops( std::vector< std::pair< RootInfo , RootInfo > >& edges , std::vector< std::vector< std::pair< RootInfo , RootInfo > > >& loops); + static int InteriorFaceRootCount( const TreeOctNode* node , const int &faceIndex , int maxDepth ); + static int EdgeRootCount( const TreeOctNode* node , int edgeIndex , int maxDepth ); + static void GetRootSpan( const RootInfo& ri , pcl::poisson::Point3D< float >& start , pcl::poisson::Point3D< float >& end ); + int GetRoot( const RootInfo& ri , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , pcl::poisson::Point3D & position , RootData& rootData , int sDepth , const Real* metSolution , int nonLinearFit ); + static int GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri ); + static int GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth , RootInfo& ri ); + static int GetRootIndex( const RootInfo& ri , RootData& rootData , pcl::poisson::CoredPointIndex& index ); + static int GetRootPair( const RootInfo& root , int maxDepth , RootInfo& pair ); + + int NonLinearUpdateWeightContribution(TreeOctNode* node,const pcl::poisson::Point3D& position,Real weight=Real(1.0)); + Real NonLinearGetSampleWeight(TreeOctNode* node,const pcl::poisson::Point3D& position); + void NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const pcl::poisson::Point3D& position,Real samplesPerNode,Real& depth,Real& weight); + int NonLinearSplatOrientedPoint(TreeOctNode* node,const pcl::poisson::Point3D& point,const pcl::poisson::Point3D& normal); + Real NonLinearSplatOrientedPoint(const pcl::poisson::Point3D& point,const pcl::poisson::Point3D& normal , int kernelDepth , Real samplesPerNode , int minDepth , int maxDepth); + + int HasNormals(TreeOctNode* node,Real epsilon); + Real getCornerValue( const TreeOctNode::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution ); + pcl::poisson::Point3D< Real > getCornerNormal( const TreeOctNode::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution ); + Real getCornerValue( const TreeOctNode::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution , const double stencil1[3][3][3] , const double stencil2[3][3][3] ); + Real getCenterValue( const TreeOctNode::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node ); + public: + int threads; + static double maxMemoryUsage; + static double MemoryUsage( void ); + std::vector< pcl::poisson::Point3D >* normals; + Real postNormalSmooth; + TreeOctNode tree; + pcl::poisson::BSplineData fData; + Octree( void ); + + void setBSplineData( int maxDepth , Real normalSmooth=-1 , bool reflectBoundary=false ); + void finalize( void ); + void RefineBoundary( int subdivisionDepth ); + Real* GetWeightGrid( int& res , int depth=-1 ); + Real* GetSolutionGrid( int& res , float isoValue=0.f , int depth=-1 ); + + template int + setTree(typename pcl::PointCloud::ConstPtr input_ , int maxDepth , int minDepth , + int kernelDepth , Real samplesPerNode , Real scaleFactor , Point3D& center , Real& scale , + int useConfidence , Real constraintWeight , bool adaptiveWeights ); + + void SetLaplacianConstraints(void); + void ClipTree(void); + int LaplacianMatrixIteration( int subdivideDepth , bool showResidual , int minIters , double accuracy ); + + Real GetIsoValue(void); + void GetMCIsoTriangles( Real isoValue , int subdivideDepth , pcl::poisson::CoredMeshData* mesh , int fullDepthIso=0 , int nonLinearFit=1 , bool addBarycenter=false , bool polygonMesh=false ); + }; + + + } +} + + + + +#include "multi_grid_octree_data.hpp" +#endif // MULTI_GRID_OCTREE_DATA_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp new file mode 100755 index 0000000..ec13a00 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp @@ -0,0 +1,3928 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#include + +#include "poisson_exceptions.h" +#include "octree_poisson.h" +#include "mat.h" + +#if defined WIN32 || defined _WIN32 + #if !defined __MINGW32__ + #include + #endif +#endif + + +#define ITERATION_POWER 1.0/3 +#define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12 +#define SPLAT_ORDER 2 + +namespace pcl +{ + namespace poisson + { + + const Real MATRIX_ENTRY_EPSILON = Real(0); + const Real EPSILON=Real(1e-6); + const Real ROUND_EPS=Real(1e-5); + + void atomicOr(volatile int& dest, int value) + { +#if defined _WIN32 && !defined __MINGW32__ + #if defined (_M_IX86) + _InterlockedOr( (long volatile*)&dest, value ); + #else + InterlockedOr( (long volatile*)&dest , value ); + #endif +#else // !_WIN32 || __MINGW32__ + #pragma omp atomic + dest |= value; +#endif // _WIN32 && !__MINGW32__ + } + + + ///////////////////// + // SortedTreeNodes // + ///////////////////// + SortedTreeNodes::SortedTreeNodes(void) + { + nodeCount=NULL; + treeNodes=NULL; + maxDepth=0; + } + SortedTreeNodes::~SortedTreeNodes(void){ + if( nodeCount ) delete[] nodeCount; + if( treeNodes ) delete[] treeNodes; + nodeCount = NULL; + treeNodes = NULL; + } + + void SortedTreeNodes::set( TreeOctNode& root ) + { + if( nodeCount ) delete[] nodeCount; + if( treeNodes ) delete[] treeNodes; + maxDepth = root.maxDepth()+1; + nodeCount = new int[ maxDepth+1 ]; + treeNodes = new TreeOctNode*[ root.nodes() ]; + + nodeCount[0] = 0 , nodeCount[1] = 1; + treeNodes[0] = &root; + for( int d=1 ; dchildren ) for( int c=0 ; c<8 ; c++ ) treeNodes[ nodeCount[d+1]++ ] = temp->children + c; + } + } + for( int i=0 ; inodeData.nodeIndex = i; + } + SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::operator[] ( const TreeOctNode* node ) { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::operator[] ( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::cornerIndices( const TreeOctNode* node ) { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::cornerIndices( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + void SortedTreeNodes::setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int maxDepth , int threads ) const + { + if( threads<=0 ) threads = 1; + // The vector of per-depth node spans + std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) ); + int minDepth , off[3]; + rootNode->depthAndOffset( minDepth , off ); + cData.offsets.resize( this->maxDepth , -1 ); + int nodeCount = 0; + { + int start=rootNode->nodeData.nodeIndex , end=start; + for( int d=minDepth ; d<=maxDepth ; d++ ) + { + spans[d] = std::pair< int , int >( start , end+1 ); + cData.offsets[d] = nodeCount - spans[d].first; + nodeCount += spans[d].second - spans[d].first; + if( dchildren ) start++; + while( end> start && !treeNodes[end ]->children ) end--; + if( start==end && !treeNodes[start]->children ) break; + start = treeNodes[start]->children[0].nodeData.nodeIndex; + end = treeNodes[end ]->children[7].nodeData.nodeIndex; + } + } + } + cData.cTable.resize( nodeCount ); + std::vector< int > count( threads ); +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tchildren ) continue; + const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , minDepth ); + for( int c=0 ; cchildren ) ) + { + int _d , _off[3]; + neighbors.neighbors[xx][yy][zz]->depthAndOffset( _d , _off ); + _off[0] >>= (d-minDepth) , _off[1] >>= (d-minDepth) , _off[2] >>= (d-minDepth); + if( _off[0]==off[0] && _off[1]==off[1] && _off[2]==off[2] ) + { + cornerOwner = false; + break; + } + else fprintf( stderr , "[WARNING] How did we leave the subtree?\n" ); + } + } + if( cornerOwner ) + { + const TreeOctNode* n = node; + int d = n->depth(); + do + { + const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.neighbors[d]; + // Set all the corner indices at the current depth + for( int cc=0 ; ccparent->children+c) ) break; + n = n->parent; + d--; + } + while( 1 ); + count[t]++; + } + } + } + } + } + cData.cCount = 0; + std::vector< int > offsets( threads+1 ); + offsets[0] = 0; + for (int t = 0; t < threads; t++) + { + cData.cCount += count[t]; + offsets[t + 1] = offsets[t] + count[t]; + } + + unsigned int paralellExceptionCount = 0; +#pragma omp parallel for num_threads( threads ) + for (int t = 0; t < threads; t++) + { + for (int d = minDepth; d <= maxDepth; d++) + { + int start = spans[d].first, end = spans[d].second, width = end - start; + for (int i = start + (width*t) / threads; i < start + (width*(t + 1)) / threads; i++) + { + for (int c = 0; c < Cube::CORNERS; c++) + { + int& idx = cData[ treeNodes[i] ][c]; + if ( idx<0 ) + { +#pragma omp critical + { + // found unindexed corner + ++paralellExceptionCount; + } + } + else + { + int div = idx / ( nodeCount*Cube::CORNERS ); + int rem = idx % ( nodeCount*Cube::CORNERS ); + idx = rem + offsets[div]; + } + } + } + } + } + + if (paralellExceptionCount > 0) + POISSON_THROW_EXCEPTION (pcl::poisson::PoissonOpenMPException, "Found " << paralellExceptionCount << " unindexed corner nodes during openMP loop execution."); + } + int SortedTreeNodes::getMaxCornerCount( const TreeOctNode* rootNode , int depth , int maxDepth , int threads ) const + { + if( threads<=0 ) threads = 1; + int res = 1< > cornerCount( threads ); + for( int t=0 ; t& _cornerCount = cornerCount[t]; + TreeOctNode::ConstNeighborKey3 neighborKey; + neighborKey.set( maxDepth ); + int start = nodeCount[depth] , end = nodeCount[maxDepth+1] , range = end-start; + for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ ) + { + TreeOctNode* node = treeNodes[start+i]; + int d , off[3]; + node->depthAndOffset( d , off ); + if( dchildren ) continue; + + const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth ); + for( int c=0 ; cchildren ) ) + { + cornerOwner = false; + break; + } + } + if( cornerOwner ) _cornerCount[ ( ( off[0]>>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++; + } + } + } + int maxCount = 0; + for( int i=0 ; i( maxCount , c ); + } + return maxCount; + } + SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::operator[] ( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::operator[] ( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::edgeIndices( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::edgeIndices( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } + void SortedTreeNodes::setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int maxDepth , int threads ) + { + if( threads<=0 ) threads = 1; + std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) ); + + int minDepth , off[3]; + rootNode->depthAndOffset( minDepth , off ); + eData.offsets.resize( this->maxDepth , -1 ); + int nodeCount = 0; + { + int start=rootNode->nodeData.nodeIndex , end=start; + for( int d=minDepth ; d<=maxDepth ; d++ ) + { + spans[d] = std::pair< int , int >( start , end+1 ); + eData.offsets[d] = nodeCount - spans[d].first; + nodeCount += spans[d].second - spans[d].first; + if( dchildren ) start++; + while( end> start && !treeNodes[end ]->children ) end--; + if( start==end && !treeNodes[start]->children ) break; + start = treeNodes[start]->children[0].nodeData.nodeIndex; + end = treeNodes[end ]->children[7].nodeData.nodeIndex; + } + } + } + eData.eTable.resize( nodeCount ); + std::vector< int > count( threads ); +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; t offsets( threads+1 ); + offsets[0] = 0; + for (int t = 0; t < threads; t++) + { + eData.eCount += count[t]; + offsets[t + 1] = offsets[t] + count[t]; + } + + unsigned int paralellExceptionCount = 0; +#pragma omp parallel for num_threads( threads ) + for (int t = 0; t < threads; t++) + { + for (int d = minDepth; d <= maxDepth; d++) + { + int start = spans[d].first, end = spans[d].second, width = end - start; + for (int i = start + (width*t) / threads; i < start + (width*(t + 1)) / threads; i++) + { + for (int e = 0; e < Cube::EDGES; e++) + { + int& idx = eData[treeNodes[i]][e]; + if (idx < 0) + { +#pragma omp critical + { + // found unindexed edge + ++paralellExceptionCount; + } + } + else + { + int div = idx / ( nodeCount*Cube::EDGES ); + int rem = idx % ( nodeCount*Cube::EDGES ); + idx = rem + offsets[div]; + } + } + } + } + } + + if(paralellExceptionCount > 0) + POISSON_THROW_EXCEPTION (pcl::poisson::PoissonOpenMPException, "Found " << paralellExceptionCount << " unindexed edges during openMP loop execution."); + + } + int SortedTreeNodes::getMaxEdgeCount( const TreeOctNode* rootNode , int depth , int threads ) const + { + if( threads<=0 ) threads = 1; + int res = 1< > edgeCount( threads ); + for( int t=0 ; t& _edgeCount = edgeCount[t]; + TreeOctNode::ConstNeighborKey3 neighborKey; + neighborKey.set( maxDepth-1 ); + int start = nodeCount[depth] , end = nodeCount[maxDepth] , range = end-start; + for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ ) + { + TreeOctNode* node = treeNodes[start+i]; + const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth ); + int d , off[3]; + node->depthAndOffset( d , off ); + + for( int e=0 ; e>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++; + } + } + } + int maxCount = 0; + for( int i=0 ; i( maxCount , c ); + } + return maxCount; + } + + + + ////////////////// + // TreeNodeData // + ////////////////// + int TreeNodeData::UseIndex=1; + TreeNodeData::TreeNodeData( void ) + { + if( UseIndex ) + { + nodeIndex = -1; + centerWeightContribution=0; + } + else mcIndex=0; + normalIndex = -1; + constraint = solution = 0; + pointIndex = -1; + } + TreeNodeData::~TreeNodeData( void ) { } + + + //////////// + // Octree // + //////////// + template + double Octree::maxMemoryUsage=0; + + + + template + double Octree::MemoryUsage(void){ + double mem = 0;//double( MemoryInfo::Usage() ) / (1<<20); + if(mem>maxMemoryUsage){maxMemoryUsage=mem;} + return mem; + } + + template + Octree::Octree(void) + { + threads = 1; + radius = 0; + width = 0; + postNormalSmooth = 0; + _constrainValues = false; + } + + template + int Octree::NonLinearSplatOrientedPoint( TreeOctNode* node , const Point3D& position , const Point3D& normal ) + { + double x , dxdy , dxdydz , dx[DIMENSION][SPLAT_ORDER+1]; + int off[3]; + TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node ); + double width; + Point3D center; + Real w; + node->centerAndWidth( center , w ); + width=w; + for( int i=0 ; i<3 ; i++ ) + { +#if SPLAT_ORDER==2 + off[i] = 0; + x = ( center[i] - position[i] - width ) / width; + dx[i][0] = 1.125+1.500*x+0.500*x*x; + x = ( center[i] - position[i] ) / width; + dx[i][1] = 0.750 - x*x; + + dx[i][2] = 1. - dx[i][1] - dx[i][0]; +#elif SPLAT_ORDER==1 + x = ( position[i] - center[i] ) / width; + if( x<0 ) + { + off[i] = 0; + dx[i][0] = -x; + } + else + { + off[i] = 1; + dx[i][0] = 1. - x; + } + dx[i][1] = 1. - dx[i][0]; +#elif SPLAT_ORDER==0 + off[i] = 1; + dx[i][0] = 1.; +#else +# error Splat order not supported +#endif // SPLAT_ORDER + } + for( int i=off[0] ; i<=off[0]+SPLAT_ORDER ; i++ ) for( int j=off[1] ; j<=off[1]+SPLAT_ORDER ; j++ ) + { + dxdy = dx[0][i] * dx[1][j]; + for( int k=off[2] ; k<=off[2]+SPLAT_ORDER ; k++ ) + if( neighbors.neighbors[i][j][k] ) + { + dxdydz = dxdy * dx[2][k]; + TreeOctNode* _node = neighbors.neighbors[i][j][k]; + int idx =_node->nodeData.normalIndex; + if( idx<0 ) + { + Point3D n; + n[0] = n[1] = n[2] = 0; + _node->nodeData.nodeIndex = 0; + idx = _node->nodeData.normalIndex = int(normals->size()); + normals->push_back(n); + } + (*normals)[idx] += normal * Real( dxdydz ); + } + } + return 0; + } + template + Real Octree::NonLinearSplatOrientedPoint( const Point3D& position , const Point3D& normal , int splatDepth , Real samplesPerNode , + int minDepth , int maxDepth ) + { + double dx; + Point3D n; + TreeOctNode* temp; + int cnt=0; + double width; + Point3D< Real > myCenter; + Real myWidth; + myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5); + myWidth = Real(1.0); + + temp = &tree; + while( temp->depth()children ) + { + fprintf( stderr , "Octree::NonLinearSplatOrientedPoint error\n" ); + return -1; + } + int cIndex=TreeOctNode::CornerIndex(myCenter,position); + temp=&temp->children[cIndex]; + myWidth/=2; + if(cIndex&1) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if(cIndex&2) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if(cIndex&4) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + Real alpha,newDepth; + NonLinearGetSampleDepthAndWeight( temp , position , samplesPerNode , newDepth , alpha ); + + if( newDepthmaxDepth ) newDepth=Real(maxDepth); + int topDepth=int(ceil(newDepth)); + + dx = 1.0-(topDepth-newDepth); + if( topDepth<=minDepth ) + { + topDepth=minDepth; + dx=1; + } + else if( topDepth>maxDepth ) + { + topDepth=maxDepth; + dx=1; + } + while( temp->depth()>topDepth ) temp=temp->parent; + while( temp->depth()children) temp->initChildren(); + int cIndex=TreeOctNode::CornerIndex(myCenter,position); + temp=&temp->children[cIndex]; + myWidth/=2; + if(cIndex&1) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if(cIndex&2) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if(cIndex&4) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + } + width = 1.0 / ( 1<depth() ); + n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx ); + NonLinearSplatOrientedPoint( temp , position , n ); + if( fabs(1.0-dx) > EPSILON ) + { + dx = Real(1.0-dx); + temp = temp->parent; + width = 1.0 / ( 1<depth() ); + + n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx ); + NonLinearSplatOrientedPoint( temp , position , n ); + } + return alpha; + } + template + void Octree::NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const Point3D& position,Real samplesPerNode,Real& depth,Real& weight){ + TreeOctNode* temp=node; + weight = Real(1.0)/NonLinearGetSampleWeight(temp,position); + if( weight>=samplesPerNode ) depth=Real( temp->depth() + log( weight / samplesPerNode ) / log(double(1<<(DIMENSION-1))) ); + else + { + Real oldAlpha,newAlpha; + oldAlpha=newAlpha=weight; + while( newAlphaparent ) + { + temp=temp->parent; + oldAlpha=newAlpha; + newAlpha=Real(1.0)/NonLinearGetSampleWeight(temp,position); + } + depth = Real( temp->depth() + log( newAlpha / samplesPerNode ) / log( newAlpha / oldAlpha ) ); + } + weight=Real(pow(double(1<<(DIMENSION-1)),-double(depth))); + } + + template + Real Octree::NonLinearGetSampleWeight( TreeOctNode* node , const Point3D& position ) + { + Real weight=0; + double x,dxdy,dx[DIMENSION][3]; + TreeOctNode::Neighbors3& neighbors=neighborKey.setNeighbors( node ); + double width; + Point3D center; + Real w; + node->centerAndWidth(center,w); + width=w; + + for( int i=0 ; inodeData.centerWeightContribution ); + } + return Real( 1.0 / weight ); + } + + template + int Octree::NonLinearUpdateWeightContribution( TreeOctNode* node , const Point3D& position , Real weight ) + { + TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node ); + double x,dxdy,dx[DIMENSION][3]; + double width; + Point3D center; + Real w; + node->centerAndWidth( center , w ); + width=w; + const double SAMPLE_SCALE = 1. / ( 0.125 * 0.125 + 0.75 * 0.75 + 0.125 * 0.125 ); + + for( int i=0 ; inodeData.centerWeightContribution += Real( dxdy * dx[2][k] ); + } + return 0; + } + + template< int Degree > template int + Octree::setTree(typename pcl::PointCloud::ConstPtr input_, int maxDepth , int minDepth , + int kernelDepth , Real samplesPerNode , Real scaleFactor , Point3D& center , Real& scale , + int useConfidence , Real constraintWeight , bool adaptiveWeights ) + { + _minDepth = std::min< int >( std::max< int >( 0 , minDepth ) , maxDepth ); + _constrainValues = (constraintWeight>0); + double pointWeightSum = 0; + Point3D min , max , position , normal , myCenter; + Real myWidth; + int i , cnt=0; + TreeOctNode* temp; + int splatDepth=0; + + TreeNodeData::UseIndex = 1; + neighborKey.set( maxDepth ); + splatDepth = kernelDepth; + if( splatDepth<0 ) splatDepth = 0; + + + tree.setFullDepth( _minDepth ); + // Read through once to get the center and scale + while (cnt != input_->size ()) + { + Point3D< Real > p; + p[0] = input_->points[cnt].x; + p[1] = input_->points[cnt].y; + p[2] = input_->points[cnt].z; + + for (i = 0; i < DIMENSION; i++) + { + if( !cnt || p[i]max[i] ) max[i] = p[i]; + } + cnt++; + } + + scale = std::max< Real >( max[0]-min[0] , std::max< Real >( max[1]-min[1] , max[2]-min[2] ) ); + center = ( max+min ) /2; + + scale *= scaleFactor; + for( i=0 ; i0 ) + { + cnt = 0; + while (cnt != input_->size ()) + { + position[0] = input_->points[cnt].x; + position[1] = input_->points[cnt].y; + position[2] = input_->points[cnt].z; + normal[0] = input_->points[cnt].normal_x; + normal[1] = input_->points[cnt].normal_y; + normal[2] = input_->points[cnt].normal_z; + + for( i=0 ; imyCenter[i]+myWidth/2 ) break; + if( i!=DIMENSION ) continue; + Real weight=Real( 1. ); + if( useConfidence ) weight = Real( Length(normal) ); + temp = &tree; + int d=0; + while( dchildren ) temp->initChildren(); + int cIndex=TreeOctNode::CornerIndex(myCenter,position); + temp=&temp->children[cIndex]; + myWidth/=2; + if(cIndex&1) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if(cIndex&2) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if(cIndex&4) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + d++; + } + NonLinearUpdateWeightContribution( temp , position , weight ); + cnt++; + } + } + + normals = new std::vector< Point3D >(); + cnt=0; + while (cnt != input_->size ()) + { + position[0] = input_->points[cnt].x; + position[1] = input_->points[cnt].y; + position[2] = input_->points[cnt].z; + normal[0] = input_->points[cnt].normal_x; + normal[1] = input_->points[cnt].normal_y; + normal[2] = input_->points[cnt].normal_z; + cnt ++; + for( i=0 ; imyCenter[i]+myWidth/2) break; + if( i!=DIMENSION ) continue; + Real l = Real( Length( normal ) ); + if( l!=l || l<=EPSILON ) continue; + if( !useConfidence ) normal /= l; + + l = Real(1.); + Real pointWeight = Real(1.f); + if( samplesPerNode>0 && splatDepth ) + { + pointWeight = NonLinearSplatOrientedPoint( position , normal , splatDepth , samplesPerNode , _minDepth , maxDepth ); + } + else + { + Real alpha=1; + temp = &tree; + int d=0; + if( splatDepth ) + { + while( dchildren[cIndex]; + myWidth/=2; + if(cIndex&1) myCenter[0]+=myWidth/2; + else myCenter[0]-=myWidth/2; + if(cIndex&2) myCenter[1]+=myWidth/2; + else myCenter[1]-=myWidth/2; + if(cIndex&4) myCenter[2]+=myWidth/2; + else myCenter[2]-=myWidth/2; + d++; + } + alpha = NonLinearGetSampleWeight( temp , position ); + } + for( i=0 ; ichildren){temp->initChildren();} + int cIndex=TreeOctNode::CornerIndex(myCenter,position); + temp=&temp->children[cIndex]; + myWidth/=2; + if(cIndex&1) myCenter[0]+=myWidth/2; + else myCenter[0]-=myWidth/2; + if(cIndex&2) myCenter[1]+=myWidth/2; + else myCenter[1]-=myWidth/2; + if(cIndex&4) myCenter[2]+=myWidth/2; + else myCenter[2]-=myWidth/2; + d++; + } + NonLinearSplatOrientedPoint( temp , position , normal ); + pointWeight = alpha; + } + pointWeight = 1; + pointWeightSum += pointWeight; + if( _constrainValues ) + { + int d = 0; + TreeOctNode* temp = &tree; + myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5); + myWidth = Real(1.0); + while( 1 ) + { + int idx = temp->nodeData.pointIndex; + if( idx==-1 ) + { + Point3D< Real > p; + p[0] = p[1] = p[2] = 0; + idx = int( _points.size() ); + _points.push_back( PointData( position*pointWeight , pointWeight ) ); + temp->nodeData.pointIndex = idx; + } + else + { + _points[idx].weight += pointWeight; + _points[idx].position += position * pointWeight; + } + + int cIndex = TreeOctNode::CornerIndex( myCenter , position ); + if( !temp->children ) break; + temp = &temp->children[cIndex]; + myWidth /= 2; + if( cIndex&1 ) myCenter[0] += myWidth/2; + else myCenter[0] -= myWidth/2; + if( cIndex&2 ) myCenter[1] += myWidth/2; + else myCenter[1] -= myWidth/2; + if( cIndex&4 ) myCenter[2] += myWidth/2; + else myCenter[2] -= myWidth/2; + d++; + } + } + } + + + if( _constrainValues ) + for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode(n) ) + if( n->nodeData.pointIndex!=-1 ) + { + int idx = n->nodeData.pointIndex; + _points[idx].position /= _points[idx].weight; + if( adaptiveWeights ) _points[idx].weight *= (1<d); + else _points[idx].weight *= (1<depthAndOffset( d , off ); + res = 1<nodeData.normalIndex<0 ) continue; + Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex]; + for( int d=0 ; d<3 ; d++ ) if( off[d]==0 || off[d]==res-1 ) normal[d] = 0; + } +#endif // FORCE_NEUMANN_FIELD + _sNodes.set( tree ); + + + return cnt; + } + + + template + void Octree::setBSplineData( int maxDepth , Real normalSmooth , bool reflectBoundary ) + { + radius = 0.5 + 0.5 * Degree; + width=int(double(radius+0.5-EPSILON)*2); + if( normalSmooth>0 ) postNormalSmooth = normalSmooth; + fData.set( maxDepth , true , reflectBoundary ); + } + + template + void Octree::finalize( void ) + { + int maxDepth = tree.maxDepth( ); + TreeOctNode::NeighborKey5 nKey; + nKey.set( maxDepth ); + for( int d=maxDepth ; d>0 ; d-- ) + for( TreeOctNode* node=tree.nextNode() ; node ; node=tree.nextNode( node ) ) + if( node->d==d ) + { + int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5; + int c = int( node - node->parent->children ); + int x , y , z; + Cube::FactorCornerIndex( c , x , y , z ); + if( x ) xStart = 1; + else xEnd = 4; + if( y ) yStart = 1; + else yEnd = 4; + if( z ) zStart = 1; + else zEnd = 4; + nKey.setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd ); + } + _sNodes.set( tree ); + MemoryUsage(); + } + template< int Degree > + Real Octree< Degree >::GetValue( const PointInfo points[3][3][3] , const bool hasPoints[3][3] , const int d[3] ) const + { + double v = 0.; + const int min[] = { std::max( 0 , d[0]+0 ) , std::max( 0 , d[1]+0 ) , std::max( 0 , d[2]+0 ) }; + const int max[] = { std::min( 2 , d[0]+2 ) , std::min( 2 , d[1]+2 ) , std::min( 2 , d[2]+2 ) }; + for( int i=min[0] ; i<=max[0] ; i++ ) for( int j=min[1] ; j<=max[1] ; j++ ) + { + if( !hasPoints[i][j] ) continue; + const PointInfo* pInfo = points[i][j]; + int ii = -d[0]+i; + int jj = -d[1]+j; + for( int k=min[2] ; k<=max[2] ; k++ ) + if( pInfo[k].weightedValue ) + v += pInfo[k].splineValues[0][ii] * pInfo[k].splineValues[1][jj] * pInfo[k].splineValues[2][-d[2]+k]; + } + return Real( v ); + } + template + Real Octree::GetLaplacian( const int idx[DIMENSION] ) const + { + return Real( fData.vvDotTable[idx[0]] * fData.vvDotTable[idx[1]] * fData.vvDotTable[idx[2]] * (fData.ddDotTable[idx[0]]+fData.ddDotTable[idx[1]]+fData.ddDotTable[idx[2]] ) ); + } + template< int Degree > + Real Octree< Degree >::GetLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 ) const + { + int symIndex[] = + { + BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) , + BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) , + BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) + }; + return GetLaplacian( symIndex ); + } + template< int Degree > + Real Octree< Degree >::GetDivergence( const TreeOctNode* node1 , const TreeOctNode* node2 , const Point3D< Real >& normal1 ) const + { + int symIndex[] = + { + BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) , + BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) , + BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) , + }; + int aSymIndex[] = + { + #if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + fData.Index( node2->off[0] , node1->off[0] ) , + fData.Index( node2->off[1] , node1->off[1] ) , + fData.Index( node2->off[2] , node1->off[2] ) + #else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + fData.Index( node1->off[0] , node2->off[0] ) , + fData.Index( node1->off[1] , node2->off[1] ) , + fData.Index( node1->off[2] , node2->off[2] ) + #endif // GRADIENT_DOMAIN_SOLUTION + }; + double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; +#if GRADIENT_DOMAIN_SOLUTION + return Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) ); +#else // !GRADIENT_DOMAIN_SOLUTION + return -Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) ); +#endif // GRADIENT_DOMAIN_SOLUTION + } + template< int Degree > + Real Octree< Degree >::GetDivergenceMinusLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 , Real value1 , const Point3D& normal1 ) const + { + int symIndex[] = + { + BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) , + BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) , + BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) + }; + int aSymIndex[] = + { + #if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + fData.Index( node2->off[0] , node1->off[0] ) , + fData.Index( node2->off[1] , node1->off[1] ) , + fData.Index( node2->off[2] , node1->off[2] ) + #else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + fData.Index( node1->off[0] , node2->off[0] ) , + fData.Index( node1->off[1] , node2->off[1] ) , + fData.Index( node1->off[2] , node2->off[2] ) + #endif // GRADIENT_DOMAIN_SOLUTION + }; + double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; + return Real( dot * + ( + #if GRADIENT_DOMAIN_SOLUTION + - ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1 + + ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) + #else // !GRADIENT_DOMAIN_SOLUTION + - ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1 + - ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) + #endif // GRADIENT_DOMAIN_SOLUTION + ) + ); + } + template< int Degree > + void Octree< Degree >::SetMatrixRowBounds( const TreeOctNode* node , int rDepth , const int rOff[3] , int& xStart , int& xEnd , int& yStart , int& yEnd , int& zStart , int& zEnd ) const + { + xStart = 0 , yStart = 0 , zStart = 0 , xEnd = 5 , yEnd = 5 , zEnd = 5; + int depth , off[3]; + node->depthAndOffset( depth , off ); + int width = 1 << ( depth-rDepth ); + off[0] -= rOff[0] << ( depth-rDepth ) , off[1] -= rOff[1] << ( depth-rDepth ) , off[2] -= rOff[2] << ( depth-rDepth ); + if( off[0]<0 ) xStart = -off[0]; + if( off[1]<0 ) yStart = -off[1]; + if( off[2]<0 ) zStart = -off[2]; + if( off[0]>=width ) xEnd = 4 - ( off[0]-width ); + if( off[1]>=width ) yEnd = 4 - ( off[1]-width ); + if( off[2]>=width ) zEnd = 4 - ( off[2]-width ); + } + template< int Degree > + int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 ) const { return GetMatrixRowSize( neighbors5 , 0 , 5 , 0 , 5 , 0 , 5 ); } + template< int Degree > + int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const + { + int count = 0; + for( int x=xStart ; x<=2 ; x++ ) + for( int y=yStart ; y2 ) continue; + else for( int z=zStart ; z2 ) continue; + else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 ) + count++; + return count; + } + template< int Degree > + int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const + { + return SetMatrixRow( neighbors5 , row , offset , stencil , 0 , 5 , 0 , 5 , 0 , 5 ); + } + template< int Degree > + int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const + { + bool hasPoints[3][3]; + Real diagonal = 0; + PointInfo samples[3][3][3]; + + int count = 0; + const TreeOctNode* node = neighbors5.neighbors[2][2][2]; + int index[] = { int( node->off[0] ) , int( node->off[1] ), int( node->off[2] ) }; + + if( _constrainValues ) + { + int d , idx[3]; + node->depthAndOffset( d , idx ); + idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] ); + idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] ); + idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] ); + for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) + { + hasPoints[j][k] = false; + for( int l=0 ; l<3 ; l++ ) + { + const TreeOctNode* _node = neighbors5.neighbors[j+1][k+1][l+1]; + if( _node && _node->nodeData.pointIndex!=-1 ) + { + const PointData& pData = _points[ _node->nodeData.pointIndex ]; + PointInfo& pointInfo = samples[j][k][l]; + Real weight = pData.weight; + Point3D< Real > p = pData.position; + for( int s=0 ; s<3 ; s++ ) + { + pointInfo.splineValues[0][s] = float( fData.baseBSplines[ idx[0]+j-s][s]( p[0] ) ); + pointInfo.splineValues[1][s] = float( fData.baseBSplines[ idx[1]+k-s][s]( p[1] ) ); + pointInfo.splineValues[2][s] = float( fData.baseBSplines[ idx[2]+l-s][s]( p[2] ) ); + } + float value = pointInfo.splineValues[0][j] * pointInfo.splineValues[1][k] * pointInfo.splineValues[2][l]; + diagonal += value * value * weight; + pointInfo.weightedValue = value * weight; + for( int s=0 ; s<3 ; s++ ) pointInfo.splineValues[0][s] *= pointInfo.weightedValue; + hasPoints[j][k] = true; + } + else samples[j][k][l].weightedValue = 0; + } + } + } + + bool isInterior; + int d , off[3]; + neighbors5.neighbors[2][2][2]->depthAndOffset( d , off ); + int mn = 2 , mx = (1<=mn && off[0]=mn && off[1]=mn && off[2]2 ) continue; + else for( int z=zStart ; z2 ) continue; + else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 ) + { + const TreeOctNode* _node = neighbors5.neighbors[x][y][z]; + int _index[] = { int( _node->off[0] ) , int( _node->off[1] ), int( _node->off[2] ) }; + Real temp; + if( isInterior ) temp = Real( stencil[x][y][z] ); + else temp = GetLaplacian( node , _node ); + if( _constrainValues ) + { + int _d[] = { _index[0]-index[0] , _index[1]-index[1] , _index[2]-index[2] }; + if( x==2 && y==2 && z==2 ) temp += diagonal; + else temp += GetValue( samples , hasPoints , _d ); + } + if( x==2 && y==2 && z==2 ) temp /= 2; + if( fabs(temp)>MATRIX_ENTRY_EPSILON ) + { + row[count].N = _node->nodeData.nodeIndex-offset; + row[count].Value = temp; + count++; + } + } + return count; + } + template< int Degree > + void Octree< Degree >::SetDivergenceStencil( int depth , Point3D< double > *stencil , bool scatter ) const + { + int offset[] = { 2 , 2 , 2 }; + short d , off[3]; + TreeOctNode::Index( depth , offset , d , off ); + int index1[3] , index2[3]; + if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); + else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); + for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) + { + int _offset[] = { x , y , z }; + TreeOctNode::Index( depth , _offset , d , off ); + if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); + else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); + int symIndex[] = + { + BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) , + BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) , + BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) , + }; + int aSymIndex[] = + { + #if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + fData.Index( index1[0] , index2[0] ) , + fData.Index( index1[1] , index2[1] ) , + fData.Index( index1[2] , index2[2] ) + #else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + fData.Index( index2[0] , index1[0] ) , + fData.Index( index2[1] , index1[1] ) , + fData.Index( index2[2] , index1[2] ) + #endif // GRADIENT_DOMAIN_SOLUTION + }; + + double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; +#if GRADIENT_DOMAIN_SOLUTION + Point3D temp; + temp[0] = fData.dvDotTable[aSymIndex[0]] * dot; + temp[1] = fData.dvDotTable[aSymIndex[1]] * dot; + temp[2] = fData.dvDotTable[aSymIndex[2]] * dot; + stencil[25*x + 5*y + z] = temp; + // stencil[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot; + // stencil[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot; + // stencil[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot; +#else // !GRADIENT_DOMAIN_SOLUTION + Point3D temp; + temp[0] = -fData.dvDotTable[aSymIndex[0]] * dot; + temp[1] = -fData.dvDotTable[aSymIndex[1]] * dot; + temp[2] = -fData.dvDotTable[aSymIndex[2]] * dot; + stencil[25*x + 5*y + z] = temp; + // stencil[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot; + // stencil[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot; + // stencil[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot; +#endif // GRADIENT_DOMAIN_SOLUTION + } + } + template< int Degree > + void Octree< Degree >::SetLaplacianStencil( int depth , double stencil[5][5][5] ) const + { + int offset[] = { 2 , 2 , 2 }; + short d , off[3]; + TreeOctNode::Index( depth , offset , d , off ); + int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) }; + for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) + { + int _offset[] = { x , y , z }; + short _d , _off[3]; + TreeOctNode::Index( depth , _offset , _d , _off ); + int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) }; + int symIndex[3]; + symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] ); + symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] ); + symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] ); + stencil[x][y][z] = GetLaplacian( symIndex ); + } + } + template< int Degree > + void Octree< Degree >::SetLaplacianStencils( int depth , Stencil< double , 5 > stencils[2][2][2] ) const + { + if( depth<=1 ) return; + for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) + { + short d , off[3]; + int offset[] = { 4+i , 4+j , 4+k }; + TreeOctNode::Index( depth , offset , d , off ); + int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) }; + for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) + { + int _offset[] = { x , y , z }; + short _d , _off[3]; + TreeOctNode::Index( depth-1 , _offset , _d , _off ); + int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) }; + int symIndex[3]; + symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] ); + symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] ); + symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] ); + stencils[i][j][k].values[x][y][z] = GetLaplacian( symIndex ); + } + } + } + template< int Degree > + void Octree< Degree >::SetDivergenceStencils( int depth , Stencil< Point3D< double > , 5 > stencils[2][2][2] , bool scatter ) const + { + if( depth<=1 ) return; + int index1[3] , index2[3]; + for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) + { + short d , off[3]; + int offset[] = { 4+i , 4+j , 4+k }; + TreeOctNode::Index( depth , offset , d , off ); + if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); + else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); + for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) + { + int _offset[] = { x , y , z }; + TreeOctNode::Index( depth-1 , _offset , d , off ); + if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); + else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); + + int symIndex[] = + { + BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) , + BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) , + BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) , + }; + int aSymIndex[] = + { + #if GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the vector-field with the gradient of the basis function + fData.Index( index1[0] , index2[0] ) , + fData.Index( index1[1] , index2[1] ) , + fData.Index( index1[2] , index2[2] ) + #else // !GRADIENT_DOMAIN_SOLUTION + // Take the dot-product of the divergence of the vector-field with the basis function + fData.Index( index2[0] , index1[0] ) , + fData.Index( index2[1] , index1[1] ) , + fData.Index( index2[2] , index1[2] ) + #endif // GRADIENT_DOMAIN_SOLUTION + }; + double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; +#if GRADIENT_DOMAIN_SOLUTION + stencils[i][j][k].values[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot; + stencils[i][j][k].values[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot; + stencils[i][j][k].values[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot; +#else // !GRADIENT_DOMAIN_SOLUTION + stencils[i][j][k].values[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot; + stencils[i][j][k].values[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot; + stencils[i][j][k].values[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot; +#endif // GRADIENT_DOMAIN_SOLUTION + } + } + } + template< int Degree > + void Octree< Degree >::SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const + { + if( depth>2 ) + { + int idx[3]; + int off[] = { 2 , 2 , 2 }; + for( int c=0 ; c<8 ; c++ ) + { + VertexData::CornerIndex( depth , off , c , fData.depth , idx ); + idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount; + for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ ) + { + short _d , _off[3]; + int _offset[] = { x+1 , y+1 , z+1 }; + TreeOctNode::Index( depth , _offset , _d , _off ); + stencil1[c].values[x][y][z] = fData.valueTables[ idx[0]+int(_off[0]) ] * fData.valueTables[ idx[1]+int(_off[1]) ] * fData.valueTables[ idx[2]+int(_off[2]) ]; + } + } + } + if( depth>3 ) + for( int _c=0 ; _c<8 ; _c++ ) + { + int idx[3]; + int _cx , _cy , _cz; + Cube::FactorCornerIndex( _c , _cx , _cy , _cz ); + int off[] = { 4+_cx , 4+_cy , 4+_cz }; + for( int c=0 ; c<8 ; c++ ) + { + VertexData::CornerIndex( depth , off , c , fData.depth , idx ); + idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount; + for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ ) + { + short _d , _off[3]; + int _offset[] = { x+1 , y+1 , z+1 }; + TreeOctNode::Index( depth-1 , _offset , _d , _off ); + stencil2[_c][c].values[x][y][z] = fData.valueTables[ idx[0]+int(_off[0]) ] * fData.valueTables[ idx[1]+int(_off[1]) ] * fData.valueTables[ idx[2]+int(_off[2]) ]; + } + } + } + } + template< int Degree > + void Octree< Degree >::UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ ) + { + if( node->parent ) + { + int x , y , z , c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + if( x==0 ) endX = 4; + else startX = 1; + if( y==0 ) endY = 4; + else startY = 1; + if( z==0 ) endZ = 4; + else startZ = 1; + } + } + + template< int Degree > + void Octree< Degree >::UpdateConstraintsFromCoarser( const OctNode< TreeNodeData , Real >::NeighborKey5& neighborKey5 , TreeOctNode* node , Real* metSolution , const Stencil< double , 5 >& lapStencil ) const + { + bool isInterior; + { + int d , off[3]; + node->depthAndOffset( d , off ); + int mn = 4 , mx = (1<=mn && off[0]=mn && off[1]=mn && off[2]depth(); + if( depth<=_minDepth ) return; + int i = node->nodeData.nodeIndex; + // Offset the constraints using the solution from lower resolutions. + int startX = 0 , endX = 5 , startY = 0 , endY = 5 , startZ = 0 , endZ = 5; + UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ ); + + const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1]; + for( int x=startX ; xnodeData.nodeIndex>=0 ) + { + const TreeOctNode* _node = neighbors5.neighbors[x][y][z]; + Real _solution = metSolution[ _node->nodeData.nodeIndex ]; + { + if( isInterior ) node->nodeData.constraint -= Real( lapStencil.values[x][y][z] * _solution ); + else node->nodeData.constraint -= GetLaplacian( _node , node ) * _solution; + } + } + if( _constrainValues ) + { + int d , idx[3]; + node->depthAndOffset( d, idx ); + idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] ); + idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] ); + idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] ); + const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth]; + for( int x=1 ; x<4 ; x++ ) for( int y=1 ; y<4 ; y++ ) for( int z=1 ; z<4 ; z++ ) + if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.pointIndex!=-1 ) + { + const PointData& pData = _points[ neighbors5.neighbors[x][y][z]->nodeData.pointIndex ]; + Real pointValue = pData.value; + Point3D< Real > p = pData.position; + node->nodeData.constraint -= + Real( + fData.baseBSplines[idx[0]][x-1]( p[0] ) * + fData.baseBSplines[idx[1]][y-1]( p[1] ) * + fData.baseBSplines[idx[2]][z-1]( p[2] ) * + pointValue + ); + } + } + } + struct UpSampleData + { + int start; + double v[2]; + UpSampleData( void ) { start = 0 , v[0] = v[1] = 0.; } + UpSampleData( int s , double v1 , double v2 ) { start = s , v[0] = v1 , v[1] = v2; } + }; + template< int Degree > + void Octree< Degree >::UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , Vector< Real >& Solution ) const + { + int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; + Solution.Resize( range ); + if( !depth ) return; + else if( depth==1 ) for( int i=start ; inodeData.solution; + else + { + // For every node at the current depth +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tdepthAndOffset( d , off ); + for( int d=0 ; d<3 ; d++ ) + { + if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); + else if( off[d]+1==(1<nodeData.solution * dxyz ); + } + } + } + } + } + } + // Clear the coarser solution + start = sNodes.nodeCount[depth-1] , end = sNodes.nodeCount[depth] , range = end-start; +#pragma omp parallel for num_threads( threads ) + for( int i=start ; inodeData.solution = Real( 0. ); + } + template< int Degree > + void Octree< Degree >::DownSampleFinerConstraints( int depth , SortedTreeNodes& sNodes ) const + { + if( !depth ) return; +#pragma omp parallel for num_threads( threads ) + for( int i=sNodes.nodeCount[depth-1] ; inodeData.constraint = Real( 0 ); + + if( depth==1 ) + { + sNodes.treeNodes[0]->nodeData.constraint = Real( 0 ); + for( int i=sNodes.nodeCount[depth] ; inodeData.constraint += sNodes.treeNodes[i]->nodeData.constraint; + return; + } + std::vector< Vector< double > > constraints( threads ); + for( int t=0 ; tdepthAndOffset( d , off ); + for( int d=0 ; d<3 ; d++ ) + { + if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); + else if( off[d]+1==(1<nodeData.nodeIndex-lStart] += sNodes.treeNodes[i]->nodeData.constraint * dxyz; + } + } + } + } + } +#pragma omp parallel for num_threads( threads ) + for( int i=lStart ; inodeData.constraint += cSum; + } + } + template< int Degree > + template< class C > + void Octree< Degree >::DownSample( int depth , const SortedTreeNodes& sNodes , C* constraints ) const + { + if( depth==0 ) return; + if( depth==1 ) + { + for( int i=sNodes.nodeCount[1] ; i > _constraints( threads ); + for( int t=0 ; tdepthAndOffset( d , off ); + for( int d=0 ; d<3 ; d++ ) + { + if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); + else if( off[d]+1==(1<parent ); + C c = constraints[i]; + for( int ii=0 ; ii<2 ; ii++ ) + { + int _ii = ii + usData[0].start; + C cx = C( c*usData[0].v[ii] ); + for( int jj=0 ; jj<2 ; jj++ ) + { + int _jj = jj + usData[1].start; + C cxy = C( cx*usData[1].v[jj] ); + for( int kk=0 ; kk<2 ; kk++ ) + { + int _kk = kk + usData[2].start; + if( neighbors.neighbors[_ii][_jj][_kk] ) + _constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += C( cxy*usData[2].v[kk] ); + } + } + } + } + } +#pragma omp parallel for num_threads( threads ) + for( int i=lStart ; i + template< class C > + void Octree< Degree >::UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const + { + if ( depth==0 ) return; + else if( depth==1 ) + { + for( int i=sNodes.nodeCount[1] ; idepthAndOffset( d , off ); + for( int d=0 ; d<3 ; d++ ) + { + if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); + else if( off[d]+1==(1<parent ); + for( int ii=0 ; ii<2 ; ii++ ) + { + int _ii = ii + usData[0].start; + double dx = usData[0].v[ii]; + for( int jj=0 ; jj<2 ; jj++ ) + { + int _jj = jj + usData[1].start; + double dxy = dx * usData[1].v[jj]; + for( int kk=0 ; kk<2 ; kk++ ) + { + int _kk = kk + usData[2].start; + if( neighbors.neighbors[_ii][_jj][_kk] ) + { + double dxyz = dxy * usData[2].v[kk]; + int _i = neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex; + coefficients[i] += coefficients[_i] * Real( dxyz ); + } + } + } + } + } + } + } + template< int Degree > + void Octree< Degree >::SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution ) + { + int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; + // For every node at the current depth +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tnodeData.pointIndex; + if( pIdx!=-1 ) + { + neighborKey.getNeighbors( sNodes.treeNodes[i] ); + _points[ pIdx ].value = WeightedCoarserFunctionValue( neighborKey , sNodes.treeNodes[i] , metSolution ); + } + } + } + } + template< int Degree > + Real Octree< Degree >::WeightedCoarserFunctionValue( const OctNode< TreeNodeData , Real >::NeighborKey3& neighborKey , const TreeOctNode* pointNode , Real* metSolution ) const + { + int depth = pointNode->depth(); + if( !depth || pointNode->nodeData.pointIndex==-1 ) return Real(0.); + double pointValue = 0; + + Real weight = _points[ pointNode->nodeData.pointIndex ].weight; + Point3D< Real > p = _points[ pointNode->nodeData.pointIndex ].position; + + // Iterate over all basis functions that overlap the point at the coarser resolutions + { + int d , _idx[3]; + const TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1]; + neighbors.neighbors[1][1][1]->depthAndOffset( d , _idx ); + _idx[0] = BinaryNode< double >::CenterIndex( d , _idx[0]-1 ); + _idx[1] = BinaryNode< double >::CenterIndex( d , _idx[1]-1 ); + _idx[2] = BinaryNode< double >::CenterIndex( d , _idx[2]-1 ); + for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) for( int l=0 ; l<3 ; l++ ) + if( neighbors.neighbors[j][k][l] && neighbors.neighbors[j][k][l]->nodeData.nodeIndex>=0 ) + { + // Accumulate the contribution from these basis nodes + const TreeOctNode* basisNode = neighbors.neighbors[j][k][l]; + int idx[] = { _idx[0]+j , _idx[1]+k , _idx[2]+l }; + pointValue += + fData.baseBSplines[ idx[0] ][2-j]( p[0] ) * + fData.baseBSplines[ idx[1] ][2-k]( p[1] ) * + fData.baseBSplines[ idx[2] ][2-l]( p[2] ) * + metSolution[basisNode->nodeData.nodeIndex]; + } + } + return Real( pointValue * weight ); + } + template + int Octree::GetFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix , int depth , const SortedTreeNodes& sNodes , Real* metSolution ) + { + int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; + double stencil[5][5][5]; + SetLaplacianStencil( depth , stencil ); + Stencil< double , 5 > stencils[2][2][2]; + SetLaplacianStencils( depth , stencils ); + matrix.Resize( range ); +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tparent ) + { + c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + } + else x = y = z = 0; + UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] ); + } + } + return 1; + } + template + int Octree::GetRestrictedFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix,int depth,const int* entries,int entryCount, + const TreeOctNode* rNode , Real radius , + const SortedTreeNodes& sNodes , Real* metSolution ) + { + for( int i=0 ; inodeData.nodeIndex = i; + double stencil[5][5][5]; + int rDepth , rOff[3]; + rNode->depthAndOffset( rDepth , rOff ); + matrix.Resize( entryCount ); + SetLaplacianStencil( depth , stencil ); + Stencil< double , 5 > stencils[2][2][2]; + SetLaplacianStencils( depth , stencils ); +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tdepthAndOffset( d , off ); + off[0] >>= (depth-rDepth) , off[1] >>= (depth-rDepth) , off[2] >>= (depth-rDepth); + bool isInterior = ( off[0]==rOff[0] && off[1]==rOff[1] && off[2]==rOff[2] ); + + neighborKey5.getNeighbors( node ); + + int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5; + if( !isInterior ) SetMatrixRowBounds( neighborKey5.neighbors[depth].neighbors[2][2][2] , rDepth , rOff , xStart , xEnd , yStart , yEnd , zStart , zEnd ); + + // Get the matrix row size + int count = GetMatrixRowSize( neighborKey5.neighbors[depth] , xStart , xEnd , yStart , yEnd , zStart , zEnd ); + + // Allocate memory for the row +#pragma omp critical (matrix_set_row_size) + { + matrix.SetRowSize( i , count ); + } + + // Set the matrix row entries + matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , 0 , stencil , xStart , xEnd , yStart , yEnd , zStart , zEnd ); + // Adjust the system constraints + int x , y , z , c; + if( node->parent ) + { + c = int( node - node->parent->children ); + Cube::FactorCornerIndex( c , x , y , z ); + } + else x = y = z = 0; + UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] ); + } + } + for( int i=0 ; inodeData.nodeIndex = entries[i]; + return 1; + } + + template + int Octree::LaplacianMatrixIteration( int subdivideDepth , bool showResidual , int minIters , double accuracy ) + { + int i,iter=0; + double t = 0; + fData.setDotTables( fData.DD_DOT_FLAG | fData.DV_DOT_FLAG ); + + SparseMatrix< float >::SetAllocator( MEMORY_ALLOCATOR_BLOCK_SIZE ); + _sNodes.treeNodes[0]->nodeData.solution = 0; + + std::vector< Real > metSolution( _sNodes.nodeCount[ _sNodes.maxDepth ] , 0 ); + + for( i=1 ; i<_sNodes.maxDepth ; i++ ) + { + if( subdivideDepth>0 ) iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , subdivideDepth , showResidual , minIters , accuracy ); + else iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , showResidual , minIters , accuracy ); + } + SparseMatrix< float >::internalAllocator.reset(); + fData.clearDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG | fData.DD_DOT_FLAG ); + + return iter; + } + + template + int Octree::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , bool showResidual , int minIters , double accuracy ) + { + int iter = 0; + Vector< Real > X , B; + SparseSymmetricMatrix< Real > M; + double systemTime=0. , solveTime=0. , updateTime=0. , evaluateTime = 0.; + + X.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] ); + if( depth<=_minDepth ) UpSampleCoarserSolution( depth , sNodes , X ); + else + { + // Up-sample the cumulative solution into the previous depth + UpSample( depth-1 , sNodes , metSolution ); + // Add in the solution from that depth + if( depth ) +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution; + } + if( _constrainValues ) + { + SetCoarserPointValues( depth , sNodes , metSolution ); + } + + SparseSymmetricMatrix< Real >::internalAllocator.rollBack(); + { + int maxECount = ( (2*Degree+1)*(2*Degree+1)*(2*Degree+1) + 1 ) / 2; + maxECount = ( ( maxECount + 15 ) / 16 ) * 16; + M.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] ); + for( int i=0 ; i::internalAllocator.rollBack(); + GetFixedDepthLaplacian( M , depth , sNodes , metSolution ); + // Set the constraint vector + B.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] ); + for( int i=sNodes.nodeCount[depth] ; inodeData.constraint; + } + + // Solve the linear system + iter += SparseSymmetricMatrix< Real >::Solve( M , B , std::max< int >( int( pow( M.rows , ITERATION_POWER ) ) , minIters ) , X , Real(accuracy) , 0 , threads , (depth<=_minDepth) && !_constrainValues ); + + if( showResidual ) + { + double mNorm = 0; + for( int i=0 ; i %g (%f) [%d]\n" , M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter ); + } + + // Copy the solution back into the tree (over-writing the constraints) + for( int i=sNodes.nodeCount[depth] ; inodeData.solution = Real( X[i-sNodes.nodeCount[depth]] ); + + return iter; + } + template + int Octree::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , int startingDepth , bool showResidual , int minIters , double accuracy ) + { + if( startingDepth>=depth ) return SolveFixedDepthMatrix( depth , sNodes , metSolution , showResidual , minIters , accuracy ); + + int i , j , d , tIter=0; + SparseSymmetricMatrix< Real > _M; + Vector< Real > B , B_ , X_; + AdjacencySetFunction asf; + AdjacencyCountFunction acf; + double systemTime = 0 , solveTime = 0 , memUsage = 0 , evaluateTime = 0 , gTime = 0, sTime = 0; + Real myRadius , myRadius2; + + if( depth>_minDepth ) + { + // Up-sample the cumulative solution into the previous depth + UpSample( depth-1 , sNodes , metSolution ); + // Add in the solution from that depth + if( depth ) +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution; + } + + if( _constrainValues ) + { + SetCoarserPointValues( depth , sNodes , metSolution ); + } + B.Resize( sNodes.nodeCount[depth+1] - sNodes.nodeCount[depth] ); + + // Back-up the constraints + for( i=sNodes.nodeCount[depth] ; inodeData.constraint; + sNodes.treeNodes[i]->nodeData.constraint = 0; + } + + myRadius = 2*radius-Real(0.5); + myRadius = int(myRadius-ROUND_EPS)+ROUND_EPS; + myRadius2 = Real(radius+ROUND_EPS-0.5); + d = depth-startingDepth; + std::vector< int > subDimension( sNodes.nodeCount[d+1]-sNodes.nodeCount[d] ); + int maxDimension = 0; + for( i=sNodes.nodeCount[d] ; inextNode() ; temp ; ) + { + if( temp->depth()==depth ) acf.adjacencyCount++ , temp = sNodes.treeNodes[i]->nextBranch( temp ); + else temp = sNodes.treeNodes[i]->nextNode ( temp ); + } + for( j=sNodes.nodeCount[d] ; j( maxDimension , subDimension[i-sNodes.nodeCount[d]] ); + } + asf.adjacencies = new int[maxDimension]; + MapReduceVector< Real > mrVector; + mrVector.resize( threads , maxDimension ); + // Iterate through the coarse-level nodes + for( i=sNodes.nodeCount[d] ; inextNode() ; temp ; ) + { + if( temp->depth()==depth ) asf.adjacencies[ asf.adjacencyCount++ ] = temp->nodeData.nodeIndex , temp = sNodes.treeNodes[i]->nextBranch( temp ); + else temp = sNodes.treeNodes[i]->nextNode ( temp ); + } + for( j=sNodes.nodeCount[d] ; jnodeData.solution; + } + // Get the associated matrix + SparseSymmetricMatrix< Real >::internalAllocator.rollBack(); + GetRestrictedFixedDepthLaplacian( _M , depth , asf.adjacencies , asf.adjacencyCount , sNodes.treeNodes[i] , myRadius , sNodes , metSolution ); +#pragma omp parallel for num_threads( threads ) schedule( static ) + for( j=0 ; jnodeData.constraint; + sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.constraint = 0; + } + + // Solve the matrix + // Since we don't have the full matrix, the system shouldn't be singular, so we shouldn't have to correct it + iter += SparseSymmetricMatrix< Real >::Solve( _M , B_ , std::max< int >( int( pow( _M.rows , ITERATION_POWER ) ) , minIters ) , X_ , mrVector , Real(accuracy) , 0 ); + + if( showResidual ) + { + double mNorm = 0; + for( int i=0 ; i<_M.rows ; i++ ) for( int j=0 ; j<_M.rowSizes[i] ; j++ ) mNorm += _M[i][j].Value * _M[i][j].Value; + double bNorm = B_.Norm( 2 ) , rNorm = ( B_ - _M * X_ ).Norm( 2 ); + printf( "\t\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , _M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter ); + } + + // Update the solution for all nodes in the sub-tree + for( j=0 ; jdepth()>sNodes.treeNodes[i]->depth() ) temp=temp->parent; + if( temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex ) sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution = Real( X_[j] ); + } + systemTime += gTime; + solveTime += sTime; + memUsage = std::max< double >( MemoryUsage() , memUsage ); + tIter += iter; + } + delete[] asf.adjacencies; + return tIter; + } + template + int Octree::HasNormals(TreeOctNode* node,Real epsilon) + { + int hasNormals=0; + if( node->nodeData.normalIndex>=0 && ( (*normals)[node->nodeData.normalIndex][0]!=0 || (*normals)[node->nodeData.normalIndex][1]!=0 || (*normals)[node->nodeData.normalIndex][2]!=0 ) ) hasNormals=1; + if( node->children ) for(int i=0;ichildren[i],epsilon); + + return hasNormals; + } + template + void Octree::ClipTree( void ) + { + int maxDepth = tree.maxDepth(); + for( TreeOctNode* temp=tree.nextNode() ; temp ; temp=tree.nextNode(temp) ) + if( temp->children && temp->d>=_minDepth ) + { + int hasNormals=0; + for( int i=0 ; ichildren[i] , EPSILON/(1<children=NULL; + } + MemoryUsage(); + } + template + void Octree::SetLaplacianConstraints( void ) + { + // To set the Laplacian constraints, we iterate over the + // splatted normals and compute the dot-product of the + // divergence of the normal field with all the basis functions. + // Within the same depth: set directly as a gather + // Coarser depths + fData.setDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG ); + + int maxDepth = _sNodes.maxDepth-1; + Point3D< Real > zeroPoint; + zeroPoint[0] = zeroPoint[1] = zeroPoint[2] = 0; + std::vector< Real > constraints( _sNodes.nodeCount[maxDepth] , Real(0) ); + std::vector< Point3D< Real > > coefficients( _sNodes.nodeCount[maxDepth] , zeroPoint ); + + // Clear the constraints +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint = Real( 0. ); + + // For the scattering part of the operation, we parallelize by duplicating the constraints and then summing at the end. + std::vector< std::vector< Real > > _constraints( threads ); + for( int t=0 ; t=0 ; d-- ) + { + Point3D< double > stencil[5][5][5]; + SetDivergenceStencil( d , &stencil[0][0][0] , false ); + Stencil< Point3D< double > , 5 > stencils[2][2][2]; + SetDivergenceStencils( d , stencils , true ); +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tdepth(); + neighborKey5.getNeighbors( node ); + + bool isInterior , isInterior2; + { + int d , off[3]; + node->depthAndOffset( d , off ); + int mn = 2 , mx = (1<=mn && off[0]=mn && off[1]=mn && off[2]=mn && off[0]=mn && off[1]=mn && off[2]parent->children ); + Cube::FactorCornerIndex( c , cx , cy , cz ); + } + else cx = cy = cz = 0; + Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz]; + + // Set constraints from current depth + { + const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth]; + + if( isInterior ) + for( int x=startX ; xnodeData.normalIndex>=0 ) + { + const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex]; + node->nodeData.constraint += Real( stencil[x][y][z][0] * _normal[0] + stencil[x][y][z][1] * _normal[1] + stencil[x][y][z][2] * _normal[2] ); + } + } + else + for( int x=startX ; xnodeData.normalIndex>=0 ) + { + const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex]; + node->nodeData.constraint += GetDivergence( _node , node , _normal ); + } + } + UpdateCoarserSupportBounds( neighbors5.neighbors[2][2][2] , startX , endX , startY , endY , startZ , endZ ); + } + if( node->nodeData.nodeIndex<0 || node->nodeData.normalIndex<0 ) continue; + const Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex]; + if( normal[0]==0 && normal[1]==0 && normal[2]==0 ) continue; + if( depth& div = _stencil.values[x][y][z]; + _constraints[t][ _node->nodeData.nodeIndex ] += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] ); + } + else _constraints[t][ _node->nodeData.nodeIndex ] += GetDivergence( node , _node , normal ); + } + } + } + } + } +#pragma omp parallel for num_threads( threads ) schedule( static ) + for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ ) + { + Real cSum = Real(0.); + for( int t=0 ; t=0 ; d-- ) DownSample( d , _sNodes , &constraints[0] ); + + // Coarse-to-fine up-sampling of coefficients + for( int d=0 ; dnodeData.constraint += constraints[i]; + + // Compute the contribution from all coarser depths + for( int d=0 ; d<=maxDepth ; d++ ) + { + int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end - start; + Stencil< Point3D< double > , 5 > stencils[2][2][2]; + SetDivergenceStencils( d , stencils , false ); +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tdepth(); + if( !depth ) continue; + int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5; + UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ ); + const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.getNeighbors( node->parent ); + + bool isInterior; + { + int d , off[3]; + node->depthAndOffset( d , off ); + int mn = 4 , mx = (1<=mn && off[0]=mn && off[1]=mn && off[2]parent->children ); + Cube::FactorCornerIndex( c , cx , cy , cz ); + } + else cx = cy = cz = 0; + Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz]; + + Real constraint = Real(0); + for( int x=startX ; xnodeData.nodeIndex; + if( isInterior ) + { + Point3D< double >& div = _stencil.values[x][y][z]; + Point3D< Real >& normal = coefficients[_i]; + constraint += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] ); + } + else constraint += GetDivergence( _node , node , coefficients[_i] ); + } + node->nodeData.constraint += constraint; + } + } + } + + fData.clearDotTables( fData.DV_DOT_FLAG ); + + // Set the point weights for evaluating the iso-value +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tnodeData.nodeIndex<0 || temp->nodeData.normalIndex<0 ) temp->nodeData.centerWeightContribution = 0; + else temp->nodeData.centerWeightContribution = Real( Length((*normals)[temp->nodeData.normalIndex]) ); + } + MemoryUsage(); + delete normals; + normals = NULL; + } + template + void Octree::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencyCount++;} + template + void Octree::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;} + template + void Octree::RefineFunction::Function( TreeOctNode* node1 , const TreeOctNode* node2 ) + { + if( !node1->children && node1->depth()initChildren(); + } + template< int Degree > + void Octree< Degree >::FaceEdgesFunction::Function( const TreeOctNode* node1 , const TreeOctNode* node2 ) + { + if( !node1->children && MarchingCubes::HasRoots( node1->nodeData.mcIndex ) ) + { + RootInfo ri1 , ri2; + int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES]; + int count=MarchingCubes::AddTriangleIndices( node1->nodeData.mcIndex , isoTri ); + + for( int j=0 ; jpush_back( std::pair< RootInfo , RootInfo >( ri2 , ri1 ) ); + if( vertexCount->count( key1 )==0 ) + { + (*vertexCount)[key1].first = ri1; + (*vertexCount)[key1].second=0; + } + if( vertexCount->count( key2 )==0 ) + { + (*vertexCount)[key2].first = ri2; + (*vertexCount)[key2].second=0; + } + (*vertexCount)[key1].second--; + (*vertexCount)[key2].second++; + } + else fprintf( stderr , "Bad Edge 1: %d %d\n" , ri1.key , ri2.key ); + } + } + + template< int Degree > + void Octree< Degree >::RefineBoundary( int subdivideDepth ) + { + // This implementation is somewhat tricky. + // We would like to ensure that leaf-nodes across a subdivision boundary have the same depth. + // We do this by calling the setNeighbors function. + // The key is to implement this in a single pass through the leaves, ensuring that refinements don't propogate. + // To this end, we do the minimal refinement that ensures that a cross boundary neighbor, and any of its cross-boundary + // neighbors are all refined simultaneously. + // For this reason, the implementation can only support nodes deeper than sDepth. + bool flags[3][3][3]; + int maxDepth = tree.maxDepth(); + + int sDepth; + if( subdivideDepth<=0 ) sDepth = 0; + else sDepth = maxDepth-subdivideDepth; + if( sDepth<=0 ) return; + + // Ensure that face adjacent neighbors across the subdivision boundary exist to allow for + // a consistent definition of the iso-surface + TreeOctNode::NeighborKey3 nKey; + nKey.set( maxDepth ); + for( TreeOctNode* leaf=tree.nextLeaf() ; leaf ; leaf=tree.nextLeaf( leaf ) ) + if( leaf->depth()>sDepth ) + { + int d , off[3] , _off[3]; + leaf->depthAndOffset( d , off ); + int res = (1< + void Octree::GetMCIsoTriangles( Real isoValue , int subdivideDepth , CoredMeshData* mesh , int fullDepthIso , int nonLinearFit , bool addBarycenter , bool polygonMesh ) + { + fData.setValueTables( fData.VALUE_FLAG | fData.D_VALUE_FLAG , 0 , postNormalSmooth ); + + // Ensure that the subtrees are self-contained + RefineBoundary( subdivideDepth ); + + RootData rootData , coarseRootData; + std::vector< Point3D< float > >* interiorPoints; + int maxDepth = tree.maxDepth(); + + int sDepth = subdivideDepth<=0 ? 0 : std::max< int >( 0 , maxDepth-subdivideDepth ); + + std::vector< Real > metSolution( _sNodes.nodeCount[maxDepth] , 0 ); +#pragma omp parallel for num_threads( threads ) + for( int i=_sNodes.nodeCount[_minDepth] ; i<_sNodes.nodeCount[maxDepth] ; i++ ) metSolution[i] = _sNodes.treeNodes[i]->nodeData.solution; + for( int d=0 ; dnodeData.mcIndex = 0; + + rootData.boundaryValues = new std::unordered_map< long long , std::pair< Real , Point3D< Real > > >(); + int offSet = 0; + + int maxCCount = _sNodes.getMaxCornerCount( &tree , sDepth , maxDepth , threads ); + int maxECount = _sNodes.getMaxEdgeCount ( &tree , sDepth , threads ); + rootData.cornerValues = new Real [ maxCCount ]; + rootData.cornerNormals = new Point3D< Real >[ maxCCount ]; + rootData.interiorRoots = new int [ maxECount ]; + rootData.cornerValuesSet = new char[ maxCCount ]; + rootData.cornerNormalsSet = new char[ maxCCount ]; + rootData.edgesSet = new char[ maxECount ]; + _sNodes.setCornerTable( coarseRootData , &tree , sDepth , threads ); + coarseRootData.cornerValues = new Real[ coarseRootData.cCount ]; + coarseRootData.cornerNormals = new Point3D< Real >[ coarseRootData.cCount ]; + coarseRootData.cornerValuesSet = new char[ coarseRootData.cCount ]; + coarseRootData.cornerNormalsSet = new char[ coarseRootData.cCount ]; + memset( coarseRootData.cornerValuesSet , 0 , sizeof( char ) * coarseRootData.cCount ); + memset( coarseRootData.cornerNormalsSet , 0 , sizeof( char ) * coarseRootData.cCount ); + MemoryUsage(); + + std::vector< TreeOctNode::ConstNeighborKey3 > nKeys( threads ); + for( int t=0 ; t nKeys5( threads ); + for( int t=0 ; tchildren ) continue; + + _sNodes.setCornerTable( rootData , _sNodes.treeNodes[i] , threads ); + _sNodes.setEdgeTable ( rootData , _sNodes.treeNodes[i] , threads ); + memset( rootData.cornerValuesSet , 0 , sizeof( char ) * rootData.cCount ); + memset( rootData.cornerNormalsSet , 0 , sizeof( char ) * rootData.cCount ); + memset( rootData.edgesSet , 0 , sizeof( char ) * rootData.eCount ); + interiorPoints = new std::vector< Point3D< float > >(); + for( int d=maxDepth ; d>sDepth ; d-- ) + { + int leafNodeCount = 0; + std::vector< TreeOctNode* > leafNodes; + for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodeCount++; + leafNodes.reserve( leafNodeCount ); + for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodes.push_back( node ); + Stencil< double , 3 > stencil1[8] , stencil2[8][8]; + SetEvaluationStencils( d , stencil1 , stencil2 ); + + // First set the corner values and associated marching-cube indices +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tdepthAndOffset( d , off ); + int res = 1<<(d-sDepth); + off[0] %= res , off[1] %=res , off[2] %= res; + res--; + if( !(off[0]%res) && !(off[1]%res) && !(off[2]%res) ) + { + const TreeOctNode* temp = leaf; + while( temp->d!=sDepth ) temp = temp->parent; + int x = off[0]==0 ? 0 : 1 , y = off[1]==0 ? 0 : 1 , z = off[2]==0 ? 0 : 1; + int c = Cube::CornerIndex( x , y , z ); + int idx = coarseRootData.cornerIndices( temp )[ c ]; + coarseRootData.cornerValues[ idx ] = rootData.cornerValues[ rootData.cornerIndices( leaf )[c] ]; + coarseRootData.cornerValuesSet[ idx ] = true; + } + + // Compute the iso-vertices + if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) ) + SetMCRootPositions( leaf , sDepth , isoValue , nKeys5[t] , rootData , interiorPoints , mesh , &metSolution[0] , nonLinearFit ); + } + // Note that this should be broken off for multi-threading as + // the SetMCRootPositions writes to interiorPoints (with lockupdateing) + // while GetMCIsoTriangles reads from interiorPoints (without locking) +#if MISHA_DEBUG + std::vector< Point3D< float > > barycenters; + std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? & barycenters : NULL; +#endif // MISHA_DEBUG +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; tnodeData.mcIndex ) ) +#if MISHA_DEBUG + GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , polygonMesh , barycenterPtr ); +#else // !MISHA_DEBUG + GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , addBarycenter , polygonMesh ); +#endif // MISHA_DEBUG + } +#if MISHA_DEBUG + for( int i=0 ; ipush_back( barycenters[i] ); +#endif // MISHA_DEBUG + } + offSet = mesh->outOfCorePointCount(); +#if 1 + delete interiorPoints; +#endif + } + MemoryUsage(); + delete[] rootData.cornerValues , delete[] rootData.cornerNormals , rootData.cornerValues = NULL , rootData.cornerNormals = NULL; + delete[] rootData.cornerValuesSet , delete[] rootData.cornerNormalsSet , rootData.cornerValuesSet = NULL , rootData.cornerNormalsSet = NULL; + delete[] rootData.interiorRoots ; rootData.interiorRoots = NULL; + delete[] rootData.edgesSet ; rootData.edgesSet = NULL; + coarseRootData.interiorRoots = NULL; + coarseRootData.boundaryValues = rootData.boundaryValues; + for( auto iter=rootData.boundaryRoots.cbegin() ; iter!=rootData.boundaryRoots.cend() ; iter++ ) + coarseRootData.boundaryRoots[iter->first] = iter->second; + + for( int d=sDepth ; d>=0 ; d-- ) + { + Stencil< double , 3 > stencil1[8] , stencil2[8][8]; + SetEvaluationStencils( d , stencil1 , stencil2 ); +#if MISHA_DEBUG + std::vector< Point3D< float > > barycenters; + std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? &barycenters : NULL; +#endif // MISHA_DEBUG + for( int i=_sNodes.nodeCount[d] ; i<_sNodes.nodeCount[d+1] ; i++ ) + { + TreeOctNode* leaf = _sNodes.treeNodes[i]; + if( leaf->children ) continue; + + // First set the corner values and associated marching-cube indices + SetIsoCorners( isoValue , leaf , coarseRootData , coarseRootData.cornerValuesSet , coarseRootData.cornerValues , nKey , &metSolution[0] , stencil1 , stencil2 ); + + // Now compute the iso-vertices + if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) ) + { + SetMCRootPositions( leaf , 0 , isoValue , nKey5 , coarseRootData , NULL , mesh , &metSolution[0] , nonLinearFit ); +#if MISHA_DEBUG + GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , polygonMesh , barycenterPtr ); +#else // !MISHA_DEBUG + GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , addBarycenter , polygonMesh ); +#endif // MISHA_DEBUG + } + } + } + MemoryUsage(); + + delete[] coarseRootData.cornerValues , delete[] coarseRootData.cornerNormals; + delete[] coarseRootData.cornerValuesSet , delete[] coarseRootData.cornerNormalsSet; + delete rootData.boundaryValues; + } + template + Real Octree::getCenterValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey , const TreeOctNode* node){ + int idx[3]; + Real value=0; + + VertexData::CenterIndex(node,fData.depth,idx); + idx[0]*=fData.functionCount; + idx[1]*=fData.functionCount; + idx[2]*=fData.functionCount; + int minDepth = std::max< int >( 0 , std::min< int >( _minDepth , node->depth()-1 ) ); + for( int i=minDepth ; i<=node->depth() ; i++ ) + for(int j=0;j<3;j++) + for(int k=0;k<3;k++) + for(int l=0;l<3;l++) + { + const TreeOctNode* n=neighborKey.neighbors[i].neighbors[j][k][l]; + if( n ) + { + Real temp=n->nodeData.solution; + value+=temp*Real( + fData.valueTables[idx[0]+int(n->off[0])]* + fData.valueTables[idx[1]+int(n->off[1])]* + fData.valueTables[idx[2]+int(n->off[2])]); + } + } + if(node->children) + { + for(int i=0;ichildren[i]; + while(1){ + value+=n->nodeData.solution*Real( + fData.valueTables[idx[0]+int(n->off[0])]* + fData.valueTables[idx[1]+int(n->off[1])]* + fData.valueTables[idx[2]+int(n->off[2])]); + if( n->children ) n=&n->children[ii]; + else break; + } + } + } + return value; + } + template< int Degree > + Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution ) + { + int idx[3]; + double value = 0; + + VertexData::CornerIndex( node , corner , fData.depth , idx ); + idx[0] *= fData.functionCount; + idx[1] *= fData.functionCount; + idx[2] *= fData.functionCount; + + int d = node->depth(); + int cx , cy , cz; + int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3; + Cube::FactorCornerIndex( corner , cx , cy , cz ); + { + TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d]; + if( cx==0 ) endX = 2; + else startX = 1; + if( cy==0 ) endY = 2; + else startY = 1; + if( cz==0 ) endZ = 2; + else startZ = 1; + for( int x=startX ; xoff[0]) ]* + fData.valueTables[ idx[1]+int(n->off[1]) ]* + fData.valueTables[ idx[2]+int(n->off[2]) ]; + value += n->nodeData.solution * v; + } + } + } + if( d>0 && d>_minDepth ) + { + int _corner = int( node - node->parent->children ); + int _cx , _cy , _cz; + Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); + if( cx!=_cx ) startX = 0 , endX = 3; + if( cy!=_cy ) startY = 0 , endY = 3; + if( cz!=_cz ) startZ = 0 , endZ = 3; + TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1]; + for( int x=startX ; xoff[0]) ]* + fData.valueTables[ idx[1]+int(n->off[1]) ]* + fData.valueTables[ idx[2]+int(n->off[2]) ]; + value += metSolution[ n->nodeData.nodeIndex ] * v; + } + } + } + return Real( value ); + } + template< int Degree > + Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution , const double stencil1[3][3][3] , const double stencil2[3][3][3] ) + { + double value = 0; + int d = node->depth(); + int cx , cy , cz; + int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3; + Cube::FactorCornerIndex( corner , cx , cy , cz ); + { + TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d]; + if( cx==0 ) endX = 2; + else startX = 1; + if( cy==0 ) endY = 2; + else startY = 1; + if( cz==0 ) endZ = 2; + else startZ = 1; + for( int x=startX ; xnodeData.solution * stencil1[x][y][z]; + } + } + if( d>0 && d>_minDepth ) + { + int _corner = int( node - node->parent->children ); + int _cx , _cy , _cz; + Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); + if( cx!=_cx ) startX = 0 , endX = 3; + if( cy!=_cy ) startY = 0 , endY = 3; + if( cz!=_cz ) startZ = 0 , endZ = 3; + TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1]; + for( int x=startX ; xnodeData.nodeIndex ] * stencil2[x][y][z]; + } + } + return Real( value ); + } + template< int Degree > + Point3D< Real > Octree< Degree >::getCornerNormal( const OctNode< TreeNodeData , Real >::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution ) + { + int idx[3]; + Point3D< Real > normal; + normal[0] = normal[1] = normal[2] = 0.; + + VertexData::CornerIndex( node , corner , fData.depth , idx ); + idx[0] *= fData.functionCount; + idx[1] *= fData.functionCount; + idx[2] *= fData.functionCount; + + int d = node->depth(); + // Iterate over all ancestors that can overlap the corner + { + TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d]; + for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ ) + { + const TreeOctNode* n=neighbors.neighbors[j][k][l]; + if( n ) + { + int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] }; + double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] }; + double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] }; + Real solution = n->nodeData.solution; + normal[0] += Real( dValues[0] * values[1] * values[2] * solution ); + normal[1] += Real( values[0] * dValues[1] * values[2] * solution ); + normal[2] += Real( values[0] * values[1] * dValues[2] * solution ); + } + } + } + if( d>0 && d>_minDepth ) + { + TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d-1]; + for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ ) + { + const TreeOctNode* n=neighbors.neighbors[j][k][l]; + if( n ) + { + int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] }; + double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] }; + double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] }; + Real solution = metSolution[ n->nodeData.nodeIndex ]; + normal[0] += Real( dValues[0] * values[1] * values[2] * solution ); + normal[1] += Real( values[0] * dValues[1] * values[2] * solution ); + normal[2] += Real( values[0] * values[1] * dValues[2] * solution ); + } + } + } + return normal; + } + template< int Degree > + Real Octree::GetIsoValue( void ) + { + Real isoValue , weightSum; + + neighborKey2.set( fData.depth ); + fData.setValueTables( fData.VALUE_FLAG , 0 ); + + isoValue = weightSum = 0; +#pragma omp parallel for num_threads( threads ) reduction( + : isoValue , weightSum ) + for( int t=0 ; tnodeData.centerWeightContribution; + if( w!=0 ) + { + isoValue += getCenterValue( nKey , temp ) * w; + weightSum += w; + } + } + } + return isoValue/weightSum; + } + + template< int Degree > + void Octree< Degree >::SetIsoCorners( Real isoValue , TreeOctNode* leaf , SortedTreeNodes::CornerTableData& cData , char* valuesSet , Real* values , TreeOctNode::ConstNeighborKey3& nKey , const Real* metSolution , const Stencil< double , 3 > stencil1[8] , const Stencil< double , 3 > stencil2[8][8] ) + { + Real cornerValues[ Cube::CORNERS ]; + const SortedTreeNodes::CornerIndices& cIndices = cData[ leaf ]; + + bool isInterior; + int d , off[3]; + leaf->depthAndOffset( d , off ); + int mn = 2 , mx = (1<=mn && off[0]=mn && off[1]=mn && off[2]parent->children)][c].values ); + else cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution ); + values[vIndex] = cornerValues[c]; + valuesSet[vIndex] = 1; + } + } + + leaf->nodeData.mcIndex = MarchingCubes::GetIndex( cornerValues , isoValue ); + + // Set the marching cube indices for all interior nodes. + if( leaf->parent ) + { + TreeOctNode* parent = leaf->parent; + int c = int( leaf - leaf->parent->children ); + int mcid = leaf->nodeData.mcIndex & (1<nodeData.mcIndex, mcid); + + while( 1 ) + { + if( parent->parent && parent->parent->d>=_minDepth && (parent-parent->parent->children)==c ) + { + poisson::atomicOr(parent->parent->nodeData.mcIndex, mcid); + parent = parent->parent; + } + else break; + } + } + } + } + + + template + int Octree::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,int maxDepth){ + int c1,c2,e1,e2,dir,off,cnt=0; + int corners[Cube::CORNERS/2]; + if(node->children){ + Cube::FaceCorners(faceIndex,corners[0],corners[1],corners[2],corners[3]); + Cube::FactorFaceIndex(faceIndex,dir,off); + c1=corners[0]; + c2=corners[3]; + switch(dir){ + case 0: + e1=Cube::EdgeIndex(1,off,1); + e2=Cube::EdgeIndex(2,off,1); + break; + case 1: + e1=Cube::EdgeIndex(0,off,1); + e2=Cube::EdgeIndex(2,1,off); + break; + case 2: + e1=Cube::EdgeIndex(0,1,off); + e2=Cube::EdgeIndex(1,1,off); + break; + }; + cnt+=EdgeRootCount(&node->children[c1],e1,maxDepth)+EdgeRootCount(&node->children[c1],e2,maxDepth); + switch(dir){ + case 0: + e1=Cube::EdgeIndex(1,off,0); + e2=Cube::EdgeIndex(2,off,0); + break; + case 1: + e1=Cube::EdgeIndex(0,off,0); + e2=Cube::EdgeIndex(2,0,off); + break; + case 2: + e1=Cube::EdgeIndex(0,0,off); + e2=Cube::EdgeIndex(1,0,off); + break; + }; + cnt+=EdgeRootCount(&node->children[c2],e1,maxDepth)+EdgeRootCount(&node->children[c2],e2,maxDepth); + for(int i=0;ichildren[corners[i]].children){cnt+=InteriorFaceRootCount(&node->children[corners[i]],faceIndex,maxDepth);}} + } + return cnt; + } + + template + int Octree::EdgeRootCount(const TreeOctNode* node,int edgeIndex,int maxDepth){ + int f1,f2,c1,c2; + const TreeOctNode* temp; + Cube::FacesAdjacentToEdge(edgeIndex,f1,f2); + + int eIndex; + const TreeOctNode* finest=node; + eIndex=edgeIndex; + if(node->depth()faceNeighbor(f1); + if(temp && temp->children){ + finest=temp; + eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1); + } + else{ + temp=node->faceNeighbor(f2); + if(temp && temp->children){ + finest=temp; + eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2); + } + else{ + temp=node->edgeNeighbor(edgeIndex); + if(temp && temp->children){ + finest=temp; + eIndex=Cube::EdgeReflectEdgeIndex(edgeIndex); + } + } + } + } + + Cube::EdgeCorners(eIndex,c1,c2); + if(finest->children) return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth); + else return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex); + } + template + int Octree::IsBoundaryFace(const TreeOctNode* node,int faceIndex,int subdivideDepth){ + int dir,offset,d,o[3],idx; + + if(subdivideDepth<0){return 0;} + if(node->d<=subdivideDepth){return 1;} + Cube::FactorFaceIndex(faceIndex,dir,offset); + node->depthAndOffset(d,o); + + idx=(int(o[dir])<<1) + (offset<<1); + return !(idx%(2<<(int(node->d)-subdivideDepth))); + } + template + int Octree::IsBoundaryEdge(const TreeOctNode* node,int edgeIndex,int subdivideDepth){ + int dir,x,y; + Cube::FactorEdgeIndex(edgeIndex,dir,x,y); + return IsBoundaryEdge(node,dir,x,y,subdivideDepth); + } + template + int Octree::IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subdivideDepth ) + { + int d , o[3] , idx1 , idx2 , mask; + + if( subdivideDepth<0 ) return 0; + if( node->d<=subdivideDepth ) return 1; + node->depthAndOffset( d , o ); + + switch( dir ) + { + case 0: + idx1 = o[1] + x; + idx2 = o[2] + y; + break; + case 1: + idx1 = o[0] + x; + idx2 = o[2] + y; + break; + case 2: + idx1 = o[0] + x; + idx2 = o[1] + y; + break; + } + mask = 1<<( int(node->d) - subdivideDepth ); + return !(idx1%(mask)) || !(idx2%(mask)); + } + template< int Degree > + void Octree< Degree >::GetRootSpan( const RootInfo& ri , Point3D< float >& start , Point3D< float >& end ) + { + int o , i1 , i2; + Real width; + Point3D< Real > c; + + Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 ); + ri.node->centerAndWidth( c , width ); + switch(o) + { + case 0: + start[0] = c[0] - width/2; + end [0] = c[0] + width/2; + start[1] = end[1] = c[1] - width/2 + width*i1; + start[2] = end[2] = c[2] - width/2 + width*i2; + break; + case 1: + start[0] = end[0] = c[0] - width/2 + width*i1; + start[1] = c[1] - width/2; + end [1] = c[1] + width/2; + start[2] = end[2] = c[2] - width/2 + width*i2; + break; + case 2: + start[0] = end[0] = c[0] - width/2 + width*i1; + start[1] = end[1] = c[1] - width/2 + width*i2; + start[2] = c[2] - width/2; + end [2] = c[2] + width/2; + break; + } + } + ////////////////////////////////////////////////////////////////////////////////////// + // The assumption made when calling this code is that the edge has at most one root // + ////////////////////////////////////////////////////////////////////////////////////// + template< int Degree > + int Octree< Degree >::GetRoot( const RootInfo& ri , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , Point3D< Real > & position , RootData& rootData , int sDepth , const Real* metSolution , int nonLinearFit ) + { + if( !MarchingCubes::HasRoots( ri.node->nodeData.mcIndex ) ) return 0; + int c1 , c2; + Cube::EdgeCorners( ri.edgeIndex , c1 , c2 ); + if( !MarchingCubes::HasEdgeRoots( ri.node->nodeData.mcIndex , ri.edgeIndex ) ) return 0; + + long long key1 , key2; + Point3D< Real > n[2]; + + int i , o , i1 , i2 , rCount=0; + Polynomial<2> P; + std::vector< double > roots; + double x0 , x1; + Real center , width; + Real averageRoot=0; + Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 ); + int idx1[3] , idx2[3]; + key1 = VertexData::CornerIndex( ri.node , c1 , fData.depth , idx1 ); + key2 = VertexData::CornerIndex( ri.node , c2 , fData.depth , idx2 ); + + bool isBoundary = ( IsBoundaryEdge( ri.node , ri.edgeIndex , sDepth )!=0 ); + bool haveKey1 , haveKey2; + std::pair< Real , Point3D< Real > > keyValue1 , keyValue2; + int iter1 , iter2; + { + iter1 = rootData.cornerIndices( ri.node )[ c1 ]; + iter2 = rootData.cornerIndices( ri.node )[ c2 ]; + keyValue1.first = rootData.cornerValues[iter1]; + keyValue2.first = rootData.cornerValues[iter2]; + if( isBoundary ) + { +#pragma omp critical (normal_hash_access) + { + haveKey1 = ( rootData.boundaryValues->count( key1 )>0 ); + haveKey2 = ( rootData.boundaryValues->count( key2 )>0 ); + if( haveKey1 ) keyValue1 = (*rootData.boundaryValues)[key1]; + if( haveKey2 ) keyValue2 = (*rootData.boundaryValues)[key2]; + } + } + else + { + haveKey1 = ( rootData.cornerNormalsSet[ iter1 ] != 0 ); + haveKey2 = ( rootData.cornerNormalsSet[ iter2 ] != 0 ); + keyValue1.first = rootData.cornerValues[iter1]; + keyValue2.first = rootData.cornerValues[iter2]; + if( haveKey1 ) keyValue1.second = rootData.cornerNormals[iter1]; + if( haveKey2 ) keyValue2.second = rootData.cornerNormals[iter2]; + } + } + if( !haveKey1 || !haveKey2 ) neighborKey5.getNeighbors( ri.node ); + if( !haveKey1 ) keyValue1.second = getCornerNormal( neighborKey5 , ri.node , c1 , metSolution ); + x0 = keyValue1.first; + n[0] = keyValue1.second; + + if( !haveKey2 ) keyValue2.second = getCornerNormal( neighborKey5 , ri.node , c2 , metSolution ); + x1 = keyValue2.first; + n[1] = keyValue2.second; + + if( !haveKey1 || !haveKey2 ) + { + if( isBoundary ) + { +#pragma omp critical (normal_hash_access) + { + if( !haveKey1 ) (*rootData.boundaryValues)[key1] = keyValue1; + if( !haveKey2 ) (*rootData.boundaryValues)[key2] = keyValue2; + } + } + else + { + if( !haveKey1 ) rootData.cornerNormals[iter1] = keyValue1.second , rootData.cornerNormalsSet[ iter1 ] = 1; + if( !haveKey2 ) rootData.cornerNormals[iter2] = keyValue2.second , rootData.cornerNormalsSet[ iter2 ] = 1; + } + } + + Point3D< Real > c; + ri.node->centerAndWidth(c,width); + center=c[o]; + for( i=0 ; i=0 && roots[i]<=1 ) + { + averageRoot += Real( roots[i] ); + rCount++; + } + if( rCount && nonLinearFit ) averageRoot /= rCount; + else averageRoot = Real((x0-isoValue)/(x0-x1)); + if( averageRoot<0 || averageRoot>1 ) + { + fprintf( stderr , "[WARNING] Bad average root: %f\n" , averageRoot ); + fprintf( stderr , "\t(%f %f) , (%f %f) (%f)\n" , x0 , x1 , dx0 , dx1 , isoValue ); + if( averageRoot<0 ) averageRoot = 0; + if( averageRoot>1 ) averageRoot = 1; + } + position[o] = Real(center-width/2+width*averageRoot); + return 1; + } + template< int Degree > + int Octree< Degree >::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth,RootInfo& ri ) + { + int c1,c2,f1,f2; + const TreeOctNode *temp,*finest; + int finestIndex; + + Cube::FacesAdjacentToEdge(edgeIndex,f1,f2); + + finest=node; + finestIndex=edgeIndex; + if(node->depth()faceNeighbor(f1);} + if(temp && temp->children){ + finest=temp; + finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1); + } + else{ + if(IsBoundaryFace(node,f2,sDepth)){temp=NULL;} + else{temp=node->faceNeighbor(f2);} + if(temp && temp->children){ + finest=temp; + finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2); + } + else{ + if(IsBoundaryEdge(node,edgeIndex,sDepth)){temp=NULL;} + else{temp=node->edgeNeighbor(edgeIndex);} + if(temp && temp->children){ + finest=temp; + finestIndex=Cube::EdgeReflectEdgeIndex(edgeIndex); + } + } + } + } + + Cube::EdgeCorners(finestIndex,c1,c2); + if(finest->children){ + if (GetRootIndex(&finest->children[c1],finestIndex,maxDepth,sDepth,ri)) {return 1;} + else if (GetRootIndex(&finest->children[c2],finestIndex,maxDepth,sDepth,ri)) {return 1;} + else + { + fprintf( stderr , "[WARNING] Couldn't find root index with either child\n" ); + return 0; + } + } + else + { + if( !(MarchingCubes::edgeMask()[finest->nodeData.mcIndex] & (1<depthAndOffset(d,off); + ri.node=finest; + ri.edgeIndex=finestIndex; + int eIndex[2],offset; + offset=BinaryNode::Index( d , off[o] ); + switch(o) + { + case 0: + eIndex[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[1],i1); + eIndex[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[2],i2); + break; + case 1: + eIndex[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[0],i1); + eIndex[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[2],i2); + break; + case 2: + eIndex[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[0],i1); + eIndex[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[1],i2); + break; + } + ri.key = (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45; + return 1; + } + } + template + int Octree::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri ) + { + int c1,c2,f1,f2; + const TreeOctNode *temp,*finest; + int finestIndex; + + + // The assumption is that the super-edge has a root along it. + if(!(MarchingCubes::edgeMask()[node->nodeData.mcIndex] & (1<depth()children ) + { + temp=node->faceNeighbor( f1 ); + if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f1 ); + else + { + temp = node->faceNeighbor( f2 ); + if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f2 ); + else + { + temp = node->edgeNeighbor( edgeIndex ); + if( temp && temp->children ) finest = temp , finestIndex = Cube::EdgeReflectEdgeIndex( edgeIndex ); + } + } + } + + Cube::EdgeCorners( finestIndex , c1 , c2 ); + if( finest->children ) + { + if ( GetRootIndex( finest->children + c1 , finestIndex , maxDepth , ri ) ) return 1; + else if( GetRootIndex( finest->children + c2 , finestIndex , maxDepth , ri ) ) return 1; + else + { + int d1 , off1[3] , d2 , off2[3]; + node->depthAndOffset( d1 , off1 ); + finest->depthAndOffset( d2 , off2 ); + fprintf( stderr , "[WARNING] Couldn't find root index with either child [%d] (%d %d %d) -> [%d] (%d %d %d) (%d %d)\n" , d1 , off1[0] , off1[1] , off1[2] , d2 , off2[0] , off2[1] , off2[2] , node->children!=NULL , finest->children!=NULL ); + printf( "\t" ); + for( int i=0 ; i<8 ; i++ ) if( node->nodeData.mcIndex & (1<nodeData.mcIndex & (1<depthAndOffset(d,off); + ri.node=finest; + ri.edgeIndex=finestIndex; + int offset,eIndex[2]; + offset = BinaryNode< Real >::CenterIndex( d , off[o] ); + switch(o){ + case 0: + eIndex[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[1],i1); + eIndex[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[2],i2); + break; + case 1: + eIndex[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[0],i1); + eIndex[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[2],i2); + break; + case 2: + eIndex[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[0],i1); + eIndex[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[1],i2); + break; + } + ri.key= (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45; + return 1; + } + } + template + int Octree::GetRootPair(const RootInfo& ri,int maxDepth,RootInfo& pair){ + const TreeOctNode* node=ri.node; + int c1,c2,c; + Cube::EdgeCorners(ri.edgeIndex,c1,c2); + while(node->parent){ + c=int(node-node->parent->children); + if(c!=c1 && c!=c2){return 0;} + if(!MarchingCubes::HasEdgeRoots(node->parent->nodeData.mcIndex,ri.edgeIndex)){ + if(c==c1){return GetRootIndex(&node->parent->children[c2],ri.edgeIndex,maxDepth,pair);} + else{return GetRootIndex(&node->parent->children[c1],ri.edgeIndex,maxDepth,pair);} + } + node=node->parent; + } + return 0; + + } + template + int Octree< Degree >::GetRootIndex( const RootInfo& ri , RootData& rootData , CoredPointIndex& index ) + { + long long key = ri.key; + auto rootIter = rootData.boundaryRoots.find( key ); + if( rootIter!=rootData.boundaryRoots.end() ) + { + index.inCore = 1; + index.index = rootIter->second; + return 1; + } + else if( rootData.interiorRoots ) + { + int eIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ]; + if( rootData.edgesSet[ eIndex ] ) + { + index.inCore = 0; + index.index = rootData.interiorRoots[ eIndex ]; + return 1; + } + } + return 0; + } + template< int Degree > + int Octree< Degree >::SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData , + std::vector< Point3D< float > >* interiorPositions , CoredMeshData* mesh , const Real* metSolution , int nonLinearFit ) + { + Point3D< Real > position; + int eIndex; + RootInfo ri; + int count=0; + if( !MarchingCubes::HasRoots( node->nodeData.mcIndex ) ) return 0; + for( int i=0 ; i::iterator iter , end; + // Check if the root has already been set +#pragma omp critical (boundary_roots_hash_access) + { + iter = rootData.boundaryRoots.find( key ); + end = rootData.boundaryRoots.end(); + } + if( iter==end ) + { + // Get the root information + GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit ); + // Add the root if it hasn't been added already +#pragma omp critical (boundary_roots_hash_access) + { + iter = rootData.boundaryRoots.find( key ); + end = rootData.boundaryRoots.end(); + if( iter==end ) + { + mesh->inCorePoints.push_back( position ); + rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() ) - 1; + } + } + if( iter==end ) count++; + } + } + else + { + int nodeEdgeIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ]; + if( !rootData.edgesSet[ nodeEdgeIndex ] ) + { + // Get the root information + GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit ); + // Add the root if it hasn't been added already +#pragma omp critical (add_point_access) + { + if( !rootData.edgesSet[ nodeEdgeIndex ] ) + { + rootData.interiorRoots[ nodeEdgeIndex ] = mesh->addOutOfCorePoint( position ); + interiorPositions->push_back( position ); + rootData.edgesSet[ nodeEdgeIndex ] = 1; + count++; + } + } + } + } + } + } + return count; + } + template + int Octree< Degree >::SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , CoredMeshData* mesh , int nonLinearFit ) + { + Point3D< Real > position; + int i,j,k,eIndex,hits=0; + RootInfo ri; + int count=0; + TreeOctNode* node; + + node = tree.nextLeaf(); + while( node ) + { + if( MarchingCubes::HasRoots( node->nodeData.mcIndex ) ) + { + hits=0; + for( i=0 ; iinCorePoints.push_back( position ); + rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() )-1; + count++; + } + } + } + } + if( hits ) node=tree.nextLeaf(node); + else node=tree.nextBranch(node); + } + return count; + } + template + void Octree< Degree >::GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges ) + { + TreeOctNode* temp; + int count=0 , tris=0; + int isoTri[ DIMENSION * MarchingCubes::MAX_TRIANGLES ]; + FaceEdgesFunction fef; + int ref , fIndex; + std::unordered_map< long long , std::pair< RootInfo , int > > vertexCount; + + fef.edges = &edges; + fef.maxDepth = fData.depth; + fef.vertexCount = &vertexCount; + count = MarchingCubes::AddTriangleIndices( node->nodeData.mcIndex , isoTri ); + for( fIndex=0 ; fIndexfaceNeighbor( fIndex ); + // If the face neighbor exists and has higher resolution than the current node, + // get the iso-curve from the neighbor + if( temp && temp->children && !IsBoundaryFace( node , fIndex , sDepth ) ) temp->processNodeFaces( temp , &fef , ref ); + // Otherwise, get it from the node + else + { + RootInfo ri1 , ri2; + for( int j=0 ; j( ri1 , ri2 ) ); + if( vertexCount.count( key1 )==0 ) + { + vertexCount[key1].first = ri1; + vertexCount[key1].second = 0; + } + if( vertexCount.count( key2 )==0 ) + { + vertexCount[key2].first = ri2; + vertexCount[key2].second = 0; + } + vertexCount[key1].second++; + vertexCount[key2].second--; + } + else + { + int r1 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+k] ); + int r2 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+((k+1)%3)] ); + fprintf( stderr , "Bad Edge 2: %d %d\t%d %d\n" , ri1.key , ri2.key , r1 , r2 ); + } + } + } + for( int i=0 ; idepthAndOffset( d , off ); + printf( "Vertex pair not in list 1 (%lld) %d\t[%d] (%d %d %d)\n" , key , IsBoundaryEdge( ri.node , ri.edgeIndex , sDepth ) , d , off[0] , off[1] , off[2] ); + } + else + { + edges.push_back( std::pair< RootInfo , RootInfo >( ri , edges[i].first ) ); + vertexCount[ key ].second++; + vertexCount[ edges[i].first.key ].second--; + } + } + + if( vertexCount.count( edges[i].second.key )==0 ) printf( "Could not find vertex: %lld\n" , edges[i].second ); + else if( vertexCount[edges[i].second.key].second ) + { + RootInfo ri; + GetRootPair( vertexCount[edges[i].second.key].first , fData.depth , ri ); + long long key = ri.key; + if( vertexCount.count( key ) ) + { + int d , off[3]; + node->depthAndOffset( d , off ); + printf( "Vertex pair not in list 2\t[%d] (%d %d %d)\n" , d , off[0] , off[1] , off[2] ); + } + else + { + edges.push_back( std::pair< RootInfo , RootInfo >( edges[i].second , ri ) ); + vertexCount[key].second--; + vertexCount[ edges[i].second.key ].second++; + } + } + } + } + template +#if MISHA_DEBUG + int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool polygonMesh , std::vector< Point3D< float > >* barycenters ) +#else // !MISHA_DEBUG + int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool addBarycenter , bool polygonMesh ) +#endif // MISHA_DEBUG + { + int tris=0; + std::vector< std::pair< RootInfo , RootInfo > > edges; + std::vector< std::vector< std::pair< RootInfo , RootInfo > > > edgeLoops; + GetMCIsoEdges( node , sDepth , edges ); + + GetEdgeLoops( edges , edgeLoops ); + for( int i=0 ; i edgeIndices; + for( int j=0 ; j + int Octree< Degree >::GetEdgeLoops( std::vector< std::pair< RootInfo , RootInfo > >& edges , std::vector< std::vector< std::pair< RootInfo , RootInfo > > >& loops ) + { + int loopSize=0; + long long frontIdx , backIdx; + std::pair< RootInfo , RootInfo > e , temp; + loops.clear(); + + while( edges.size() ) + { + std::vector< std::pair< RootInfo , RootInfo > > front , back; + e = edges[0]; + loops.resize( loopSize+1 ); + edges[0] = edges.back(); + edges.pop_back(); + frontIdx = e.second.key; + backIdx = e.first.key; + for( int j=int(edges.size())-1 ; j>=0 ; j-- ) + { + if( edges[j].first.key==frontIdx || edges[j].second.key==frontIdx ) + { + if( edges[j].first.key==frontIdx ) temp = edges[j]; + else temp.first = edges[j].second , temp.second = edges[j].first; + frontIdx = temp.second.key; + front.push_back(temp); + edges[j] = edges.back(); + edges.pop_back(); + j = int(edges.size()); + } + else if( edges[j].first.key==backIdx || edges[j].second.key==backIdx ) + { + if( edges[j].second.key==backIdx ) temp = edges[j]; + else temp.first = edges[j].second , temp.second = edges[j].first; + backIdx = temp.first.key; + back.push_back(temp); + edges[j] = edges.back(); + edges.pop_back(); + j = int(edges.size()); + } + } + for( int j=int(back.size())-1 ; j>=0 ; j-- ) loops[loopSize].push_back( back[j] ); + loops[loopSize].push_back(e); + for( int j=0 ; j +#if MISHA_DEBUG + int Octree::AddTriangles( CoredMeshData* mesh , std::vector& edges , std::vector >* interiorPositions , int offSet , bool polygonMesh , std::vector< Point3D< float > >* barycenters ) +#else // !MISHA_DEBUG + int Octree::AddTriangles( CoredMeshData* mesh , std::vector& edges , std::vector >* interiorPositions , int offSet , bool addBarycenter , bool polygonMesh ) +#endif // MISHA_DEBUG + { + MinimalAreaTriangulation< float > MAT; + std::vector< Point3D< float > > vertices; + std::vector< TriangleIndex > triangles; + if( polygonMesh ) + { + std::vector< CoredVertexIndex > vertices( edges.size() ); + for( int i=0 ; iaddPolygon( vertices ); + } + return 1; + } + if( edges.size()>3 ) + { + bool isCoplanar = false; + +#if MISHA_DEBUG + if( barycenters ) +#else // !MISHA_DEBUG + if( addBarycenter ) +#endif // MISHA_DEBUG + for( int i=0 ; i v1 , v2; + if( edges[i].inCore ) v1 = mesh->inCorePoints[ edges[i].index ]; + else v1 = (*interiorPositions)[ edges[i].index-offSet ]; + if( edges[j].inCore ) v2 = mesh->inCorePoints[ edges[j].index ]; + else v2 = (*interiorPositions)[ edges[j].index-offSet ]; + for( int k=0 ; k<3 ; k++ ) if( v1[k]==v2[k] ) isCoplanar = true; + } + if( isCoplanar ) + { + Point3D< Real > c; + c[0] = c[1] = c[2] = 0; + for( int i=0 ; i p; + if(edges[i].inCore) p = mesh->inCorePoints[edges[i].index ]; + else p = (*interiorPositions)[edges[i].index-offSet]; + c += p; + } + c /= Real( edges.size() ); + int cIdx; +#pragma omp critical (add_point_access) + { + cIdx = mesh->addOutOfCorePoint( c ); +#if MISHA_DEBUG + barycenters->push_back( c ); +#else // !MISHA_DEBUG + interiorPositions->push_back( c ); +#endif // MISHA_DEBUG + } + for( int i=0 ; i vertices( 3 ); + vertices[0].idx = edges[i ].index; + vertices[1].idx = edges[(i+1)%edges.size()].index; + vertices[2].idx = cIdx; + vertices[0].inCore = (edges[i ].inCore!=0); + vertices[1].inCore = (edges[(i+1)%edges.size()].inCore!=0); + vertices[2].inCore = 0; +#pragma omp critical (add_polygon_access) + { + mesh->addPolygon( vertices ); + } + } + return int( edges.size() ); + } + else + { + vertices.resize( edges.size() ); + // Add the points + for( int i=0 ; i p; + if( edges[i].inCore ) p = mesh->inCorePoints[edges[i].index ]; + else p = (*interiorPositions)[edges[i].index-offSet]; + vertices[i] = p; + } + MAT.GetTriangulation( vertices , triangles ); + for( int i=0 ; i _vertices( 3 ); + for( int j=0 ; j<3 ; j++ ) + { + _vertices[j].idx = edges[ triangles[i].idx[j] ].index; + _vertices[j].inCore = (edges[ triangles[i].idx[j] ].inCore!=0); + } +#pragma omp critical (add_polygon_access) + { + mesh->addPolygon( _vertices ); + } + } + } + } + else if( edges.size()==3 ) + { + std::vector< CoredVertexIndex > vertices( 3 ); + for( int i=0 ; i<3 ; i++ ) + { + vertices[i].idx = edges[i].index; + vertices[i].inCore = (edges[i].inCore!=0); + } +#pragma omp critical (add_polygon_access) + mesh->addPolygon( vertices ); + } + return int(edges.size())-2; + } + template< int Degree > + Real* Octree< Degree >::GetSolutionGrid( int& res , float isoValue , int depth ) + { + if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth(); + BSplineData< Degree , Real > fData; + fData.set( depth ); + fData.setValueTables( fData.VALUE_FLAG ); + res = 1<d>depth ) continue; + if( n->d<_minDepth ) continue; + int d , idx[3] , start[3] , end[3]; + n->depthAndOffset( d , idx ); + for( int i=0 ; i<3 ; i++ ) + { + // Get the index of the functions + idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] ); + // Figure out which samples fall into the range + fData.setSampleSpan( idx[i] , start[i] , end[i] ); + // We only care about the odd indices + if( !(start[i]&1) ) start[i]++; + if( !( end[i]&1) ) end[i]--; + } + Real coefficient = n->nodeData.solution; + for( int x=start[0] ; x<=end[0] ; x+=2 ) + for( int y=start[1] ; y<=end[1] ; y+=2 ) + for( int z=start[2] ; z<=end[2] ; z+=2 ) + { + int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1; + values[ zz*res*res + yy*res + xx ] += + coefficient * + fData.valueTables[ idx[0]+x*fData.functionCount ] * + fData.valueTables[ idx[1]+y*fData.functionCount ] * + fData.valueTables[ idx[2]+z*fData.functionCount ]; + } + } + for( int i=0 ; i + Real* Octree< Degree >::GetWeightGrid( int& res , int depth ) + { + if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth(); + res = 1<d>depth ) continue; + int d , idx[3] , start[3] , end[3]; + n->depthAndOffset( d , idx ); + for( int i=0 ; i<3 ; i++ ) + { + // Get the index of the functions + idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] ); + // Figure out which samples fall into the range + fData.setSampleSpan( idx[i] , start[i] , end[i] ); + // We only care about the odd indices + if( !(start[i]&1) ) start[i]++; + if( !( end[i]&1) ) end[i]--; + } + for( int x=start[0] ; x<=end[0] ; x+=2 ) + for( int y=start[1] ; y<=end[1] ; y+=2 ) + for( int z=start[2] ; z<=end[2] ; z+=2 ) + { + int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1; + values[ zz*res*res + yy*res + xx ] += + n->nodeData.centerWeightContribution * + fData.valueTables[ idx[0]+x*fData.functionCount ] * + fData.valueTables[ idx[1]+y*fData.functionCount ] * + fData.valueTables[ idx[2]+z*fData.functionCount ]; + } + } + return values; + } + + //////////////// + // VertexData // + //////////////// + long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth){ + int idx[DIMENSION]; + return CenterIndex(node,maxDepth,idx); + } + long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]){ + int d,o[3]; + node->depthAndOffset(d,o); + for(int i=0;i::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);} + return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; + } + long long VertexData::CenterIndex(int depth,const int offSet[DIMENSION],int maxDepth,int idx[DIMENSION]){ + for(int i=0;i::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);} + return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; + } + long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth){ + int idx[DIMENSION]; + return CornerIndex(node,cIndex,maxDepth,idx); + } + long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] ) + { + int x[DIMENSION]; + Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] ); + int d , o[3]; + node->depthAndOffset( d , o ); + for( int i=0 ; i::CornerIndex( maxDepth+1 , d , o[i] , x[i] ); + return CornerIndexKey( idx ); + } + long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] ) + { + int x[DIMENSION]; + Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] ); + for( int i=0 ; i::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] ); + return CornerIndexKey( idx ); + } + long long VertexData::CornerIndexKey( const int idx[DIMENSION] ) + { + return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; + } + long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){ + int idx[DIMENSION]; + return FaceIndex(node,fIndex,maxDepth,idx); + } + long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION]){ + int dir,offset; + Cube::FactorFaceIndex(fIndex,dir,offset); + int d,o[3]; + node->depthAndOffset(d,o); + for(int i=0;i::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);} + idx[dir]=BinaryNode::CornerIndex(maxDepth+1,d,o[dir],offset); + return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; + } + long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth){ + int idx[DIMENSION]; + return EdgeIndex(node,eIndex,maxDepth,idx); + } + long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth,int idx[DIMENSION]){ + int o,i1,i2; + int d,off[3]; + node->depthAndOffset(d,off); + for(int i=0;i::CornerIndex(maxDepth+1,d+1,off[i]<<1,1);} + Cube::FactorEdgeIndex(eIndex,o,i1,i2); + switch(o){ + case 0: + idx[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[1],i1); + idx[2]=BinaryNode::CornerIndex(maxDepth+1,d,off[2],i2); + break; + case 1: + idx[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[0],i1); + idx[2]=BinaryNode::CornerIndex(maxDepth+1,d,off[2],i2); + break; + case 2: + idx[0]=BinaryNode::CornerIndex(maxDepth+1,d,off[0],i1); + idx[1]=BinaryNode::CornerIndex(maxDepth+1,d,off[1],i2); + break; + }; + return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; + } + + + } +} + + diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/octree_poisson.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/octree_poisson.h new file mode 100755 index 0000000..68e082a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/octree_poisson.h @@ -0,0 +1,287 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef OCT_NODE_INCLUDED +#define OCT_NODE_INCLUDED + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include "allocator.h" +#include "binary_node.h" +#include "marching_cubes_poisson.h" + +#define DIMENSION 3 + +namespace pcl +{ + namespace poisson + { + + template< class NodeData , class Real=float > + class OctNode + { + private: + static int UseAlloc; + + class AdjacencyCountFunction + { + public: + int count; + void Function( const OctNode* node1 , const OctNode* node2 ); + }; + template + void __processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2,int cIndex3,int cIndex4); + template + void __processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2); + template + void __processNodeNodes(OctNode* node,NodeAdjacencyFunction* F); + template + static void __ProcessNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,NodeAdjacencyFunction* F); + template + static void __ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,TerminatingNodeAdjacencyFunction* F); + template + static void __ProcessPointAdjacentNodes(int dx,int dy,int dz,OctNode* node2,int radius2,int cWidth2,PointAdjacencyFunction* F); + template + static void __ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,int depth,NodeAdjacencyFunction* F); + template + static void __ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int cWidth2,int depth,NodeAdjacencyFunction* F); + + // This is made private because the division by two has been pulled out. + static inline int Overlap(int c1,int c2,int c3,int dWidth); + inline static int ChildOverlap(int dx,int dy,int dz,int d,int cRadius2); + + const OctNode* __faceNeighbor(int dir,int off) const; + const OctNode* __edgeNeighbor(int o,const int i[2],const int idx[2]) const; + OctNode* __faceNeighbor(int dir,int off,int forceChildren); + OctNode* __edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren); + public: + static const int DepthShift,OffsetShift,OffsetShift1,OffsetShift2,OffsetShift3; + static const int DepthMask,OffsetMask; + + static Allocator internalAllocator; + static int UseAllocator(void); + static void SetAllocator(int blockSize); + + OctNode* parent; + OctNode* children; + short d , off[DIMENSION]; + NodeData nodeData; + + OctNode(void); + ~OctNode(void); + int initChildren(void); + + void depthAndOffset(int& depth,int offset[DIMENSION]) const; + int depth(void) const; + static inline void DepthAndOffset(const long long& index,int& depth,int offset[DIMENSION]); + static inline void CenterAndWidth(const long long& index,Point3D& center,Real& width); + static inline int Depth(const long long& index); + static inline void Index(int depth,const int offset[3],short& d,short off[DIMENSION]); + void centerAndWidth( Point3D& center , Real& width ) const; + bool isInside( Point3D< Real > p ) const; + + int leaves(void) const; + int maxDepthLeaves(int maxDepth) const; + int nodes(void) const; + int maxDepth(void) const; + + const OctNode* root(void) const; + + const OctNode* nextLeaf(const OctNode* currentLeaf=NULL) const; + OctNode* nextLeaf(OctNode* currentLeaf=NULL); + const OctNode* nextNode(const OctNode* currentNode=NULL) const; + OctNode* nextNode(OctNode* currentNode=NULL); + const OctNode* nextBranch(const OctNode* current) const; + OctNode* nextBranch(OctNode* current); + const OctNode* prevBranch(const OctNode* current) const; + OctNode* prevBranch(OctNode* current); + + void setFullDepth(int maxDepth); + + void printLeaves(void) const; + void printRange(void) const; + + template + void processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int fIndex,int processCurrent=1); + template + void processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int eIndex,int processCurrent=1); + template + void processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,int cIndex,int processCurrent=1); + template + void processNodeNodes(OctNode* node,NodeAdjacencyFunction* F,int processCurrent=1); + + template + static void ProcessNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,NodeAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,NodeAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessTerminatingNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessPointAdjacentNodes(int maxDepth,const int center1[3],OctNode* node2,int width2,PointAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessPointAdjacentNodes(int dx,int dy,int dz,OctNode* node2,int radius2,int width2,PointAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessFixedDepthNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessMaxDepthNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); + template + static void ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz,OctNode* node1,int radius1,OctNode* node2,int radius2,int width2,int depth,NodeAdjacencyFunction* F,int processCurrent=1); + + static int CornerIndex(const Point3D& center,const Point3D &p); + + OctNode* faceNeighbor(int faceIndex,int forceChildren=0); + const OctNode* faceNeighbor(int faceIndex) const; + OctNode* edgeNeighbor(int edgeIndex,int forceChildren=0); + const OctNode* edgeNeighbor(int edgeIndex) const; + OctNode* cornerNeighbor(int cornerIndex,int forceChildren=0); + const OctNode* cornerNeighbor(int cornerIndex) const; + + OctNode* getNearestLeaf(const Point3D& p); + const OctNode* getNearestLeaf(const Point3D& p) const; + + static int CommonEdge(const OctNode* node1,int eIndex1,const OctNode* node2,int eIndex2); + static int CompareForwardDepths(const void* v1,const void* v2); + static int CompareByDepthAndXYZ( const void* v1 , const void* v2 ); + static int CompareByDepthAndZIndex( const void* v1 , const void* v2 ); + static int CompareForwardPointerDepths(const void* v1,const void* v2); + static int CompareBackwardDepths(const void* v1,const void* v2); + static int CompareBackwardPointerDepths(const void* v1,const void* v2); + + + template + OctNode& operator = (const OctNode& node); + + static inline int Overlap2(const int &depth1,const int offSet1[DIMENSION],const Real& multiplier1,const int &depth2,const int offSet2[DIMENSION],const Real& multiplier2); + + + int write(const char* fileName) const; + int write(FILE* fp) const; + int read(const char* fileName); + int read(FILE* fp); + + class Neighbors3 + { + public: + OctNode* neighbors[3][3][3]; + Neighbors3( void ); + void clear( void ); + }; + class NeighborKey3 + { + public: + Neighbors3* neighbors; + + NeighborKey3( void ); + ~NeighborKey3( void ); + + void set( int depth ); + Neighbors3& setNeighbors( OctNode* root , Point3D< Real > p , int d ); + Neighbors3& getNeighbors( OctNode* root , Point3D< Real > p , int d ); + Neighbors3& setNeighbors( OctNode* node , bool flags[3][3][3] ); + Neighbors3& setNeighbors( OctNode* node ); + Neighbors3& getNeighbors( OctNode* node ); + }; + class ConstNeighbors3 + { + public: + const OctNode* neighbors[3][3][3]; + ConstNeighbors3( void ); + void clear( void ); + }; + class ConstNeighborKey3 + { + public: + ConstNeighbors3* neighbors; + + ConstNeighborKey3(void); + ~ConstNeighborKey3(void); + + void set(int depth); + ConstNeighbors3& getNeighbors( const OctNode* node ); + ConstNeighbors3& getNeighbors( const OctNode* node , int minDepth ); + }; + class Neighbors5 + { + public: + OctNode* neighbors[5][5][5]; + Neighbors5( void ); + void clear( void ); + }; + class ConstNeighbors5 + { + public: + const OctNode* neighbors[5][5][5]; + ConstNeighbors5( void ); + void clear( void ); + }; + + class NeighborKey5 + { + int _depth; + public: + Neighbors5* neighbors; + + NeighborKey5( void ); + ~NeighborKey5( void ); + + void set( int depth ); + Neighbors5& getNeighbors( OctNode* node ); + Neighbors5& setNeighbors( OctNode* node , int xStart=0 , int xEnd=5 , int yStart=0 , int yEnd=5 , int zStart=0 , int zEnd=5 ); + }; + class ConstNeighborKey5 + { + int _depth; + public: + ConstNeighbors5* neighbors; + + ConstNeighborKey5( void ); + ~ConstNeighborKey5( void ); + + void set( int depth ); + ConstNeighbors5& getNeighbors( const OctNode* node ); + }; + + void centerIndex(int maxDepth,int index[DIMENSION]) const; + int width(int maxDepth) const; + }; + + + } +} + +#include "octree_poisson.hpp" + + + +#endif // OCT_NODE diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/octree_poisson.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/octree_poisson.hpp new file mode 100755 index 0000000..1ea060c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/octree_poisson.hpp @@ -0,0 +1,1914 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#include +#include +#include + +#include "poisson_exceptions.h" + +///////////// +// OctNode // +///////////// + +namespace pcl +{ + namespace poisson + { + + template const int OctNode::DepthShift=5; + template const int OctNode::OffsetShift=19; + template const int OctNode::DepthMask=(1< const int OctNode::OffsetMask=(1< const int OctNode::OffsetShift1=DepthShift; + template const int OctNode::OffsetShift2=OffsetShift1+OffsetShift; + template const int OctNode::OffsetShift3=OffsetShift2+OffsetShift; + + template int OctNode::UseAlloc=0; + template Allocator > OctNode::internalAllocator; + + template + void OctNode::SetAllocator(int blockSize) + { + if(blockSize>0) + { + UseAlloc=1; + internalAllocator.set(blockSize); + } + else{UseAlloc=0;} + } + template + int OctNode::UseAllocator(void){return UseAlloc;} + + template + OctNode::OctNode(void){ + parent=children=NULL; + d=off[0]=off[1]=off[2]=0; + } + + template + OctNode::~OctNode(void){ + if(!UseAlloc){if(children){delete[] children;}} + parent=children=NULL; + } + template + void OctNode::setFullDepth(int maxDepth){ + if( maxDepth ) + { + if( !children ) initChildren(); + for( int i=0 ; i<8 ; i++ ) children[i].setFullDepth( maxDepth-1 ); + } + } + + template + int OctNode::initChildren(void){ + int i,j,k; + + if(UseAlloc){children=internalAllocator.newElements(8);} + else{ + if(children){delete[] children;} + children=NULL; + children=new OctNode[Cube::CORNERS]; + } + if(!children){ + POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadInitException, "Failed to initialize OctNode children."); + } + int d,off[3]; + depthAndOffset(d,off); + for(i=0;i<2;i++){ + for(j=0;j<2;j++){ + for(k=0;k<2;k++){ + int idx=Cube::CornerIndex(i,j,k); + children[idx].parent=this; + children[idx].children=NULL; + int off2[3]; + off2[0]=(off[0]<<1)+i; + off2[1]=(off[1]<<1)+j; + off2[2]=(off[2]<<1)+k; + Index(d+1,off2,children[idx].d,children[idx].off); + } + } + } + return 1; + } + template + inline void OctNode::Index(int depth,const int offset[3],short& d,short off[3]){ + d=short(depth); + off[0]=short((1< + inline void OctNode::depthAndOffset(int& depth,int offset[3]) const { + depth=int(d); + offset[0]=(int(off[0])+1)&(~(1< + inline int OctNode::depth(void) const {return int(d);} + template + inline void OctNode::DepthAndOffset(const long long& index,int& depth,int offset[3]){ + depth=int(index&DepthMask); + offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< + inline int OctNode::Depth(const long long& index){return int(index&DepthMask);} + template + void OctNode::centerAndWidth(Point3D& center,Real& width) const{ + int depth,offset[3]; + depth=int(d); + offset[0]=(int(off[0])+1)&(~(1< + bool OctNode< NodeData , Real >::isInside( Point3D< Real > p ) const + { + Point3D< Real > c; + Real w; + centerAndWidth( c , w ); + w /= 2; + return (c[0]-w) + inline void OctNode::CenterAndWidth(const long long& index,Point3D& center,Real& width){ + int depth,offset[3]; + depth=index&DepthMask; + offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< + int OctNode::maxDepth(void) const{ + if(!children){return 0;} + else{ + int c,d; + for(int i=0;ic){c=d;} + } + return c+1; + } + } + template + int OctNode::nodes(void) const{ + if(!children){return 1;} + else{ + int c=0; + for(int i=0;i + int OctNode::leaves(void) const{ + if(!children){return 1;} + else{ + int c=0; + for(int i=0;i + int OctNode::maxDepthLeaves(int maxDepth) const{ + if(depth()>maxDepth){return 0;} + if(!children){return 1;} + else{ + int c=0; + for(int i=0;i + const OctNode* OctNode::root(void) const{ + const OctNode* temp=this; + while(temp->parent){temp=temp->parent;} + return temp; + } + + + template + const OctNode* OctNode::nextBranch( const OctNode* current ) const + { + if( !current->parent || current==this ) return NULL; + if(current-current->parent->children==Cube::CORNERS-1) return nextBranch( current->parent ); + else return current+1; + } + template + OctNode* OctNode::nextBranch(OctNode* current){ + if(!current->parent || current==this){return NULL;} + if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);} + else{return current+1;} + } + template< class NodeData , class Real > + const OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( const OctNode* current ) const + { + if( !current->parent || current==this ) return NULL; + if( current-current->parent->children==0 ) return prevBranch( current->parent ); + else return current-1; + } + template< class NodeData , class Real > + OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( OctNode* current ) + { + if( !current->parent || current==this ) return NULL; + if( current-current->parent->children==0 ) return prevBranch( current->parent ); + else return current-1; + } + template + const OctNode* OctNode::nextLeaf(const OctNode* current) const{ + if(!current){ + const OctNode* temp=this; + while(temp->children){temp=&temp->children[0];} + return temp; + } + if(current->children){return current->nextLeaf();} + const OctNode* temp=nextBranch(current); + if(!temp){return NULL;} + else{return temp->nextLeaf();} + } + template + OctNode* OctNode::nextLeaf(OctNode* current){ + if(!current){ + OctNode* temp=this; + while(temp->children){temp=&temp->children[0];} + return temp; + } + if(current->children){return current->nextLeaf();} + OctNode* temp=nextBranch(current); + if(!temp){return NULL;} + else{return temp->nextLeaf();} + } + + template + const OctNode* OctNode::nextNode( const OctNode* current ) const + { + if( !current ) return this; + else if( current->children ) return ¤t->children[0]; + else return nextBranch(current); + } + template + OctNode* OctNode::nextNode( OctNode* current ) + { + if( !current ) return this; + else if( current->children ) return ¤t->children[0]; + else return nextBranch( current ); + } + + template + void OctNode::printRange(void) const{ + Point3D center; + Real width; + centerAndWidth(center,width); + for(int dim=0;dim + void OctNode::AdjacencyCountFunction::Function(const OctNode* node1,const OctNode* node2){count++;} + + template + template + void OctNode::processNodeNodes(OctNode* node,NodeAdjacencyFunction* F,int processCurrent){ + if(processCurrent){F->Function(this,node);} + if(children){__processNodeNodes(node,F);} + } + template + template + void OctNode::processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int fIndex,int processCurrent){ + if(processCurrent){F->Function(this,node);} + if(children){ + int c1,c2,c3,c4; + Cube::FaceCorners(fIndex,c1,c2,c3,c4); + __processNodeFaces(node,F,c1,c2,c3,c4); + } + } + template + template + void OctNode::processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int eIndex,int processCurrent){ + if(processCurrent){F->Function(this,node);} + if(children){ + int c1,c2; + Cube::EdgeCorners(eIndex,c1,c2); + __processNodeEdges(node,F,c1,c2); + } + } + template + template + void OctNode::processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,int cIndex,int processCurrent){ + if(processCurrent){F->Function(this,node);} + OctNode* temp=this; + while(temp->children){ + temp=&temp->children[cIndex]; + F->Function(temp,node); + } + } + template + template + void OctNode::__processNodeNodes(OctNode* node,NodeAdjacencyFunction* F){ + F->Function(&children[0],node); + F->Function(&children[1],node); + F->Function(&children[2],node); + F->Function(&children[3],node); + F->Function(&children[4],node); + F->Function(&children[5],node); + F->Function(&children[6],node); + F->Function(&children[7],node); + if(children[0].children){children[0].__processNodeNodes(node,F);} + if(children[1].children){children[1].__processNodeNodes(node,F);} + if(children[2].children){children[2].__processNodeNodes(node,F);} + if(children[3].children){children[3].__processNodeNodes(node,F);} + if(children[4].children){children[4].__processNodeNodes(node,F);} + if(children[5].children){children[5].__processNodeNodes(node,F);} + if(children[6].children){children[6].__processNodeNodes(node,F);} + if(children[7].children){children[7].__processNodeNodes(node,F);} + } + template + template + void OctNode::__processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2){ + F->Function(&children[cIndex1],node); + F->Function(&children[cIndex2],node); + if(children[cIndex1].children){children[cIndex1].__processNodeEdges(node,F,cIndex1,cIndex2);} + if(children[cIndex2].children){children[cIndex2].__processNodeEdges(node,F,cIndex1,cIndex2);} + } + template + template + void OctNode::__processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2,int cIndex3,int cIndex4){ + F->Function(&children[cIndex1],node); + F->Function(&children[cIndex2],node); + F->Function(&children[cIndex3],node); + F->Function(&children[cIndex4],node); + if(children[cIndex1].children){children[cIndex1].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} + if(children[cIndex2].children){children[cIndex2].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} + if(children[cIndex3].children){children[cIndex3].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} + if(children[cIndex4].children){children[cIndex4].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} + } + template + template + void OctNode::ProcessNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,NodeAdjacencyFunction* F,int processCurrent){ + int c1[3],c2[3],w1,w2; + node1->centerIndex(maxDepth+1,c1); + node2->centerIndex(maxDepth+1,c2); + w1=node1->width(maxDepth+1); + w2=node2->width(maxDepth+1); + + ProcessNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); + } + template + template + void OctNode::ProcessNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int width2, + NodeAdjacencyFunction* F,int processCurrent){ + if(!Overlap(dx,dy,dz,radius1+radius2)){return;} + if(processCurrent){F->Function(node2,node1);} + if(!node2->children){return;} + __ProcessNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); + } + template + template + void OctNode::ProcessTerminatingNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent){ + int c1[3],c2[3],w1,w2; + node1->centerIndex(maxDepth+1,c1); + node2->centerIndex(maxDepth+1,c2); + w1=node1->width(maxDepth+1); + w2=node2->width(maxDepth+1); + + ProcessTerminatingNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); + } + template + template + void OctNode::ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int width2, + TerminatingNodeAdjacencyFunction* F,int processCurrent) + { + if(!Overlap(dx,dy,dz,radius1+radius2)){return;} + if(processCurrent){F->Function(node2,node1);} + if(!node2->children){return;} + __ProcessTerminatingNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); + } + template + template + void OctNode::ProcessPointAdjacentNodes( int maxDepth , const int c1[3] , OctNode* node2 , int width2 , PointAdjacencyFunction* F , int processCurrent ) + { + int c2[3] , w2; + node2->centerIndex( maxDepth+1 , c2 ); + w2 = node2->width( maxDepth+1 ); + ProcessPointAdjacentNodes( c1[0]-c2[0] , c1[1]-c2[1] , c1[2]-c2[2] , node2 , (width2*w2)>>1 , w2 , F , processCurrent ); + } + template + template + void OctNode::ProcessPointAdjacentNodes(int dx,int dy,int dz, + OctNode* node2,int radius2,int width2, + PointAdjacencyFunction* F,int processCurrent) + { + if( !Overlap(dx,dy,dz,radius2) ) return; + if( processCurrent ) F->Function(node2); + if( !node2->children ) return; + __ProcessPointAdjacentNodes( -dx , -dy , -dz , node2 , radius2 , width2>>1 , F ); + } + template + template + void OctNode::ProcessFixedDepthNodeAdjacentNodes(int maxDepth, + OctNode* node1,int width1, + OctNode* node2,int width2, + int depth,NodeAdjacencyFunction* F,int processCurrent) + { + int c1[3],c2[3],w1,w2; + node1->centerIndex(maxDepth+1,c1); + node2->centerIndex(maxDepth+1,c2); + w1=node1->width(maxDepth+1); + w2=node2->width(maxDepth+1); + + ProcessFixedDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); + } + template + template + void OctNode::ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int width2, + int depth,NodeAdjacencyFunction* F,int processCurrent) + { + int d=node2->depth(); + if(d>depth){return;} + if(!Overlap(dx,dy,dz,radius1+radius2)){return;} + if(d==depth){if(processCurrent){F->Function(node2,node1);}} + else{ + if(!node2->children){return;} + __ProcessFixedDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,depth-1,F); + } + } + template + template + void OctNode::ProcessMaxDepthNodeAdjacentNodes(int maxDepth, + OctNode* node1,int width1, + OctNode* node2,int width2, + int depth,NodeAdjacencyFunction* F,int processCurrent) + { + int c1[3],c2[3],w1,w2; + node1->centerIndex(maxDepth+1,c1); + node2->centerIndex(maxDepth+1,c2); + w1=node1->width(maxDepth+1); + w2=node2->width(maxDepth+1); + ProcessMaxDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); + } + template + template + void OctNode::ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int width2, + int depth,NodeAdjacencyFunction* F,int processCurrent) + { + int d=node2->depth(); + if(d>depth){return;} + if(!Overlap(dx,dy,dz,radius1+radius2)){return;} + if(processCurrent){F->Function(node2,node1);} + if(dchildren){__ProcessMaxDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2>>1,depth-1,F);} + } + template + template + void OctNode::__ProcessNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int cWidth2, + NodeAdjacencyFunction* F) + { + int cWidth=cWidth2>>1; + int radius=radius2>>1; + int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); + if(o){ + int dx1=dx-cWidth; + int dx2=dx+cWidth; + int dy1=dy-cWidth; + int dy2=dy+cWidth; + int dz1=dz-cWidth; + int dz2=dz+cWidth; + if(o& 1){F->Function(&node2->children[0],node1);if(node2->children[0].children){__ProcessNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,F);}} + if(o& 2){F->Function(&node2->children[1],node1);if(node2->children[1].children){__ProcessNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,F);}} + if(o& 4){F->Function(&node2->children[2],node1);if(node2->children[2].children){__ProcessNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,F);}} + if(o& 8){F->Function(&node2->children[3],node1);if(node2->children[3].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,F);}} + if(o& 16){F->Function(&node2->children[4],node1);if(node2->children[4].children){__ProcessNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,F);}} + if(o& 32){F->Function(&node2->children[5],node1);if(node2->children[5].children){__ProcessNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,F);}} + if(o& 64){F->Function(&node2->children[6],node1);if(node2->children[6].children){__ProcessNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,F);}} + if(o&128){F->Function(&node2->children[7],node1);if(node2->children[7].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} + } + } + template + template + void OctNode::__ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int cWidth2, + TerminatingNodeAdjacencyFunction* F) + { + int cWidth=cWidth2>>1; + int radius=radius2>>1; + int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); + if(o){ + int dx1=dx-cWidth; + int dx2=dx+cWidth; + int dy1=dy-cWidth; + int dy2=dy+cWidth; + int dz1=dz-cWidth; + int dz2=dz+cWidth; + if(o& 1){if(F->Function(&node2->children[0],node1) && node2->children[0].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,F);}} + if(o& 2){if(F->Function(&node2->children[1],node1) && node2->children[1].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,F);}} + if(o& 4){if(F->Function(&node2->children[2],node1) && node2->children[2].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,F);}} + if(o& 8){if(F->Function(&node2->children[3],node1) && node2->children[3].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,F);}} + if(o& 16){if(F->Function(&node2->children[4],node1) && node2->children[4].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,F);}} + if(o& 32){if(F->Function(&node2->children[5],node1) && node2->children[5].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,F);}} + if(o& 64){if(F->Function(&node2->children[6],node1) && node2->children[6].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,F);}} + if(o&128){if(F->Function(&node2->children[7],node1) && node2->children[7].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} + } + } + template + template + void OctNode::__ProcessPointAdjacentNodes(int dx,int dy,int dz, + OctNode* node2,int radius2,int cWidth2, + PointAdjacencyFunction* F) + { + int cWidth=cWidth2>>1; + int radius=radius2>>1; + int o=ChildOverlap(dx,dy,dz,radius,cWidth); + if( o ) + { + int dx1=dx-cWidth; + int dx2=dx+cWidth; + int dy1=dy-cWidth; + int dy2=dy+cWidth; + int dz1=dz-cWidth; + int dz2=dz+cWidth; + if(o& 1){F->Function(&node2->children[0]);if(node2->children[0].children){__ProcessPointAdjacentNodes(dx1,dy1,dz1,&node2->children[0],radius,cWidth,F);}} + if(o& 2){F->Function(&node2->children[1]);if(node2->children[1].children){__ProcessPointAdjacentNodes(dx2,dy1,dz1,&node2->children[1],radius,cWidth,F);}} + if(o& 4){F->Function(&node2->children[2]);if(node2->children[2].children){__ProcessPointAdjacentNodes(dx1,dy2,dz1,&node2->children[2],radius,cWidth,F);}} + if(o& 8){F->Function(&node2->children[3]);if(node2->children[3].children){__ProcessPointAdjacentNodes(dx2,dy2,dz1,&node2->children[3],radius,cWidth,F);}} + if(o& 16){F->Function(&node2->children[4]);if(node2->children[4].children){__ProcessPointAdjacentNodes(dx1,dy1,dz2,&node2->children[4],radius,cWidth,F);}} + if(o& 32){F->Function(&node2->children[5]);if(node2->children[5].children){__ProcessPointAdjacentNodes(dx2,dy1,dz2,&node2->children[5],radius,cWidth,F);}} + if(o& 64){F->Function(&node2->children[6]);if(node2->children[6].children){__ProcessPointAdjacentNodes(dx1,dy2,dz2,&node2->children[6],radius,cWidth,F);}} + if(o&128){F->Function(&node2->children[7]);if(node2->children[7].children){__ProcessPointAdjacentNodes(dx2,dy2,dz2,&node2->children[7],radius,cWidth,F);}} + } + } + template + template + void OctNode::__ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int cWidth2, + int depth,NodeAdjacencyFunction* F) + { + int cWidth=cWidth2>>1; + int radius=radius2>>1; + int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); + if(o){ + int dx1=dx-cWidth; + int dx2=dx+cWidth; + int dy1=dy-cWidth; + int dy2=dy+cWidth; + int dz1=dz-cWidth; + int dz2=dz+cWidth; + if(node2->depth()==depth){ + if(o& 1){F->Function(&node2->children[0],node1);} + if(o& 2){F->Function(&node2->children[1],node1);} + if(o& 4){F->Function(&node2->children[2],node1);} + if(o& 8){F->Function(&node2->children[3],node1);} + if(o& 16){F->Function(&node2->children[4],node1);} + if(o& 32){F->Function(&node2->children[5],node1);} + if(o& 64){F->Function(&node2->children[6],node1);} + if(o&128){F->Function(&node2->children[7],node1);} + } + else{ + if(o& 1){if(node2->children[0].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}} + if(o& 2){if(node2->children[1].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}} + if(o& 4){if(node2->children[2].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}} + if(o& 8){if(node2->children[3].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}} + if(o& 16){if(node2->children[4].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}} + if(o& 32){if(node2->children[5].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}} + if(o& 64){if(node2->children[6].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}} + if(o&128){if(node2->children[7].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}} + } + } + } + template + template + void OctNode::__ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz, + OctNode* node1,int radius1, + OctNode* node2,int radius2,int cWidth2, + int depth,NodeAdjacencyFunction* F) + { + int cWidth=cWidth2>>1; + int radius=radius2>>1; + int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); + if(o){ + int dx1=dx-cWidth; + int dx2=dx+cWidth; + int dy1=dy-cWidth; + int dy2=dy+cWidth; + int dz1=dz-cWidth; + int dz2=dz+cWidth; + if(node2->depth()<=depth){ + if(o& 1){F->Function(&node2->children[0],node1);} + if(o& 2){F->Function(&node2->children[1],node1);} + if(o& 4){F->Function(&node2->children[2],node1);} + if(o& 8){F->Function(&node2->children[3],node1);} + if(o& 16){F->Function(&node2->children[4],node1);} + if(o& 32){F->Function(&node2->children[5],node1);} + if(o& 64){F->Function(&node2->children[6],node1);} + if(o&128){F->Function(&node2->children[7],node1);} + } + if(node2->depth()children[0].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}} + if(o& 2){if(node2->children[1].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}} + if(o& 4){if(node2->children[2].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}} + if(o& 8){if(node2->children[3].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}} + if(o& 16){if(node2->children[4].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}} + if(o& 32){if(node2->children[5].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}} + if(o& 64){if(node2->children[6].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}} + if(o&128){if(node2->children[7].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}} + } + } + } + template + inline int OctNode::ChildOverlap(int dx,int dy,int dz,int d,int cRadius2) + { + int w1=d-cRadius2; + int w2=d+cRadius2; + int overlap=0; + + int test=0,test1=0; + if(dx-w1){test =1;} + if(dx-w2){test|=2;} + + if(!test){return 0;} + if(dz-w1){test1 =test;} + if(dz-w2){test1|=test<<4;} + + if(!test1){return 0;} + if(dy-w1){overlap =test1;} + if(dy-w2){overlap|=test1<<2;} + return overlap; + } + + template + OctNode* OctNode::getNearestLeaf(const Point3D& p){ + Point3D center; + Real width; + OctNode* temp; + int cIndex; + if(!children){return this;} + centerAndWidth(center,width); + temp=this; + while(temp->children){ + cIndex=CornerIndex(center,p); + temp=&temp->children[cIndex]; + width/=2; + if(cIndex&1){center.coords[0]+=width/2;} + else {center.coords[0]-=width/2;} + if(cIndex&2){center.coords[1]+=width/2;} + else {center.coords[1]-=width/2;} + if(cIndex&4){center.coords[2]+=width/2;} + else {center.coords[2]-=width/2;} + } + return temp; + } + template + const OctNode* OctNode::getNearestLeaf(const Point3D& p) const{ + int nearest; + Real temp,dist2; + if(!children){return this;} + for(int i=0;i + int OctNode::CommonEdge(const OctNode* node1,int eIndex1,const OctNode* node2,int eIndex2){ + int o1,o2,i1,i2,j1,j2; + + Cube::FactorEdgeIndex(eIndex1,o1,i1,j1); + Cube::FactorEdgeIndex(eIndex2,o2,i2,j2); + if(o1!=o2){return 0;} + + int dir[2]; + int idx1[2]; + int idx2[2]; + switch(o1){ + case 0: dir[0]=1; dir[1]=2; break; + case 1: dir[0]=0; dir[1]=2; break; + case 2: dir[0]=0; dir[1]=1; break; + }; + int d1,d2,off1[3],off2[3]; + node1->depthAndOffset(d1,off1); + node2->depthAndOffset(d2,off2); + idx1[0]=off1[dir[0]]+(1<d2){ + idx2[0]<<=(d1-d2); + idx2[1]<<=(d1-d2); + } + else{ + idx1[0]<<=(d2-d1); + idx1[1]<<=(d2-d1); + } + if(idx1[0]==idx2[0] && idx1[1]==idx2[1]){return 1;} + else {return 0;} + } + template + int OctNode::CornerIndex(const Point3D& center,const Point3D& p){ + int cIndex=0; + if(p.coords[0]>center.coords[0]){cIndex|=1;} + if(p.coords[1]>center.coords[1]){cIndex|=2;} + if(p.coords[2]>center.coords[2]){cIndex|=4;} + return cIndex; + } + template + template + OctNode& OctNode::operator = (const OctNode& node){ + int i; + if(children){delete[] children;} + children=NULL; + + d=node.depth (); + for(i=0;ioffset[i] = node.offset[i];} + if(node.children){ + initChildren(); + for(i=0;i + int OctNode::CompareForwardDepths(const void* v1,const void* v2){ + return ((const OctNode*)v1)->depth-((const OctNode*)v2)->depth; + } + template< class NodeData , class Real > + int OctNode< NodeData , Real >::CompareByDepthAndXYZ( const void* v1 , const void* v2 ) + { + const OctNode *n1 = (*(const OctNode< NodeData , Real >**)v1); + const OctNode *n2 = (*(const OctNode< NodeData , Real >**)v2); + if( n1->d!=n2->d ) return int(n1->d)-int(n2->d); + else if( n1->off[0]!=n2->off[0] ) return int(n1->off[0]) - int(n2->off[0]); + else if( n1->off[1]!=n2->off[1] ) return int(n1->off[1]) - int(n2->off[1]); + else if( n1->off[2]!=n2->off[2] ) return int(n1->off[2]) - int(n2->off[2]); + return 0; + } + + long long _InterleaveBits( int p[3] ) + { + long long key = 0; + long long _p[3] = {p[0],p[1],p[2]}; + for( int i=0 ; i<31 ; i++ ) key |= ( ( _p[0] & (1ull< + int OctNode::CompareByDepthAndZIndex( const void* v1 , const void* v2 ) + { + const OctNode* n1 = (*(const OctNode**)v1); + const OctNode* n2 = (*(const OctNode**)v2); + int d1 , off1[3] , d2 , off2[3]; + n1->depthAndOffset( d1 , off1 ) , n2->depthAndOffset( d2 , off2 ); + if ( d1>d2 ) return 1; + else if( d1k2 ) return 1; + else if( k1 + int OctNode::CompareForwardPointerDepths( const void* v1 , const void* v2 ) + { + const OctNode* n1 = (*(const OctNode**)v1); + const OctNode* n2 = (*(const OctNode**)v2); + if(n1->d!=n2->d){return int(n1->d)-int(n2->d);} + while( n1->parent!=n2->parent ) + { + n1=n1->parent; + n2=n2->parent; + } + if(n1->off[0]!=n2->off[0]){return int(n1->off[0])-int(n2->off[0]);} + if(n1->off[1]!=n2->off[1]){return int(n1->off[1])-int(n2->off[1]);} + return int(n1->off[2])-int(n2->off[2]); + return 0; + } + template + int OctNode::CompareBackwardDepths(const void* v1,const void* v2){ + return ((const OctNode*)v2)->depth-((const OctNode*)v1)->depth; + } + template + int OctNode::CompareBackwardPointerDepths(const void* v1,const void* v2){ + return (*(const OctNode**)v2)->depth()-(*(const OctNode**)v1)->depth(); + } + template + inline int OctNode::Overlap2(const int &depth1,const int offSet1[DIMENSION],const Real& multiplier1,const int &depth2,const int offSet2[DIMENSION],const Real& multiplier2){ + int d=depth2-depth1; + Real w=multiplier2+multiplier1*(1<=w || + fabs(Real(offSet2[1]-(offSet1[1]<=w || + fabs(Real(offSet2[2]-(offSet1[2]<=w + ){return 0;} + return 1; + } + template + inline int OctNode::Overlap(int c1,int c2,int c3,int dWidth){ + if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;} + else{return 1;} + } + template + OctNode* OctNode::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);} + template + const OctNode* OctNode::faceNeighbor(int faceIndex) const {return __faceNeighbor(faceIndex>>1,faceIndex&1);} + template + OctNode* OctNode::__faceNeighbor(int dir,int off,int forceChildren){ + if(!parent){return NULL;} + int pIndex=int(this-(parent->children)); + pIndex^=(1<children[pIndex];} + else{ + OctNode* temp=parent->__faceNeighbor(dir,off,forceChildren); + if(!temp){return NULL;} + if(!temp->children){ + if(forceChildren){temp->initChildren();} + else{return temp;} + } + return &temp->children[pIndex]; + } + } + template + const OctNode* OctNode::__faceNeighbor(int dir,int off) const { + if(!parent){return NULL;} + int pIndex=int(this-(parent->children)); + pIndex^=(1<children[pIndex];} + else{ + const OctNode* temp=parent->__faceNeighbor(dir,off); + if(!temp || !temp->children){return temp;} + else{return &temp->children[pIndex];} + } + } + + template + OctNode* OctNode::edgeNeighbor(int edgeIndex,int forceChildren){ + int idx[2],o,i[2]; + Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); + switch(o){ + case 0: idx[0]=1; idx[1]=2; break; + case 1: idx[0]=0; idx[1]=2; break; + case 2: idx[0]=0; idx[1]=1; break; + }; + return __edgeNeighbor(o,i,idx,forceChildren); + } + template + const OctNode* OctNode::edgeNeighbor(int edgeIndex) const { + int idx[2],o,i[2]; + Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); + switch(o){ + case 0: idx[0]=1; idx[1]=2; break; + case 1: idx[0]=0; idx[1]=2; break; + case 2: idx[0]=0; idx[1]=1; break; + }; + return __edgeNeighbor(o,i,idx); + } + template + const OctNode* OctNode::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{ + if(!parent){return NULL;} + int pIndex=int(this-(parent->children)); + int aIndex,x[DIMENSION]; + + Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); + aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; + pIndex^=(7 ^ (1<__faceNeighbor(idx[0],i[0]); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor + const OctNode* temp=parent->__faceNeighbor(idx[1],i[1]); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==0) { // I can get the neighbor from the parent + return &parent->children[pIndex]; + } + else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor + const OctNode* temp=parent->__edgeNeighbor(o,i,idx); + if(!temp || !temp->children){return temp;} + else{return &temp->children[pIndex];} + } + else{return NULL;} + } + template + OctNode* OctNode::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){ + if(!parent){return NULL;} + int pIndex=int(this-(parent->children)); + int aIndex,x[DIMENSION]; + + Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); + aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; + pIndex^=(7 ^ (1<__faceNeighbor(idx[0],i[0],0); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor + OctNode* temp=parent->__faceNeighbor(idx[1],i[1],0); + if(!temp || !temp->children){return NULL;} + else{return &temp->children[pIndex];} + } + else if(aIndex==0) { // I can get the neighbor from the parent + return &parent->children[pIndex]; + } + else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor + OctNode* temp=parent->__edgeNeighbor(o,i,idx,forceChildren); + if(!temp){return NULL;} + if(!temp->children){ + if(forceChildren){temp->initChildren();} + else{return temp;} + } + return &temp->children[pIndex]; + } + else{return NULL;} + } + + template + const OctNode* OctNode::cornerNeighbor(int cornerIndex) const { + int pIndex,aIndex=0; + if(!parent){return NULL;} + + pIndex=int(this-(parent->children)); + aIndex=(cornerIndex ^ pIndex); // The disagreement bits + pIndex=(~pIndex)&7; // The antipodal point + if(aIndex==7){ // Agree on no bits + return &parent->children[pIndex]; + } + else if(aIndex==0){ // Agree on all bits + const OctNode* temp=((const OctNode*)parent)->cornerNeighbor(cornerIndex); + if(!temp || !temp->children){return temp;} + else{return &temp->children[pIndex];} + } + else if(aIndex==6){ // Agree on face 0 + const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==5){ // Agree on face 1 + const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==3){ // Agree on face 2 + const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==4){ // Agree on edge 2 + const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==2){ // Agree on edge 1 + const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==1){ // Agree on edge 0 + const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else{return NULL;} + } + template + OctNode* OctNode::cornerNeighbor(int cornerIndex,int forceChildren){ + int pIndex,aIndex=0; + if(!parent){return NULL;} + + pIndex=int(this-(parent->children)); + aIndex=(cornerIndex ^ pIndex); // The disagreement bits + pIndex=(~pIndex)&7; // The antipodal point + if(aIndex==7){ // Agree on no bits + return &parent->children[pIndex]; + } + else if(aIndex==0){ // Agree on all bits + OctNode* temp=((OctNode*)parent)->cornerNeighbor(cornerIndex,forceChildren); + if(!temp){return NULL;} + if(!temp->children){ + if(forceChildren){temp->initChildren();} + else{return temp;} + } + return &temp->children[pIndex]; + } + else if(aIndex==6){ // Agree on face 0 + OctNode* temp=((OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1,0); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==5){ // Agree on face 1 + OctNode* temp=((OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1,0); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==3){ // Agree on face 2 + OctNode* temp=((OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2,0); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==4){ // Agree on edge 2 + OctNode* temp=((OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==2){ // Agree on edge 1 + OctNode* temp=((OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else if(aIndex==1){ // Agree on edge 0 + OctNode* temp=((OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); + if(!temp || !temp->children){return NULL;} + else{return & temp->children[pIndex];} + } + else{return NULL;} + } + //////////////////////// + // OctNodeNeighborKey // + //////////////////////// + template + OctNode::Neighbors3::Neighbors3(void){clear();} + template + void OctNode::Neighbors3::clear(void){ + for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} + } + template + OctNode::NeighborKey3::NeighborKey3(void){ neighbors=NULL; } + template + OctNode::NeighborKey3::~NeighborKey3(void) + { + if( neighbors ) delete[] neighbors; + neighbors = NULL; + } + + template + void OctNode::NeighborKey3::set( int d ) + { + if( neighbors ) delete[] neighbors; + neighbors = NULL; + if( d<0 ) return; + neighbors = new Neighbors3[d+1]; + } + template< class NodeData , class Real > + typename OctNode::Neighbors3& OctNode::NeighborKey3::setNeighbors( OctNode* root , Point3D< Real > p , int d ) + { + if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) ) + { + neighbors[d].clear(); + + if( !d ) neighbors[d].neighbors[1][1][1] = root; + else + { + Neighbors3& temp = setNeighbors( root , p , d-1 ); + + int i , j , k , x1 , y1 , z1 , x2 , y2 , z2; + Point3D< Real > c; + Real w; + temp.neighbors[1][1][1]->centerAndWidth( c , w ); + int idx = CornerIndex( c , p ); + Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); + Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 ); + + if( !temp.neighbors[1][1][1]->children ) temp.neighbors[1][1][1]->initChildren(); + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)]; + + + // Set the neighbors from across the faces + i=x1<<1; + if( temp.neighbors[i][1][1] ) + { + if( !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren(); + for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; + } + j=y1<<1; + if( temp.neighbors[1][j][1] ) + { + if( !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren(); + for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; + } + k=z1<<1; + if( temp.neighbors[1][1][k] ) + { + if( !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren(); + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; + } + + // Set the neighbors from across the edges + i=x1<<1 , j=y1<<1; + if( temp.neighbors[i][j][1] ) + { + if( !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren(); + for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; + } + i=x1<<1 , k=z1<<1; + if( temp.neighbors[i][1][k] ) + { + if( !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren(); + for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; + } + j=y1<<1 , k=z1<<1; + if( temp.neighbors[1][j][k] ) + { + if( !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren(); + for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; + } + + // Set the neighbor from across the corner + i=x1<<1 , j=y1<<1 , k=z1<<1; + if( temp.neighbors[i][j][k] ) + { + if( !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren(); + neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + } + } + } + return neighbors[d]; + } + template< class NodeData , class Real > + typename OctNode::Neighbors3& OctNode::NeighborKey3::getNeighbors( OctNode* root , Point3D< Real > p , int d ) + { + if( !neighbors[d].neighbors[1][1][1] || !neighbors[d].neighbors[1][1][1]->isInside( p ) ) + { + neighbors[d].clear(); + + if( !d ) neighbors[d].neighbors[1][1][1] = root; + else + { + Neighbors3& temp = getNeighbors( root , p , d-1 ); + + int i , j , k , x1 , y1 , z1 , x2 , y2 , z2; + Point3D< Real > c; + Real w; + temp.neighbors[1][1][1]->centerAndWidth( c , w ); + int idx = CornerIndex( c , p ); + Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); + Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 ); + + if( !temp.neighbors[1][1][1] || !temp.neighbors[1][1][1]->children ) + { + POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Couldn't find node at appropriate depth"); + } + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + neighbors[d].neighbors[x2+i][y2+j][z2+k] = &temp.neighbors[1][1][1]->children[Cube::CornerIndex(i,j,k)]; + + + // Set the neighbors from across the faces + i=x1<<1; + if( temp.neighbors[i][1][1] ) + for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; + j=y1<<1; + if( temp.neighbors[1][j][1] ) + for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; + k=z1<<1; + if( temp.neighbors[1][1][k] ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; + + // Set the neighbors from across the edges + i=x1<<1 , j=y1<<1; + if( temp.neighbors[i][j][1] ) + for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; + i=x1<<1 , k=z1<<1; + if( temp.neighbors[i][1][k] ) + for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; + j=y1<<1 , k=z1<<1; + if( temp.neighbors[1][j][k] ) + for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; + + // Set the neighbor from across the corner + i=x1<<1 , j=y1<<1 , k=z1<<1; + if( temp.neighbors[i][j][k] ) + neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + } + } + return neighbors[d]; + } + + template< class NodeData , class Real > + typename OctNode::Neighbors3& OctNode::NeighborKey3::setNeighbors( OctNode* node ) + { + int d = node->depth(); + if( node==neighbors[d].neighbors[1][1][1] ) + { + bool reset = false; + for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) if( !neighbors[d].neighbors[i][j][k] ) reset = true; + if( reset ) neighbors[d].neighbors[1][1][1] = NULL; + } + if( node!=neighbors[d].neighbors[1][1][1] ) + { + neighbors[d].clear(); + + if( !node->parent ) neighbors[d].neighbors[1][1][1] = node; + else + { + int i,j,k,x1,y1,z1,x2,y2,z2; + int idx=int(node-node->parent->children); + Cube::FactorCornerIndex( idx ,x1,y1,z1); + Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); + for(i=0;i<2;i++){ + for(j=0;j<2;j++){ + for(k=0;k<2;k++){ + neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; + } + } + } + Neighbors3& temp=setNeighbors(node->parent); + + // Set the neighbors from across the faces + i=x1<<1; + if(temp.neighbors[i][1][1]){ + if(!temp.neighbors[i][1][1]->children){temp.neighbors[i][1][1]->initChildren();} + for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} + } + j=y1<<1; + if(temp.neighbors[1][j][1]){ + if(!temp.neighbors[1][j][1]->children){temp.neighbors[1][j][1]->initChildren();} + for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} + } + k=z1<<1; + if(temp.neighbors[1][1][k]){ + if(!temp.neighbors[1][1][k]->children){temp.neighbors[1][1][k]->initChildren();} + for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} + } + + // Set the neighbors from across the edges + i=x1<<1; j=y1<<1; + if(temp.neighbors[i][j][1]){ + if(!temp.neighbors[i][j][1]->children){temp.neighbors[i][j][1]->initChildren();} + for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} + } + i=x1<<1; k=z1<<1; + if(temp.neighbors[i][1][k]){ + if(!temp.neighbors[i][1][k]->children){temp.neighbors[i][1][k]->initChildren();} + for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} + } + j=y1<<1; k=z1<<1; + if(temp.neighbors[1][j][k]){ + if(!temp.neighbors[1][j][k]->children){temp.neighbors[1][j][k]->initChildren();} + for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} + } + + // Set the neighbor from across the corner + i=x1<<1; j=y1<<1; k=z1<<1; + if(temp.neighbors[i][j][k]){ + if(!temp.neighbors[i][j][k]->children){temp.neighbors[i][j][k]->initChildren();} + neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + } + } + } + return neighbors[d]; + } + // Note the assumption is that if you enable an edge, you also enable adjacent faces. + // And, if you enable a corner, you enable adjacent edges and faces. + template< class NodeData , class Real > + typename OctNode::Neighbors3& OctNode::NeighborKey3::setNeighbors( OctNode* node , bool flags[3][3][3] ) + { + int d = node->depth(); + if( node==neighbors[d].neighbors[1][1][1] ) + { + bool reset = false; + for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) if( flags[i][j][k] && !neighbors[d].neighbors[i][j][k] ) reset = true; + if( reset ) neighbors[d].neighbors[1][1][1] = NULL; + } + if( node!=neighbors[d].neighbors[1][1][1] ) + { + neighbors[d].clear(); + + if( !node->parent ) neighbors[d].neighbors[1][1][1] = node; + else + { + int x1,y1,z1,x2,y2,z2; + int idx=int(node-node->parent->children); + Cube::FactorCornerIndex( idx ,x1,y1,z1); + Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); + for( int i=0 ; i<2 ; i++ ) + for( int j=0 ; j<2 ; j++ ) + for( int k=0 ; k<2 ; k++ ) + neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; + + Neighbors3& temp=setNeighbors( node->parent , flags ); + + // Set the neighbors from across the faces + { + int i=x1<<1; + if( temp.neighbors[i][1][1] ) + { + if( flags[i][1][1] && !temp.neighbors[i][1][1]->children ) temp.neighbors[i][1][1]->initChildren(); + if( temp.neighbors[i][1][1]->children ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; + } + } + { + int j = y1<<1; + if( temp.neighbors[1][j][1] ) + { + if( flags[1][j][1] && !temp.neighbors[1][j][1]->children ) temp.neighbors[1][j][1]->initChildren(); + if( temp.neighbors[1][j][1]->children ) for( int i=0 ; i<2 ; i++ ) for( int k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; + } + } + { + int k = z1<<1; + if( temp.neighbors[1][1][k] ) + { + if( flags[1][1][k] && !temp.neighbors[1][1][k]->children ) temp.neighbors[1][1][k]->initChildren(); + if( temp.neighbors[1][1][k]->children ) for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; + } + } + + // Set the neighbors from across the edges + { + int i=x1<<1 , j=y1<<1; + if( temp.neighbors[i][j][1] ) + { + if( flags[i][j][1] && !temp.neighbors[i][j][1]->children ) temp.neighbors[i][j][1]->initChildren(); + if( temp.neighbors[i][j][1]->children ) for( int k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; + } + } + { + int i=x1<<1 , k=z1<<1; + if( temp.neighbors[i][1][k] ) + { + if( flags[i][1][k] && !temp.neighbors[i][1][k]->children ) temp.neighbors[i][1][k]->initChildren(); + if( temp.neighbors[i][1][k]->children ) for( int j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; + } + } + { + int j=y1<<1 , k=z1<<1; + if( temp.neighbors[1][j][k] ) + { + if( flags[1][j][k] && !temp.neighbors[1][j][k]->children ) temp.neighbors[1][j][k]->initChildren(); + if( temp.neighbors[1][j][k]->children ) for( int i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; + } + } + + // Set the neighbor from across the corner + { + int i=x1<<1 , j=y1<<1 , k=z1<<1; + if( temp.neighbors[i][j][k] ) + { + if( flags[i][j][k] && !temp.neighbors[i][j][k]->children ) temp.neighbors[i][j][k]->initChildren(); + if( temp.neighbors[i][j][k]->children ) neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + } + } + } + } + return neighbors[d]; + } + + template + typename OctNode::Neighbors3& OctNode::NeighborKey3::getNeighbors(OctNode* node){ + int d=node->depth(); + if(node!=neighbors[d].neighbors[1][1][1]){ + neighbors[d].clear(); + + if(!node->parent){neighbors[d].neighbors[1][1][1]=node;} + else{ + int i,j,k,x1,y1,z1,x2,y2,z2; + int idx=int(node-node->parent->children); + Cube::FactorCornerIndex( idx ,x1,y1,z1); + Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); + for(i=0;i<2;i++){ + for(j=0;j<2;j++){ + for(k=0;k<2;k++){ + neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; + } + } + } + Neighbors3& temp=getNeighbors(node->parent); + + // Set the neighbors from across the faces + i=x1<<1; + if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){ + for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} + } + j=y1<<1; + if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){ + for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} + } + k=z1<<1; + if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){ + for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} + } + + // Set the neighbors from across the edges + i=x1<<1; j=y1<<1; + if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){ + for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} + } + i=x1<<1; k=z1<<1; + if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){ + for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} + } + j=y1<<1; k=z1<<1; + if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){ + for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} + } + + // Set the neighbor from across the corner + i=x1<<1; j=y1<<1; k=z1<<1; + if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){ + neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + } + } + } + return neighbors[node->depth()]; + } + + /////////////////////// + // ConstNeighborKey3 // + /////////////////////// + template + OctNode::ConstNeighbors3::ConstNeighbors3(void){clear();} + template + void OctNode::ConstNeighbors3::clear(void){ + for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} + } + template + OctNode::ConstNeighborKey3::ConstNeighborKey3(void){neighbors=NULL;} + template + OctNode::ConstNeighborKey3::~ConstNeighborKey3(void){ + if(neighbors){delete[] neighbors;} + neighbors=NULL; + } + + template + void OctNode::ConstNeighborKey3::set(int d){ + if(neighbors){delete[] neighbors;} + neighbors=NULL; + if(d<0){return;} + neighbors=new ConstNeighbors3[d+1]; + } + template + typename OctNode::ConstNeighbors3& OctNode::ConstNeighborKey3::getNeighbors(const OctNode* node){ + int d=node->depth(); + if(node!=neighbors[d].neighbors[1][1][1]){ + neighbors[d].clear(); + + if(!node->parent){neighbors[d].neighbors[1][1][1]=node;} + else{ + int i,j,k,x1,y1,z1,x2,y2,z2; + int idx=int(node-node->parent->children); + Cube::FactorCornerIndex( idx ,x1,y1,z1); + Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); + for(i=0;i<2;i++){ + for(j=0;j<2;j++){ + for(k=0;k<2;k++){ + neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; + } + } + } + ConstNeighbors3& temp=getNeighbors(node->parent); + + // Set the neighbors from across the faces + i=x1<<1; + if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){ + for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} + } + j=y1<<1; + if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){ + for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} + } + k=z1<<1; + if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){ + for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} + } + + // Set the neighbors from across the edges + i=x1<<1; j=y1<<1; + if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){ + for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} + } + i=x1<<1; k=z1<<1; + if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){ + for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} + } + j=y1<<1; k=z1<<1; + if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){ + for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} + } + + // Set the neighbor from across the corner + i=x1<<1; j=y1<<1; k=z1<<1; + if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){ + neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + } + } + } + return neighbors[node->depth()]; + } + template + typename OctNode::ConstNeighbors3& OctNode::ConstNeighborKey3::getNeighbors( const OctNode* node , int minDepth ) + { + int d=node->depth(); + if (d < minDepth) + { + POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Node depth lower than min-depth: (actual)" << d << " < (minimum)" << minDepth); + } + if( node!=neighbors[d].neighbors[1][1][1] ) + { + neighbors[d].clear(); + + if( d==minDepth ) neighbors[d].neighbors[1][1][1]=node; + else + { + int i,j,k,x1,y1,z1,x2,y2,z2; + int idx = int(node-node->parent->children); + Cube::FactorCornerIndex( idx ,x1,y1,z1); + Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); + + ConstNeighbors3& temp=getNeighbors( node->parent , minDepth ); + + // Set the syblings + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + neighbors[d].neighbors[x2+i][y2+j][z2+k] = &node->parent->children[ Cube::CornerIndex(i,j,k) ]; + + // Set the neighbors from across the faces + i=x1<<1; + if( temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children ) + for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][y2+j][z2+k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)]; + + j=y1<<1; + if( temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children ) + for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[x2+i][j][z2+k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)]; + + k=z1<<1; + if( temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[x2+i][y2+j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)]; + + // Set the neighbors from across the edges + i=x1<<1 , j=y1<<1; + if( temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children ) + for( k=0 ; k<2 ; k++ ) neighbors[d].neighbors[i][j][z2+k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)]; + + i=x1<<1 , k=z1<<1; + if( temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children ) + for( j=0 ; j<2 ; j++ ) neighbors[d].neighbors[i][y2+j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)]; + + j=y1<<1 , k=z1<<1; + if( temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children ) + for( i=0 ; i<2 ; i++ ) neighbors[d].neighbors[x2+i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)]; + + // Set the neighbor from across the corner + i=x1<<1 , j=y1<<1 , k=z1<<1; + if( temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children ) + neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + } + } + return neighbors[node->depth()]; + } + + template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); } + template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); } + template< class NodeData , class Real > + void OctNode< NodeData , Real >::Neighbors5::clear( void ) + { + for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL; + } + template< class NodeData , class Real > + void OctNode< NodeData , Real >::ConstNeighbors5::clear( void ) + { + for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL; + } + template< class NodeData , class Real > + OctNode< NodeData , Real >::NeighborKey5::NeighborKey5( void ) + { + _depth = -1; + neighbors = NULL; + } + template< class NodeData , class Real > + OctNode< NodeData , Real >::ConstNeighborKey5::ConstNeighborKey5( void ) + { + _depth = -1; + neighbors = NULL; + } + template< class NodeData , class Real > + OctNode< NodeData , Real >::NeighborKey5::~NeighborKey5( void ) + { + if( neighbors ) delete[] neighbors; + neighbors = NULL; + } + template< class NodeData , class Real > + OctNode< NodeData , Real >::ConstNeighborKey5::~ConstNeighborKey5( void ) + { + if( neighbors ) delete[] neighbors; + neighbors = NULL; + } + template< class NodeData , class Real > + void OctNode< NodeData , Real >::NeighborKey5::set( int d ) + { + if( neighbors ) delete[] neighbors; + neighbors = NULL; + if(d<0) return; + _depth = d; + neighbors=new Neighbors5[d+1]; + } + template< class NodeData , class Real > + void OctNode< NodeData , Real >::ConstNeighborKey5::set( int d ) + { + if( neighbors ) delete[] neighbors; + neighbors = NULL; + if(d<0) return; + _depth = d; + neighbors=new ConstNeighbors5[d+1]; + } + template< class NodeData , class Real > + typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::getNeighbors( OctNode* node ) + { + int d=node->depth(); + if( node!=neighbors[d].neighbors[2][2][2] ) + { + neighbors[d].clear(); + + if( !node->parent ) neighbors[d].neighbors[2][2][2]=node; + else + { + getNeighbors( node->parent ); + Neighbors5& temp = neighbors[d-1]; + int x1 , y1 , z1 , x2 , y2 , z2; + int idx = int( node - node->parent->children ); + Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); + + Neighbors5& n = neighbors[d]; + Cube::FactorCornerIndex( (~idx)&7 , x2 , y2 , z2 ); + int i , j , k; + int fx0 = x2+1 , fy0 = y2+1 , fz0 = z2+1; // Indices of the bottom left corner of the parent within the 5x5x5 + int cx1 = x1*2+1 , cy1 = y1*2+1 , cz1 = z1*2+1; + int cx2 = x2*2+1 , cy2 = y2*2+1 , cz2 = z2*2+1; + int fx1 = x1*3 , fy1 = y1*3 , fz1 = z1*3; + int fx2 = x2*4 , fy2 = y2*4 , fz2 = z2*4; + + // Set the syblings + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx0+i][fy0+j][fz0+k] = node->parent->children + Cube::CornerIndex( i , j , k ); + + // Set the neighbors from across the faces + if( temp.neighbors[cx1][2][2] && temp.neighbors[cx1][2][2]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx1+i][fy0+j][fz0+k] = temp.neighbors[cx1][2][2]->children + Cube::CornerIndex( i , j , k ); + if( temp.neighbors[2][cy1][2] && temp.neighbors[2][cy1][2]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx0+i][fy1+j][fz0+k] = temp.neighbors[2][cy1][2]->children + Cube::CornerIndex( i , j , k ); + if( temp.neighbors[2][2][cz1] && temp.neighbors[2][2][cz1]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx0+i][fy0+j][fz1+k] = temp.neighbors[2][2][cz1]->children + Cube::CornerIndex( i , j , k ); + if( temp.neighbors[cx2][2][2] && temp.neighbors[cx2][2][2]->children ) + for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx2 ][fy0+j][fz0+k] = temp.neighbors[cx2][2][2]->children + Cube::CornerIndex( x1 , j , k ); + if( temp.neighbors[2][cy2][2] && temp.neighbors[2][cy2][2]->children ) + for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx0+i][fy2 ][fz0+k] = temp.neighbors[2][cy2][2]->children + Cube::CornerIndex( i , y1 , k ); + if( temp.neighbors[2][2][cz2] && temp.neighbors[2][2][cz2]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) + n.neighbors[fx0+i][fy0+j][fz2 ] = temp.neighbors[2][2][cz2]->children + Cube::CornerIndex( i , j , z1 ); + + // Set the neighbors from across the edges + if( temp.neighbors[cx1][cy1][2] && temp.neighbors[cx1][cy1][2]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx1+i][fy1+j][fz0+k] = temp.neighbors[cx1][cy1][2]->children + Cube::CornerIndex( i , j , k ); + if( temp.neighbors[cx1][2][cz1] && temp.neighbors[cx1][2][cz1]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx1+i][fy0+j][fz1+k] = temp.neighbors[cx1][2][cz1]->children + Cube::CornerIndex( i , j , k ); + if( temp.neighbors[2][cy1][cz1] && temp.neighbors[2][cy1][cz1]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx0+i][fy1+j][fz1+k] = temp.neighbors[2][cy1][cz1]->children + Cube::CornerIndex( i , j , k ); + if( temp.neighbors[cx1][cy2][2] && temp.neighbors[cx1][cy2][2]->children ) + for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx1+i][fy2 ][fz0+k] = temp.neighbors[cx1][cy2][2]->children + Cube::CornerIndex( i , y1 , k ); + if( temp.neighbors[cx1][2][cz2] && temp.neighbors[cx1][2][cz2]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) + n.neighbors[fx1+i][fy0+j][fz2 ] = temp.neighbors[cx1][2][cz2]->children + Cube::CornerIndex( i , j , z1 ); + if( temp.neighbors[cx2][cy1][2] && temp.neighbors[cx2][cy1][2]->children ) + for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx2 ][fy1+j][fz0+k] = temp.neighbors[cx2][cy1][2]->children + Cube::CornerIndex( x1 , j , k ); + if( temp.neighbors[2][cy1][cz2] && temp.neighbors[2][cy1][cz2]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) + n.neighbors[fx0+i][fy1+j][fz2 ] = temp.neighbors[2][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 ); + if( temp.neighbors[cx2][2][cz1] && temp.neighbors[cx2][2][cz1]->children ) + for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx2 ][fy0+j][fz1+k] = temp.neighbors[cx2][2][cz1]->children + Cube::CornerIndex( x1 , j , k ); + if( temp.neighbors[2][cy2][cz1] && temp.neighbors[2][cy2][cz1]->children ) + for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx0+i][fy2 ][fz1+k] = temp.neighbors[2][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k ); + if( temp.neighbors[cx2][cy2][2] && temp.neighbors[cx2][cy2][2]->children ) + for( k=0 ; k<2 ; k++ ) + n.neighbors[fx2 ][fy2 ][fz0+k] = temp.neighbors[cx2][cy2][2]->children + Cube::CornerIndex( x1 , y1 , k ); + if( temp.neighbors[cx2][2][cz2] && temp.neighbors[cx2][2][cz2]->children ) + for( j=0 ; j<2 ; j++ ) + n.neighbors[fx2 ][fy0+j][fz2 ] = temp.neighbors[cx2][2][cz2]->children + Cube::CornerIndex( x1 , j , z1 ); + if( temp.neighbors[2][cy2][cz2] && temp.neighbors[2][cy2][cz2]->children ) + for( i=0 ; i<2 ; i++ ) + n.neighbors[fx0+i][fy2 ][fz2 ] = temp.neighbors[2][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 ); + + // Set the neighbor from across the corners + if( temp.neighbors[cx1][cy1][cz1] && temp.neighbors[cx1][cy1][cz1]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx1+i][fy1+j][fz1+k] = temp.neighbors[cx1][cy1][cz1]->children + Cube::CornerIndex( i , j , k ); + if( temp.neighbors[cx1][cy1][cz2] && temp.neighbors[cx1][cy1][cz2]->children ) + for( i=0 ; i<2 ; i++ ) for( j=0 ; j<2 ; j++ ) + n.neighbors[fx1+i][fy1+j][fz2 ] = temp.neighbors[cx1][cy1][cz2]->children + Cube::CornerIndex( i , j , z1 ); + if( temp.neighbors[cx1][cy2][cz1] && temp.neighbors[cx1][cy2][cz1]->children ) + for( i=0 ; i<2 ; i++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx1+i][fy2 ][fz1+k] = temp.neighbors[cx1][cy2][cz1]->children + Cube::CornerIndex( i , y1 , k ); + if( temp.neighbors[cx2][cy1][cz1] && temp.neighbors[cx2][cy1][cz1]->children ) + for( j=0 ; j<2 ; j++ ) for( k=0 ; k<2 ; k++ ) + n.neighbors[fx2 ][fy1+j][fz1+k] = temp.neighbors[cx2][cy1][cz1]->children + Cube::CornerIndex( x1 , j , k ); + if( temp.neighbors[cx1][cy2][cz2] && temp.neighbors[cx1][cy2][cz2]->children ) + for( i=0 ; i<2 ; i++ ) + n.neighbors[fx1+i][fy2 ][fz2 ] = temp.neighbors[cx1][cy2][cz2]->children + Cube::CornerIndex( i , y1 , z1 ); + if( temp.neighbors[cx2][cy1][cz2] && temp.neighbors[cx2][cy1][cz2]->children ) + for( j=0 ; j<2 ; j++ ) + n.neighbors[fx2 ][fy1+j][fz2 ] = temp.neighbors[cx2][cy1][cz2]->children + Cube::CornerIndex( x1 , j , z1 ); + if( temp.neighbors[cx2][cy2][cz1] && temp.neighbors[cx2][cy2][cz1]->children ) + for( k=0 ; k<2 ; k++ ) + n.neighbors[fx2 ][fy2 ][fz1+k] = temp.neighbors[cx2][cy2][cz1]->children + Cube::CornerIndex( x1 , y1 , k ); + if( temp.neighbors[cx2][cy2][cz2] && temp.neighbors[cx2][cy2][cz2]->children ) + n.neighbors[fx2 ][fy2 ][fz2 ] = temp.neighbors[cx2][cy2][cz2]->children + Cube::CornerIndex( x1 , y1 , z1 ); + } + } + return neighbors[d]; + } + template< class NodeData , class Real > + typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) + { + int d=node->depth(); + if( node!=neighbors[d].neighbors[2][2][2] ) + { + neighbors[d].clear(); + + if( !node->parent ) neighbors[d].neighbors[2][2][2]=node; + else + { + setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd ); + Neighbors5& temp = neighbors[d-1]; + int x1 , y1 , z1 , x2 , y2 , z2 , ii , jj , kk; + int idx = int( node-node->parent->children ); + Cube::FactorCornerIndex( idx , x1 , y1 , z1 ); + + for( int i=xStart ; i>1); + for( int j=yStart ; j>1); + for( int k=zStart ; k>1); + if(temp.neighbors[x2][y2][z2] ) + { + if( !temp.neighbors[x2][y2][z2]->children ) temp.neighbors[x2][y2][z2]->initChildren(); + neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk); + } + } + } + } + } + } + return neighbors[d]; + } + template< class NodeData , class Real > + typename OctNode< NodeData , Real >::ConstNeighbors5& OctNode< NodeData , Real >::ConstNeighborKey5::getNeighbors( const OctNode* node ) + { + int d=node->depth(); + if( node!=neighbors[d].neighbors[2][2][2] ) + { + neighbors[d].clear(); + + if(!node->parent) neighbors[d].neighbors[2][2][2]=node; + else + { + getNeighbors( node->parent ); + ConstNeighbors5& temp = neighbors[d-1]; + int x1,y1,z1,x2,y2,z2,ii,jj,kk; + int idx=int(node-node->parent->children); + Cube::FactorCornerIndex(idx,x1,y1,z1); + + for(int i=0;i<5;i++) + { + x2=i+x1; + ii=x2&1; + x2=1+(x2>>1); + for(int j=0;j<5;j++) + { + y2=j+y1; + jj=y2&1; + y2=1+(y2>>1); + for(int k=0;k<5;k++) + { + z2=k+z1; + kk=z2&1; + z2=1+(z2>>1); + if(temp.neighbors[x2][y2][z2] && temp.neighbors[x2][y2][z2]->children) + neighbors[d].neighbors[i][j][k] = temp.neighbors[x2][y2][z2]->children + Cube::CornerIndex(ii,jj,kk); + } + } + } + } + } + return neighbors[d]; + } + + + template + int OctNode::write(const char* fileName) const{ + FILE* fp=fopen(fileName,"wb"); + if(!fp){return 0;} + int ret=write(fp); + fclose(fp); + return ret; + } + template + int OctNode::write(FILE* fp) const{ + fwrite(this,sizeof(OctNode),1,fp); + if(children){for(int i=0;i + int OctNode::read(const char* fileName){ + FILE* fp=fopen(fileName,"rb"); + if(!fp){return 0;} + int ret=read(fp); + fclose(fp); + return ret; + } + template + int OctNode::read(FILE* fp){ + fread(this,sizeof(OctNode),1,fp); + parent=NULL; + if(children){ + children=NULL; + initChildren(); + for(int i=0;i + int OctNode::width(int maxDepth) const { + int d=depth(); + return 1<<(maxDepth-d); + } + template + void OctNode::centerIndex(int maxDepth,int index[DIMENSION]) const { + int d,o[3]; + depthAndOffset(d,o); + for(int i=0;i::CornerIndex(maxDepth,d+1,o[i]<<1,1);} + } + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/poisson_exceptions.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/poisson_exceptions.h new file mode 100755 index 0000000..6a861a6 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/poisson_exceptions.h @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2019-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +/** POISSON_THROW_EXCEPTION is a helper macro to be used for throwing exceptions. e.g. + * POISSON_THROW_EXCEPTION (PoissonBadArgumentException, "[ERROR] B-spline up-sampling not supported for degree " << Degree); + * + * \note + * Adapted from PCL_THROW_EXCEPTION. We intentionally do not reuse PCL_THROW_EXCEPTION here + * to avoid introducing any dependencies on PCL in this 3rd party module. + */ +#define POISSON_THROW_EXCEPTION(ExceptionName, message) \ +{ \ + std::ostringstream s; \ + s << message; \ + throw ExceptionName(s.str(), __FILE__, BOOST_CURRENT_FUNCTION, __LINE__); \ +} + +namespace pcl +{ + namespace poisson + { + /** \class PoissonException + * \brief A base class for all poisson exceptions which inherits from std::runtime_error + * + * \note + * Adapted from PCLException. We intentionally do not reuse PCLException here + * to avoid introducing any dependencies on PCL in this 3rd party module. + */ + class PoissonException : public std::runtime_error + { + public: + PoissonException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : std::runtime_error (createDetailedMessage (error_description, + file_name, + function_name, + line_number)) + , file_name_ (file_name) + , function_name_ (function_name) + , line_number_ (line_number) + {} + + protected: + static std::string + createDetailedMessage (const std::string& error_description, + const char* file_name, + const char* function_name, + unsigned line_number) + { + std::ostringstream sstream; + if (function_name) + sstream << function_name << ' '; + + if (file_name) + { + sstream << "in " << file_name << ' '; + if (line_number) + sstream << "@ " << line_number << ' '; + } + sstream << ": " << error_description; + + return (sstream.str ()); + } + + const char* file_name_; + const char* function_name_; + unsigned line_number_; + }; + + /** \class PoissonBadArgumentException + * \brief An exception that is thrown when the arguments number or type is wrong/unhandled. + */ + class PoissonBadArgumentException : public PoissonException + { + public: + PoissonBadArgumentException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::poisson::PoissonException (error_description, file_name, function_name, line_number) {} + }; + + /** \class PoissonOpenMPException + * \brief An exception that is thrown when something goes wrong inside an openMP for loop. + */ + class PoissonOpenMPException : public PoissonException + { + public: + PoissonOpenMPException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::poisson::PoissonException (error_description, file_name, function_name, line_number) {} + }; + + /** \class PoissonBadInitException + * \brief An exception that is thrown when initialization fails. + */ + class PoissonBadInitException : public PoissonException + { + public: + PoissonBadInitException (const std::string& error_description, + const char* file_name = nullptr, + const char* function_name = nullptr, + unsigned line_number = 0) + : pcl::poisson::PoissonException (error_description, file_name, function_name, line_number) {} + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/polynomial.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/polynomial.h new file mode 100755 index 0000000..f43f5db --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/polynomial.h @@ -0,0 +1,102 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef POLYNOMIAL_INCLUDED +#define POLYNOMIAL_INCLUDED + +#include + +namespace pcl +{ + namespace poisson + { + + template< int Degree > + class Polynomial{ + public: + double coefficients[Degree+1]; + + Polynomial(void); + template + Polynomial(const Polynomial& P); + double operator()( double t ) const; + double integral( double tMin , double tMax ) const; + + int operator == (const Polynomial& p) const; + int operator != (const Polynomial& p) const; + int isZero(void) const; + void setZero(void); + + template + Polynomial& operator = (const Polynomial &p); + Polynomial& operator += (const Polynomial& p); + Polynomial& operator -= (const Polynomial& p); + Polynomial operator - (void) const; + Polynomial operator + (const Polynomial& p) const; + Polynomial operator - (const Polynomial& p) const; + template + Polynomial operator * (const Polynomial& p) const; + + Polynomial& operator += ( double s ); + Polynomial& operator -= ( double s ); + Polynomial& operator *= ( double s ); + Polynomial& operator /= ( double s ); + Polynomial operator + ( double s ) const; + Polynomial operator - ( double s ) const; + Polynomial operator * ( double s ) const; + Polynomial operator / ( double s ) const; + + Polynomial scale( double s ) const; + Polynomial shift( double t ) const; + + Polynomial derivative(void) const; + Polynomial integral(void) const; + + void printnl(void) const; + + Polynomial& addScaled(const Polynomial& p,double scale); + + static void Negate(const Polynomial& in,Polynomial& out); + static void Subtract(const Polynomial& p1,const Polynomial& p2,Polynomial& q); + static void Scale(const Polynomial& p,double w,Polynomial& q); + static void AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,double w2,Polynomial& q); + static void AddScaled(const Polynomial& p1,const Polynomial& p2,double w2,Polynomial& q); + static void AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,Polynomial& q); + + void getSolutions(double c,std::vector& roots,double EPS) const; + + static Polynomial BSplineComponent( int i ); + }; + + + } +} + + +#include "polynomial.hpp" +#endif // POLYNOMIAL_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/polynomial.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/polynomial.hpp new file mode 100755 index 0000000..45973db --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/polynomial.hpp @@ -0,0 +1,325 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#include +#include +#include +#include "factor.h" + +//////////////// +// Polynomial // +//////////////// + +namespace pcl +{ + namespace poisson + { + + + template + Polynomial::Polynomial(void){memset(coefficients,0,sizeof(double)*(Degree+1));} + template + template + Polynomial::Polynomial(const Polynomial& P){ + memset(coefficients,0,sizeof(double)*(Degree+1)); + for(int i=0;i<=Degree && i<=Degree2;i++){coefficients[i]=P.coefficients[i];} + } + + + template + template + Polynomial& Polynomial::operator = (const Polynomial &p){ + int d=Degree + Polynomial Polynomial::derivative(void) const{ + Polynomial p; + for(int i=0;i + Polynomial Polynomial::integral(void) const{ + Polynomial p; + p.coefficients[0]=0; + for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);} + return p; + } + template<> double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; } + template<> double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; } + template<> double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; } + template + double Polynomial::operator() ( double t ) const{ + double v=coefficients[Degree]; + for( int d=Degree-1 ; d>=0 ; d-- ) v = v*t + coefficients[d]; + return v; + } + template + double Polynomial::integral( double tMin , double tMax ) const + { + double v=0; + double t1,t2; + t1=tMin; + t2=tMax; + for(int i=0;i<=Degree;i++){ + v+=coefficients[i]*(t2-t1)/(i+1); + if(t1!=-DBL_MAX && t1!=DBL_MAX){t1*=tMin;} + if(t2!=-DBL_MAX && t2!=DBL_MAX){t2*=tMax;} + } + return v; + } + template + int Polynomial::operator == (const Polynomial& p) const{ + for(int i=0;i<=Degree;i++){if(coefficients[i]!=p.coefficients[i]){return 0;}} + return 1; + } + template + int Polynomial::operator != (const Polynomial& p) const{ + for(int i=0;i<=Degree;i++){if(coefficients[i]==p.coefficients[i]){return 0;}} + return 1; + } + template + int Polynomial::isZero(void) const{ + for(int i=0;i<=Degree;i++){if(coefficients[i]!=0){return 0;}} + return 1; + } + template + void Polynomial::setZero(void){memset(coefficients,0,sizeof(double)*(Degree+1));} + + template + Polynomial& Polynomial::addScaled(const Polynomial& p,double s){ + for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i]*s;} + return *this; + } + template + Polynomial& Polynomial::operator += (const Polynomial& p){ + for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i];} + return *this; + } + template + Polynomial& Polynomial::operator -= (const Polynomial& p){ + for(int i=0;i<=Degree;i++){coefficients[i]-=p.coefficients[i];} + return *this; + } + template + Polynomial Polynomial::operator + (const Polynomial& p) const{ + Polynomial q; + for(int i=0;i<=Degree;i++){q.coefficients[i]=(coefficients[i]+p.coefficients[i]);} + return q; + } + template + Polynomial Polynomial::operator - (const Polynomial& p) const{ + Polynomial q; + for(int i=0;i<=Degree;i++) {q.coefficients[i]=coefficients[i]-p.coefficients[i];} + return q; + } + template + void Polynomial::Scale(const Polynomial& p,double w,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p.coefficients[i]*w;} + } + template + void Polynomial::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,double w2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i]*w2;} + } + template + void Polynomial::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i];} + } + template + void Polynomial::AddScaled(const Polynomial& p1,const Polynomial& p2,double w2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]+p2.coefficients[i]*w2;} + } + + template + void Polynomial::Subtract(const Polynomial &p1,const Polynomial& p2,Polynomial& q){ + for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]-p2.coefficients[i];} + } + template + void Polynomial::Negate(const Polynomial& in,Polynomial& out){ + out=in; + for(int i=0;i<=Degree;i++){out.coefficients[i]=-out.coefficients[i];} + } + + template + Polynomial Polynomial::operator - (void) const{ + Polynomial q=*this; + for(int i=0;i<=Degree;i++){q.coefficients[i]=-q.coefficients[i];} + return q; + } + template + template + Polynomial Polynomial::operator * (const Polynomial& p) const{ + Polynomial q; + for(int i=0;i<=Degree;i++){for(int j=0;j<=Degree2;j++){q.coefficients[i+j]+=coefficients[i]*p.coefficients[j];}} + return q; + } + + template + Polynomial& Polynomial::operator += ( double s ) + { + coefficients[0]+=s; + return *this; + } + template + Polynomial& Polynomial::operator -= ( double s ) + { + coefficients[0]-=s; + return *this; + } + template + Polynomial& Polynomial::operator *= ( double s ) + { + for(int i=0;i<=Degree;i++){coefficients[i]*=s;} + return *this; + } + template + Polynomial& Polynomial::operator /= ( double s ) + { + for(int i=0;i<=Degree;i++){coefficients[i]/=s;} + return *this; + } + template + Polynomial Polynomial::operator + ( double s ) const + { + Polynomial q=*this; + q.coefficients[0]+=s; + return q; + } + template + Polynomial Polynomial::operator - ( double s ) const + { + Polynomial q=*this; + q.coefficients[0]-=s; + return q; + } + template + Polynomial Polynomial::operator * ( double s ) const + { + Polynomial q; + for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]*s;} + return q; + } + template + Polynomial Polynomial::operator / ( double s ) const + { + Polynomial q; + for( int i=0 ; i<=Degree ; i++ ) q.coefficients[i] = coefficients[i]/s; + return q; + } + template + Polynomial Polynomial::scale( double s ) const + { + Polynomial q=*this; + double s2=1.0; + for(int i=0;i<=Degree;i++){ + q.coefficients[i]*=s2; + s2/=s; + } + return q; + } + template + Polynomial Polynomial::shift( double t ) const + { + Polynomial q; + for(int i=0;i<=Degree;i++){ + double temp=1; + for(int j=i;j>=0;j--){ + q.coefficients[j]+=coefficients[i]*temp; + temp*=-t*j; + temp/=(i-j+1); + } + } + return q; + } + template + void Polynomial::printnl(void) const{ + for(int j=0;j<=Degree;j++){ + printf("%6.4f x^%d ",coefficients[j],j); + if(j=0){printf("+");} + } + printf("\n"); + } + template + void Polynomial::getSolutions(double c,std::vector& roots,double EPS) const + { + double r[4][2]; + int rCount=0; + roots.clear(); + switch(Degree){ + case 1: + rCount=Factor(coefficients[1],coefficients[0]-c,r,EPS); + break; + case 2: + rCount=Factor(coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); + break; + case 3: + rCount=Factor(coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); + break; + // case 4: + // rCount=Factor(coefficients[4],coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS); + // break; + default: + printf("Can't solve polynomial of degree: %d\n",Degree); + } + for(int i=0;i + Polynomial< 0 > Polynomial< 0 >::BSplineComponent( int i ) + { + Polynomial p; + p.coefficients[0] = 1.; + return p; + } + template< int Degree > + Polynomial< Degree > Polynomial< Degree >::BSplineComponent( int i ) + { + Polynomial p; + if( i>0 ) + { + Polynomial< Degree > _p = Polynomial< Degree-1 >::BSplineComponent( i-1 ).integral(); + p -= _p; + p.coefficients[0] += _p(1); + } + if( i _p = Polynomial< Degree-1 >::BSplineComponent( i ).integral(); + p += _p; + } + return p; + } + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/ppolynomial.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/ppolynomial.h new file mode 100755 index 0000000..d1ea987 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/ppolynomial.h @@ -0,0 +1,128 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef P_POLYNOMIAL_INCLUDED +#define P_POLYNOMIAL_INCLUDED + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include "polynomial.h" + +namespace pcl +{ + namespace poisson + { + template + class StartingPolynomial{ + public: + Polynomial p; + double start; + + template + StartingPolynomial operator * (const StartingPolynomial& p) const; + StartingPolynomial scale(double s) const; + StartingPolynomial shift(double t) const; + int operator < (const StartingPolynomial& sp) const; + static int Compare(const void* v1,const void* v2); + }; + + template + class PPolynomial + { + public: + std::size_t polyCount; + StartingPolynomial* polys; + + PPolynomial(void); + PPolynomial(const PPolynomial& p); + ~PPolynomial(void); + + PPolynomial& operator = (const PPolynomial& p); + + int size(void) const; + + void set( std::size_t size ); + // Note: this method will sort the elements in sps + void set( StartingPolynomial* sps , int count ); + void reset( std::size_t newSize ); + + + double operator()( double t ) const; + double integral( double tMin , double tMax ) const; + double Integral( void ) const; + + template + PPolynomial& operator = (const PPolynomial& p); + + PPolynomial operator + (const PPolynomial& p) const; + PPolynomial operator - (const PPolynomial& p) const; + + template + PPolynomial operator * (const Polynomial& p) const; + + template + PPolynomial operator * (const PPolynomial& p) const; + + + PPolynomial& operator += ( double s ); + PPolynomial& operator -= ( double s ); + PPolynomial& operator *= ( double s ); + PPolynomial& operator /= ( double s ); + PPolynomial operator + ( double s ) const; + PPolynomial operator - ( double s ) const; + PPolynomial operator * ( double s ) const; + PPolynomial operator / ( double s ) const; + + PPolynomial& addScaled(const PPolynomial& poly,double scale); + + PPolynomial scale( double s ) const; + PPolynomial shift( double t ) const; + + PPolynomial< Degree-1 > derivative(void) const; + PPolynomial< Degree+1 > integral(void) const; + + void getSolutions(double c,std::vector& roots,double EPS,double min=-DBL_MAX,double max=DBL_MAX) const; + + void printnl( void ) const; + + PPolynomial< Degree+1 > MovingAverage( double radius ); + static PPolynomial BSpline( double radius=0.5 ); + + void write( FILE* fp , int samples , double min , double max ) const; + }; + + + } +} + + +#include "ppolynomial.hpp" +#endif // P_POLYNOMIAL_INCLUDED diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/ppolynomial.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/ppolynomial.hpp new file mode 100755 index 0000000..5f9de7d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/ppolynomial.hpp @@ -0,0 +1,441 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#include "factor.h" + +//////////////////////// +// StartingPolynomial // +//////////////////////// + +namespace pcl +{ + namespace poisson + { + + + template + template + StartingPolynomial StartingPolynomial::operator * (const StartingPolynomial& p) const{ + StartingPolynomial sp; + if(start>p.start){sp.start=start;} + else{sp.start=p.start;} + sp.p=this->p*p.p; + return sp; + } + template + StartingPolynomial StartingPolynomial::scale(double s) const{ + StartingPolynomial q; + q.start=start*s; + q.p=p.scale(s); + return q; + } + template + StartingPolynomial StartingPolynomial::shift(double s) const{ + StartingPolynomial q; + q.start=start+s; + q.p=p.shift(s); + return q; + } + + + template + int StartingPolynomial::operator < (const StartingPolynomial& sp) const{ + if(start + int StartingPolynomial::Compare(const void* v1,const void* v2){ + double d=((StartingPolynomial*)(v1))->start-((StartingPolynomial*)(v2))->start; + if (d<0) {return -1;} + else if (d>0) {return 1;} + else {return 0;} + } + + ///////////////// + // PPolynomial // + ///////////////// + template + PPolynomial::PPolynomial(void){ + polyCount=0; + polys=NULL; + } + template + PPolynomial::PPolynomial(const PPolynomial& p){ + polyCount=0; + polys=NULL; + set(p.polyCount); + memcpy(polys,p.polys,sizeof(StartingPolynomial)*p.polyCount); + } + + template + PPolynomial::~PPolynomial(void){ + if(polyCount){free(polys);} + polyCount=0; + polys=NULL; + } + template + void PPolynomial::set(StartingPolynomial* sps,int count){ + int i,c=0; + set(count); + qsort(sps,count,sizeof(StartingPolynomial),StartingPolynomial::Compare); + for( i=0 ; i + int PPolynomial::size(void) const{return int(sizeof(StartingPolynomial)*polyCount);} + + template + void PPolynomial::set( std::size_t size ) + { + if(polyCount){free(polys);} + polyCount=0; + polys=NULL; + polyCount=size; + if(size){ + polys=(StartingPolynomial*)malloc(sizeof(StartingPolynomial)*size); + memset(polys,0,sizeof(StartingPolynomial)*size); + } + } + template + void PPolynomial::reset( std::size_t newSize ) + { + polyCount=newSize; + polys=(StartingPolynomial*)realloc(polys,sizeof(StartingPolynomial)*newSize); + } + + template + PPolynomial& PPolynomial::operator = (const PPolynomial& p){ + set(p.polyCount); + memcpy(polys,p.polys,sizeof(StartingPolynomial)*p.polyCount); + return *this; + } + + template + template + PPolynomial& PPolynomial::operator = (const PPolynomial& p){ + set(p.polyCount); + for(int i=0;i + double PPolynomial::operator ()( double t ) const + { + double v=0; + for( int i=0 ; ipolys[i].start ; i++ ) v+=polys[i].p(t); + return v; + } + + template + double PPolynomial::integral( double tMin , double tMax ) const + { + int m=1; + double start,end,s,v=0; + start=tMin; + end=tMax; + if(tMin>tMax){ + m=-1; + start=tMax; + end=tMin; + } + for(int i=0;i + double PPolynomial::Integral(void) const{return integral(polys[0].start,polys[polyCount-1].start);} + template + PPolynomial PPolynomial::operator + (const PPolynomial& p) const{ + PPolynomial q; + int i,j; + std::size_t idx=0; + q.set(polyCount+p.polyCount); + i=j=-1; + + while(idx=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} + else if (i>=int( polyCount)-1) {q.polys[idx]=p.polys[++j];} + else if(polys[i+1].start + PPolynomial PPolynomial::operator - (const PPolynomial& p) const{ + PPolynomial q; + int i,j; + std::size_t idx=0; + q.set(polyCount+p.polyCount); + i=j=-1; + + while(idx=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} + else if (i>=int( polyCount)-1) {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);} + else if(polys[i+1].start + PPolynomial& PPolynomial::addScaled(const PPolynomial& p,double scale){ + int i,j; + StartingPolynomial* oldPolys=polys; + std::size_t idx=0,cnt=0,oldPolyCount=polyCount; + polyCount=0; + polys=NULL; + set(oldPolyCount+p.polyCount); + i=j=-1; + while(cnt=int( p.polyCount)-1) {polys[idx]=oldPolys[++i];} + else if (i>=int(oldPolyCount)-1) {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;} + else if (oldPolys[i+1].start + template + PPolynomial PPolynomial::operator * (const PPolynomial& p) const{ + PPolynomial q; + StartingPolynomial *sp; + int i,j,spCount=int(polyCount*p.polyCount); + + sp=(StartingPolynomial*)malloc(sizeof(StartingPolynomial)*spCount); + for(i=0;i + template + PPolynomial PPolynomial::operator * (const Polynomial& p) const{ + PPolynomial q; + q.set(polyCount); + for(int i=0;i + PPolynomial PPolynomial::scale( double s ) const + { + PPolynomial q; + q.set(polyCount); + for(std::size_t i=0;i + PPolynomial PPolynomial::shift( double s ) const + { + PPolynomial q; + q.set(polyCount); + for(std::size_t i=0;i + PPolynomial PPolynomial::derivative(void) const{ + PPolynomial q; + q.set(polyCount); + for(std::size_t i=0;i + PPolynomial PPolynomial::integral(void) const{ + int i; + PPolynomial q; + q.set(polyCount); + for(i=0;i + PPolynomial& PPolynomial::operator += ( double s ) {polys[0].p+=s;} + template + PPolynomial& PPolynomial::operator -= ( double s ) {polys[0].p-=s;} + template + PPolynomial& PPolynomial::operator *= ( double s ) + { + for(int i=0;i + PPolynomial& PPolynomial::operator /= ( double s ) + { + for(std::size_t i=0;i + PPolynomial PPolynomial::operator + ( double s ) const + { + PPolynomial q=*this; + q+=s; + return q; + } + template + PPolynomial PPolynomial::operator - ( double s ) const + { + PPolynomial q=*this; + q-=s; + return q; + } + template + PPolynomial PPolynomial::operator * ( double s ) const + { + PPolynomial q=*this; + q*=s; + return q; + } + template + PPolynomial PPolynomial::operator / ( double s ) const + { + PPolynomial q=*this; + q/=s; + return q; + } + + template + void PPolynomial::printnl(void) const{ + Polynomial p; + + if(!polyCount){ + Polynomial p; + printf("[-Infinity,Infinity]\n"); + } + else{ + for(std::size_t i=0;i + PPolynomial< 0 > PPolynomial< 0 >::BSpline( double radius ) + { + PPolynomial q; + q.set(2); + + q.polys[0].start=-radius; + q.polys[1].start= radius; + + q.polys[0].p.coefficients[0]= 1.0; + q.polys[1].p.coefficients[0]=-1.0; + return q; + } + template< int Degree > + PPolynomial< Degree > PPolynomial::BSpline( double radius ) + { + return PPolynomial< Degree-1 >::BSpline().MovingAverage( radius ); + } + template + PPolynomial PPolynomial::MovingAverage( double radius ) + { + PPolynomial A; + Polynomial p; + StartingPolynomial* sps; + + sps=(StartingPolynomial*)malloc(sizeof(StartingPolynomial)*polyCount*2); + + for(int i=0;i + void PPolynomial::getSolutions(double c,std::vector& roots,double EPS,double min,double max) const{ + Polynomial p; + std::vector tempRoots; + + p.setZero(); + for(std::size_t i=0;imax){break;} + if(ipolys[i].start && (i+1==polyCount || tempRoots[j]<=polys[i+1].start)){ + if(tempRoots[j]>min && tempRoots[j] + void PPolynomial::write(FILE* fp,int samples,double min,double max) const{ + fwrite(&samples,sizeof(int),1,fp); + for(int i=0;i + struct MatrixEntry + { + MatrixEntry( void ) { N =-1; Value = 0; } + MatrixEntry( int i ) { N = i; Value = 0; } + int N; + T Value; + }; + + template class SparseMatrix + { + private: + bool _contiguous; + int _maxEntriesPerRow; + static int UseAlloc; + public: + static Allocator > internalAllocator; + static int UseAllocator(void); + static void SetAllocator( int blockSize ); + + int rows; + int* rowSizes; + MatrixEntry** m_ppElements; + MatrixEntry< T >* operator[] ( int idx ) { return m_ppElements[idx]; } + const MatrixEntry< T >* operator[] ( int idx ) const { return m_ppElements[idx]; } + + SparseMatrix( void ); + SparseMatrix( int rows ); + SparseMatrix( int rows , int maxEntriesPerRow ); + void Resize( int rows ); + void Resize( int rows , int maxEntriesPerRow ); + void SetRowSize( int row , int count ); + int Entries( void ) const; + + SparseMatrix( const SparseMatrix& M ); + ~SparseMatrix(); + + void SetZero(); + void SetIdentity(); + + SparseMatrix& operator = (const SparseMatrix& M); + + SparseMatrix operator * (const T& V) const; + SparseMatrix& operator *= (const T& V); + + + SparseMatrix operator * (const SparseMatrix& M) const; + SparseMatrix Multiply( const SparseMatrix& M ) const; + SparseMatrix MultiplyTranspose( const SparseMatrix& Mt ) const; + + template + Vector operator * (const Vector& V) const; + template + Vector Multiply( const Vector& V ) const; + template + void Multiply( const Vector& In , Vector& Out , int threads=1 ) const; + + + SparseMatrix Transpose() const; + + static int Solve (const SparseMatrix& M,const Vector& b, int iters,Vector& solution,const T eps=1e-8); + + template + static int SolveSymmetric( const SparseMatrix& M , const Vector& b , int iters , Vector& solution , const T2 eps=1e-8 , int reset=1 , int threads=1 ); + + bool write( FILE* fp ) const; + bool write( const char* fileName ) const; + bool read( FILE* fp ); + bool read( const char* fileName ); + }; + + template< class T2 > + struct MapReduceVector + { + private: + int _dim; + public: + std::vector< T2* > out; + MapReduceVector( void ) { _dim = 0; } + ~MapReduceVector( void ) + { + if( _dim ) for( int t=0 ; t + class SparseSymmetricMatrix : public SparseMatrix< T > + { + public: + + template< class T2 > + Vector< T2 > operator * ( const Vector& V ) const; + + template< class T2 > + Vector< T2 > Multiply( const Vector& V ) const; + + template< class T2 > + void Multiply( const Vector& In, Vector& Out , bool addDCTerm=false ) const; + + template< class T2 > + void Multiply( const Vector& In, Vector& Out , MapReduceVector< T2 >& OutScratch , bool addDCTerm=false ) const; + + template< class T2 > + void Multiply( const Vector& In, Vector& Out , std::vector< T2* >& OutScratch , const std::vector< int >& bounds ) const; + + template< class T2 > + static int Solve( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& solution , T2 eps=1e-8 , int reset=1 , int threads=0 , bool addDCTerm=false , bool solveNormal=false ); + + template< class T2 > + static int Solve( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& solution , MapReduceVector& scratch , T2 eps=1e-8 , int reset=1 , bool addDCTerm=false , bool solveNormal=false ); +#if defined _WIN32 && !defined __MINGW32__ + template< class T2 > + static int SolveAtomic( const SparseSymmetricMatrix& M , const Vector& b , int iters , Vector& solution , T2 eps=1e-8 , int reset=1 , int threads=0 , bool solveNormal=false ); +#endif // _WIN32 || __MINGW32__ + template + static int Solve( const SparseSymmetricMatrix& M , const Vector& diagonal , const Vector& b , int iters , Vector& solution , int reset=1 ); + + template< class T2 > + void getDiagonal( Vector< T2 >& diagonal ) const; + }; + + + } +} + + +#include "sparse_matrix.hpp" + +#endif + diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/sparse_matrix.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/sparse_matrix.hpp new file mode 100755 index 0000000..e8fae96 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/sparse_matrix.hpp @@ -0,0 +1,973 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#include +#ifdef _WIN32 +# ifndef WIN32_LEAN_AND_MEAN +# define WIN32_LEAN_AND_MEAN +# endif // WIN32_LEAN_AND_MEAN +# ifndef NOMINMAX +# define NOMINMAX +# endif // NOMINMAX +# include +#endif //_WIN32 + +/////////////////// +// SparseMatrix // +/////////////////// +/////////////////////////////////////////// +// Static Allocator Methods and Memebers // +/////////////////////////////////////////// + +namespace pcl +{ + namespace poisson + { + + + template int SparseMatrix::UseAlloc=0; + template Allocator > SparseMatrix::internalAllocator; + template int SparseMatrix::UseAllocator(void){return UseAlloc;} + template + void SparseMatrix::SetAllocator( int blockSize ) + { + if(blockSize>0){ + UseAlloc=1; + internalAllocator.set(blockSize); + } + else{UseAlloc=0;} + } + /////////////////////////////////////// + // SparseMatrix Methods and Memebers // + /////////////////////////////////////// + + template< class T > + SparseMatrix< T >::SparseMatrix( void ) + { + _contiguous = false; + _maxEntriesPerRow = 0; + rows = 0; + rowSizes = NULL; + m_ppElements = NULL; + } + + template< class T > SparseMatrix< T >::SparseMatrix( int rows ) : SparseMatrix< T >() { Resize( rows ); } + template< class T > SparseMatrix< T >::SparseMatrix( int rows , int maxEntriesPerRow ) : SparseMatrix< T >() { Resize( rows , maxEntriesPerRow ); } + + template< class T > + SparseMatrix< T >::SparseMatrix( const SparseMatrix& M ) : SparseMatrix< T >() + { + if( M._contiguous ) Resize( M.rows , M._maxEntriesPerRow ); + else Resize( M.rows ); + for( int i=0 ; i ) * rowSizes[i] ); + } + } + template + int SparseMatrix::Entries( void ) const + { + int e = 0; + for( int i=0 ; i + SparseMatrix& SparseMatrix::operator = (const SparseMatrix& M) + { + if( M._contiguous ) Resize( M.rows , M._maxEntriesPerRow ); + else Resize( M.rows ); + for( int i=0 ; i ) * rowSizes[i] ); + } + return *this; + } + + template + SparseMatrix::~SparseMatrix( void ){ Resize( 0 ); } + + template< class T > + bool SparseMatrix< T >::write( const char* fileName ) const + { + FILE* fp = fopen( fileName , "wb" ); + if( !fp ) return false; + bool ret = write( fp ); + fclose( fp ); + return ret; + } + template< class T > + bool SparseMatrix< T >::read( const char* fileName ) + { + FILE* fp = fopen( fileName , "rb" ); + if( !fp ) return false; + bool ret = read( fp ); + fclose( fp ); + return ret; + } + template< class T > + bool SparseMatrix< T >::write( FILE* fp ) const + { + if( fwrite( &rows , sizeof( int ) , 1 , fp )!=1 ) return false; + if( fwrite( rowSizes , sizeof( int ) , rows , fp )!=rows ) return false; + for( int i=0 ; i ) , rowSizes[i] , fp )!=rowSizes[i] ) return false; + return true; + } + template< class T > + bool SparseMatrix< T >::read( FILE* fp ) + { + int r; + if( fread( &r , sizeof( int ) , 1 , fp )!=1 ) return false; + Resize( r ); + if( fread( rowSizes , sizeof( int ) , rows , fp )!=rows ) return false; + for( int i=0 ; i ) , rowSizes[i] , fp )!=rowSizes[i] ) return false; + } + return true; + } + + + template< class T > + void SparseMatrix< T >::Resize( int r ) + { + if( rows>0 ) + { + + if( !UseAlloc ) + if( _contiguous ){ if( _maxEntriesPerRow ) free( m_ppElements[0] ); } + else for( int i=0 ; i** )malloc( sizeof( MatrixEntry< T >* ) * r ); + } + _contiguous = false; + _maxEntriesPerRow = 0; + } + template< class T > + void SparseMatrix< T >::Resize( int r , int e ) + { + if( rows>0 ) + { + if( !UseAlloc ) + if( _contiguous ){ if( _maxEntriesPerRow ) free( m_ppElements[0] ); } + else for( int i=0 ; i** )malloc( sizeof( MatrixEntry< T >* ) * r ); + m_ppElements[0] = ( MatrixEntry< T >* )malloc( sizeof( MatrixEntry< T > ) * r * e ); + for( int i=1 ; i + void SparseMatrix< T >::SetRowSize( int row , int count ) + { + if( _contiguous ) + { + if (count > _maxEntriesPerRow) + { + POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "Attempted to set row size on contiguous matrix larger than max row size: (requested)"<< count << " > (maximum)" << _maxEntriesPerRow ); + } + rowSizes[row] = count; + } + else if( row>=0 && row0 ) m_ppElements[row] = ( MatrixEntry< T >* )malloc( sizeof( MatrixEntry< T > ) * count ); + } + } + } + + + template + void SparseMatrix::SetZero() + { + Resize(this->m_N, this->m_M); + } + + template + void SparseMatrix::SetIdentity() + { + SetZero(); + for(int ij=0; ij < Min( this->Rows(), this->Columns() ); ij++) + (*this)(ij,ij) = T(1); + } + + template + SparseMatrix SparseMatrix::operator * (const T& V) const + { + SparseMatrix M(*this); + M *= V; + return M; + } + + template + SparseMatrix& SparseMatrix::operator *= (const T& V) + { + for (int i=0; iRows(); i++) + { + for(int ii=0;ii + SparseMatrix SparseMatrix::Multiply( const SparseMatrix& M ) const + { + SparseMatrix R( this->Rows(), M.Columns() ); + for(int i=0; i + template + Vector SparseMatrix::Multiply( const Vector& V ) const + { + Vector R( rows ); + + for (int i=0; i + template + void SparseMatrix::Multiply( const Vector& In , Vector& Out , int threads ) const + { +#pragma omp parallel for num_threads( threads ) schedule( static ) + for( int i=0 ; i + SparseMatrix SparseMatrix::operator * (const SparseMatrix& M) const + { + return Multiply(M); + } + template + template + Vector SparseMatrix::operator * (const Vector& V) const + { + return Multiply(V); + } + + template + SparseMatrix SparseMatrix::Transpose() const + { + SparseMatrix M( this->Columns(), this->Rows() ); + + for (int i=0; iRows(); i++) + { + for(int ii=0;ii + template + int SparseMatrix::SolveSymmetric( const SparseMatrix& M , const Vector& b , int iters , Vector& solution , const T2 eps , int reset , int threads ) + { + if( reset ) + { + solution.Resize( b.Dimensions() ); + solution.SetZero(); + } + Vector< T2 > r; + r.Resize( solution.Dimensions() ); + M.Multiply( solution , r ); + r = b - r; + Vector< T2 > d = r; + double delta_new , delta_0; + for( int i=0 ; i q; + q.Resize( d.Dimensions() ); + for( ii=0; iieps*delta_0 ; ii++ ) + { + M.Multiply( d , q , threads ); + double dDotQ = 0 , alpha = 0; + for( int i=0 ; i + int SparseMatrix::Solve(const SparseMatrix& M,const Vector& b,int iters,Vector& solution,const T eps){ + SparseMatrix mTranspose=M.Transpose(); + Vector bb=mTranspose*b; + Vector d,r,Md; + T alpha,beta,rDotR; + int i; + + solution.Resize(M.Columns()); + solution.SetZero(); + + d=r=bb; + rDotR=r.Dot(r); + for(i=0;ieps;i++){ + T temp; + Md=mTranspose*(M*d); + alpha=rDotR/d.Dot(Md); + solution+=d*alpha; + r-=Md*alpha; + temp=r.Dot(r); + beta=temp/rDotR; + rDotR=temp; + d=r+d*beta; + } + return i; + } + + + + + /////////////////////////// + // SparseSymmetricMatrix // + /////////////////////////// + template + template + Vector SparseSymmetricMatrix::operator * (const Vector& V) const {return Multiply(V);} + template + template + Vector SparseSymmetricMatrix::Multiply( const Vector& V ) const + { + Vector R( SparseMatrix::rows ); + + for(int i=0; i::rows; i++){ + for(int ii=0;ii::rowSizes[i];ii++){ + int j=SparseMatrix::m_ppElements[i][ii].N; + R(i)+=SparseMatrix::m_ppElements[i][ii].Value * V.m_pV[j]; + R(j)+=SparseMatrix::m_ppElements[i][ii].Value * V.m_pV[i]; + } + } + return R; + } + + template + template + void SparseSymmetricMatrix::Multiply( const Vector& In , Vector& Out , bool addDCTerm ) const + { + Out.SetZero(); + const T2* in = &In[0]; + T2* out = &Out[0]; + T2 dcTerm = T2( 0 ); + if( addDCTerm ) + { + for( int i=0 ; i::rows ; i++ ) dcTerm += in[i]; + dcTerm /= SparseMatrix::rows; + } + for( int i=0 ; iSparseMatrix::rows ; i++ ) + { + const MatrixEntry* temp = SparseMatrix::m_ppElements[i]; + const MatrixEntry* end = temp + SparseMatrix::rowSizes[i]; + const T2& in_i_ = in[i]; + T2 out_i = T2(0); + for( ; temp!=end ; temp++ ) + { + int j=temp->N; + T2 v=temp->Value; + out_i += v * in[j]; + out[j] += v * in_i_; + } + out[i] += out_i; + } + if( addDCTerm ) for( int i=0 ; i::rows ; i++ ) out[i] += dcTerm; + } + template + template + void SparseSymmetricMatrix::Multiply( const Vector& In , Vector& Out , MapReduceVector< T2 >& OutScratch , bool addDCTerm ) const + { + int dim = int( In.Dimensions() ); + const T2* in = &In[0]; + int threads = OutScratch.threads(); + if( addDCTerm ) + { + T2 dcTerm = 0; +#pragma omp parallel for num_threads( threads ) reduction ( + : dcTerm ) + for( int t=0 ; t::rows*t)/threads ; i<(SparseMatrix::rows*(t+1))/threads ; i++ ) + { + const T2& in_i_ = in[i]; + T2& out_i_ = out[i]; + for( const MatrixEntry< T > *temp = SparseMatrix::m_ppElements[i] , *end = temp+SparseMatrix::rowSizes[i] ; temp!=end ; temp++ ) + { + int j = temp->N; + T2 v = temp->Value; + out_i_ += v * in[j]; + out[j] += v * in_i_; + } + dcTerm += in_i_; + } + } + dcTerm /= dim; + dim = int( Out.Dimensions() ); + T2* out = &Out[0]; +#pragma omp parallel for num_threads( threads ) schedule( static ) + for( int i=0 ; i::rows*t)/threads ; i<(SparseMatrix::rows*(t+1))/threads ; i++ ) + { + const T2& in_i_ = in[i]; + T2& out_i_ = out[i]; + for( const MatrixEntry< T > *temp = SparseMatrix::m_ppElements[i] , *end = temp+SparseMatrix::rowSizes[i] ; temp!=end ; temp++ ) + { + int j = temp->N; + T2 v = temp->Value; + out_i_ += v * in[j]; + out[j] += v * in_i_; + } + } + } + dim = int( Out.Dimensions() ); + T2* out = &Out[0]; +#pragma omp parallel for num_threads( threads ) schedule( static ) + for( int i=0 ; i + template + void SparseSymmetricMatrix::Multiply( const Vector& In , Vector& Out , std::vector< T2* >& OutScratch , const std::vector< int >& bounds ) const + { + int dim = In.Dimensions(); + const T2* in = &In[0]; + int threads = OutScratch.size(); +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; t* temp = SparseMatrix::m_ppElements[i]; + const MatrixEntry* end = temp + SparseMatrix::rowSizes[i]; + const T2& in_i_ = in[i]; + T2& out_i_ = out[i]; + for( ; temp!=end ; temp++ ) + { + int j = temp->N; + T2 v = temp->Value; + out_i_ += v * in[j]; + out[j] += v * in_i_; + } + } + } + T2* out = &Out[0]; +#pragma omp parallel for num_threads( threads ) schedule( static ) + for( int i=0 ; i + void MultiplyAtomic( const SparseSymmetricMatrix< T >& A , const Vector< float >& In , Vector< float >& Out , int threads , const int* partition=NULL ) + { + Out.SetZero(); + const float* in = &In[0]; + float* out = &Out[0]; + if( partition ) +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; t* temp = A[i]; + const MatrixEntry< T >* end = temp + A.rowSizes[i]; + const float& in_i = in[i]; + float out_i = 0.; + for( ; temp!=end ; temp++ ) + { + int j = temp->N; + float v = temp->Value; + out_i += v * in[j]; + AtomicIncrement( out+j , v * in_i ); + } + AtomicIncrement( out+i , out_i ); + } + else +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i* temp = A[i]; + const MatrixEntry< T >* end = temp + A.rowSizes[i]; + const float& in_i = in[i]; + float out_i = 0.f; + for( ; temp!=end ; temp++ ) + { + int j = temp->N; + float v = temp->Value; + out_i += v * in[j]; + AtomicIncrement( out+j , v * in_i ); + } + AtomicIncrement( out+i , out_i ); + } + } + template< class T > + void MultiplyAtomic( const SparseSymmetricMatrix< T >& A , const Vector< double >& In , Vector< double >& Out , int threads , const int* partition=NULL ) + { + Out.SetZero(); + const double* in = &In[0]; + double* out = &Out[0]; + + if( partition ) +#pragma omp parallel for num_threads( threads ) + for( int t=0 ; t* temp = A[i]; + const MatrixEntry< T >* end = temp + A.rowSizes[i]; + const double& in_i = in[i]; + double out_i = 0.; + for( ; temp!=end ; temp++ ) + { + int j = temp->N; + T v = temp->Value; + out_i += v * in[j]; + AtomicIncrement( out+j , v * in_i ); + } + AtomicIncrement( out+i , out_i ); + } + else +#pragma omp parallel for num_threads( threads ) + for( int i=0 ; i* temp = A[i]; + const MatrixEntry< T >* end = temp + A.rowSizes[i]; + const double& in_i = in[i]; + double out_i = 0.; + for( ; temp!=end ; temp++ ) + { + int j = temp->N; + T v = temp->Value; + out_i += v * in[j]; + AtomicIncrement( out+j , v * in_i ); + } + AtomicIncrement( out+i , out_i ); + } + } + + template< class T > + template< class T2 > + int SparseSymmetricMatrix< T >::SolveAtomic( const SparseSymmetricMatrix< T >& A , const Vector< T2 >& b , int iters , Vector< T2 >& x , T2 eps , int reset , int threads , bool solveNormal ) + { + eps *= eps; + int dim = b.Dimensions(); + if( reset ) + { + x.Resize( dim ); + x.SetZero(); + } + Vector< T2 > r( dim ) , d( dim ) , q( dim ); + Vector< T2 > temp; + if( solveNormal ) temp.Resize( dim ); + T2 *_x = &x[0] , *_r = &r[0] , *_d = &d[0] , *_q = &q[0]; + const T2* _b = &b[0]; + + std::vector< int > partition( threads+1 ); + { + int eCount = 0; + for( int i=0 ; i=eCount*(t+1) ) + { + partition[t+1] = i; + break; + } + } + } + partition[threads] = A.rows; + } + if( solveNormal ) + { + MultiplyAtomic( A , x , temp , threads , &partition[0] ); + MultiplyAtomic( A , temp , r , threads , &partition[0] ); + MultiplyAtomic( A , b , temp , threads , &partition[0] ); +#pragma omp parallel for num_threads( threads ) schedule( static ) + for( int i=0 ; ieps*delta_0 ; ii++ ) + { + if( solveNormal ) MultiplyAtomic( A , d , temp , threads , &partition[0] ) , MultiplyAtomic( A , temp , q , threads , &partition[0] ); + else MultiplyAtomic( A , d , q , threads , &partition[0] ); + double dDotQ = 0; + for( int i=0 ; i + template< class T2 > + int SparseSymmetricMatrix< T >::Solve( const SparseSymmetricMatrix& A , const Vector& b , int iters , Vector& x , MapReduceVector< T2 >& scratch , T2 eps , int reset , bool addDCTerm , bool solveNormal ) + { + int threads = scratch.threads(); + eps *= eps; + int dim = int( b.Dimensions() ); + Vector< T2 > r( dim ) , d( dim ) , q( dim ) , temp; + if( reset ) x.Resize( dim ); + if( solveNormal ) temp.Resize( dim ); + T2 *_x = &x[0] , *_r = &r[0] , *_d = &d[0] , *_q = &q[0]; + const T2* _b = &b[0]; + + double delta_new = 0 , delta_0; + if( solveNormal ) + { + A.Multiply( x , temp , scratch , addDCTerm ) , A.Multiply( temp , r , scratch , addDCTerm ) , A.Multiply( b , temp , scratch , addDCTerm ); +#pragma omp parallel for num_threads( threads ) reduction( + : delta_new ) + for( int i=0 ; ieps*delta_0 ; ii++ ) + { + if( solveNormal ) A.Multiply( d , temp , scratch , addDCTerm ) , A.Multiply( temp , q , scratch , addDCTerm ); + else A.Multiply( d , q , scratch , addDCTerm ); + double dDotQ = 0; +#pragma omp parallel for num_threads( threads ) reduction( + : dDotQ ) + for( int i=0 ; i + template< class T2 > + int SparseSymmetricMatrix::Solve( const SparseSymmetricMatrix& A , const Vector& b , int iters , Vector& x , T2 eps , int reset , int threads , bool addDCTerm , bool solveNormal ) + { + eps *= eps; + int dim = int( b.Dimensions() ); + MapReduceVector< T2 > outScratch; + if( threads<1 ) threads = 1; + if( threads>1 ) outScratch.resize( threads , dim ); + if( reset ) x.Resize( dim ); + Vector< T2 > r( dim ) , d( dim ) , q( dim ); + Vector< T2 > temp; + if( solveNormal ) temp.Resize( dim ); + T2 *_x = &x[0] , *_r = &r[0] , *_d = &d[0] , *_q = &q[0]; + const T2* _b = &b[0]; + + double delta_new = 0 , delta_0; + + if( solveNormal ) + { + if( threads>1 ) A.Multiply( x , temp , outScratch , addDCTerm ) , A.Multiply( temp , r , outScratch , addDCTerm ) , A.Multiply( b , temp , outScratch , addDCTerm ); + else A.Multiply( x , temp , addDCTerm ) , A.Multiply( temp , r , addDCTerm ) , A.Multiply( b , temp , addDCTerm ); +#pragma omp parallel for num_threads( threads ) reduction( + : delta_new ) + for( int i=0 ; i1 ) A.Multiply( x , r , outScratch , addDCTerm ); + else A.Multiply( x , r , addDCTerm ); +#pragma omp parallel for num_threads( threads ) reduction( + : delta_new ) + for( int i=0 ; ieps*delta_0 ; ii++ ) + { + if( solveNormal ) + { + if( threads>1 ) A.Multiply( d , temp , outScratch , addDCTerm ) , A.Multiply( temp , q , outScratch , addDCTerm ); + else A.Multiply( d , temp , addDCTerm ) , A.Multiply( temp , q , addDCTerm ); + } + else + { + if( threads>1 ) A.Multiply( d , q , outScratch , addDCTerm ); + else A.Multiply( d , q , addDCTerm ); + } + double dDotQ = 0; +#pragma omp parallel for num_threads( threads ) reduction( + : dDotQ ) + for( int i=0 ; i1 ) A.Multiply( x , temp , outScratch , addDCTerm ) , A.Multiply( temp , r , outScratch , addDCTerm ); + else A.Multiply( x , temp , addDCTerm ) , A.Multiply( temp , r , addDCTerm ); + } + else + { + if( threads>1 ) A.Multiply( x , r , outScratch , addDCTerm ); + else A.Multiply( x , r , addDCTerm ); + } +#pragma omp parallel for num_threads( threads ) reduction ( + : delta_new ) + for( int i=0 ; i + template + int SparseSymmetricMatrix::Solve( const SparseSymmetricMatrix& M , const Vector& diagonal , const Vector& b , int iters , Vector& solution , int reset ) + { + Vector d,r,Md; + + if(reset) + { + solution.Resize(b.Dimensions()); + solution.SetZero(); + } + Md.Resize(M.rows); + for( int i=0 ; i + template< class T2 > + void SparseSymmetricMatrix< T >::getDiagonal( Vector< T2 >& diagonal ) const + { + diagonal.Resize( SparseMatrix::rows ); + for( int i=0 ; i::rows ; i++ ) + { + diagonal[i] = 0.; + for( int j=0 ; j::rowSizes[i] ; j++ ) if( SparseMatrix::m_ppElements[i][j].N==i ) diagonal[i] += SparseMatrix::m_ppElements[i][j].Value * 2; + } + } + + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/vector.h b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/vector.h new file mode 100755 index 0000000..04e5f1d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/vector.h @@ -0,0 +1,155 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef __VECTOR_HPP +#define __VECTOR_HPP + +#define Assert assert +#include + + +namespace pcl +{ + namespace poisson + { + template + class Vector + { + public: + Vector(); + Vector( const Vector& V ); + Vector( std::size_t N ); + Vector( std::size_t N, T* pV ); + ~Vector(); + + const T& operator () (std::size_t i) const; + T& operator () (std::size_t i); + const T& operator [] (std::size_t i) const; + T& operator [] (std::size_t i); + + void SetZero(); + + std::size_t Dimensions() const; + void Resize( std::size_t N ); + + Vector operator * (const T& A) const; + Vector operator / (const T& A) const; + Vector operator - (const Vector& V) const; + Vector operator + (const Vector& V) const; + + Vector& operator *= (const T& A); + Vector& operator /= (const T& A); + Vector& operator += (const Vector& V); + Vector& operator -= (const Vector& V); + + Vector& AddScaled(const Vector& V,const T& scale); + Vector& SubtractScaled(const Vector& V,const T& scale); + static void Add(const Vector& V1,const T& scale1,const Vector& V2,const T& scale2,Vector& Out); + static void Add(const Vector& V1,const T& scale1,const Vector& V2,Vector& Out); + + Vector operator - () const; + + Vector& operator = (const Vector& V); + + T Dot( const Vector& V ) const; + + T Length() const; + + T Norm( std::size_t Ln ) const; + void Normalize(); + + bool write( FILE* fp ) const; + bool write( const char* fileName ) const; + bool read( FILE* fp ); + bool read( const char* fileName ); + + T* m_pV; + protected: + std::size_t m_N; + + }; + + template + class NVector + { + public: + NVector(); + NVector( const NVector& V ); + NVector( std::size_t N ); + NVector( std::size_t N, T* pV ); + ~NVector(); + + const T* operator () (std::size_t i) const; + T* operator () (std::size_t i); + const T* operator [] (std::size_t i) const; + T* operator [] (std::size_t i); + + void SetZero(); + + std::size_t Dimensions() const; + void Resize( std::size_t N ); + + NVector operator * (const T& A) const; + NVector operator / (const T& A) const; + NVector operator - (const NVector& V) const; + NVector operator + (const NVector& V) const; + + NVector& operator *= (const T& A); + NVector& operator /= (const T& A); + NVector& operator += (const NVector& V); + NVector& operator -= (const NVector& V); + + NVector& AddScaled(const NVector& V,const T& scale); + NVector& SubtractScaled(const NVector& V,const T& scale); + static void Add(const NVector& V1,const T& scale1,const NVector& V2,const T& scale2,NVector& Out); + static void Add(const NVector& V1,const T& scale1,const NVector& V2, NVector& Out); + + NVector operator - () const; + + NVector& operator = (const NVector& V); + + T Dot( const NVector& V ) const; + + T Length() const; + + T Norm( std::size_t Ln ) const; + void Normalize(); + + T* m_pV; + protected: + std::size_t m_N; + + }; + + } +} + + +#include "vector.hpp" + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/vector.hpp b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/vector.hpp new file mode 100755 index 0000000..2575a21 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/3rdparty/poisson4/vector.hpp @@ -0,0 +1,494 @@ +/* +Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list of +conditions and the following disclaimer. Redistributions in binary form must reproduce +the above copyright notice, this list of conditions and the following disclaimer +in the documentation and/or other materials provided with the distribution. + +Neither the name of the Johns Hopkins University nor the names of its contributors +may be used to endorse or promote products derived from this software without specific +prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR +BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +DAMAGE. +*/ + +#ifndef __VECTORIMPL_HPP +#define __VECTORIMPL_HPP + +//////////// +// Vector // +//////////// +namespace pcl +{ + namespace poisson + { + + + template + Vector::Vector() + { + m_N = 0; + m_pV = 0; + } + template + Vector::Vector( const Vector& V ) + { + m_N = 0; + m_pV = 0; + Resize(V.m_N); + memcpy( m_pV, V.m_pV, m_N*sizeof(T) ); + } + template + Vector::Vector( std::size_t N ) + { + m_N=0; + m_pV=0; + Resize(N); + } + template + void Vector::Resize( std::size_t N ) + { + if(m_N!=N){ + if(m_N){delete[] m_pV;} + m_pV=NULL; + m_N = N; + if(N){m_pV = new T[N];} + } + if (m_pV) { + memset( m_pV, 0, N*sizeof(T) ); + } + } + template + Vector::Vector( std::size_t N, T* pV ) + { + Resize(N); + memcpy( m_pV, pV, N*sizeof(T) ); + } + template + Vector::~Vector(){Resize(0);} + template + Vector& Vector::operator = (const Vector& V) + { + Resize(V.m_N); + memcpy( m_pV, V.m_pV, m_N*sizeof(T) ); + return *this; + } + template + std::size_t Vector::Dimensions() const{return m_N;} + template + void Vector::SetZero(void){for (std::size_t i=0; i + const T& Vector::operator () (std::size_t i) const + { + Assert( i < m_N ); + return m_pV[i]; + } + template + T& Vector::operator () (std::size_t i) + { + return m_pV[i]; + } + template + const T& Vector::operator [] (std::size_t i) const + { + return m_pV[i]; + } + template + T& Vector::operator [] (std::size_t i) + { + return m_pV[i]; + } + template + Vector Vector::operator * (const T& A) const + { + Vector V(*this); + for (std::size_t i=0; i + Vector& Vector::operator *= (const T& A) + { + for (std::size_t i=0; i + Vector Vector::operator / (const T& A) const + { + Vector V(*this); + for (std::size_t i=0; i + Vector& Vector::operator /= (const T& A) + { + for (std::size_t i=0; i + Vector Vector::operator + (const Vector& V0) const + { + Vector V(m_N); + for (std::size_t i=0; i + Vector& Vector::AddScaled(const Vector& V,const T& scale) + { + for (std::size_t i=0; i + Vector& Vector::SubtractScaled(const Vector& V,const T& scale) + { + for (std::size_t i=0; i + void Vector::Add(const Vector& V1,const T& scale1,const Vector& V2,const T& scale2,Vector& Out){ + for (std::size_t i=0; i + void Vector::Add(const Vector& V1,const T& scale1,const Vector& V2,Vector& Out){ + for (std::size_t i=0; i + Vector& Vector::operator += (const Vector& V) + { + for (std::size_t i=0; i + Vector Vector::operator - (const Vector& V0) const + { + Vector V(m_N); + for (std::size_t i=0; i + Vector Vector::operator - (void) const + { + Vector V(m_N); + + for (std::size_t i=0; i + Vector& Vector::operator -= (const Vector& V) + { + for (std::size_t i=0; i + T Vector::Norm( std::size_t Ln ) const + { + T N = T(); + for (std::size_t i = 0; i + void Vector::Normalize() + { + T N = 1.0f/Norm(2); + for (std::size_t i = 0; i + T Vector::Length() const + { + T N = T(); + for (std::size_t i = 0; i + T Vector::Dot( const Vector& V ) const + { + T V0 = T(); + for (std::size_t i=0; i + bool Vector< T >::read( const char* fileName ) + { + FILE* fp = fopen( fileName , "rb" ); + if( !fp ) return false; + bool ret = read( fp ); + fclose( fp ); + return ret; + } + template< class T > + bool Vector< T >::write( const char* fileName ) const + { + FILE* fp = fopen( fileName , "wb" ); + if( !fp ) return false; + bool ret = write( fp ); + fclose( fp ); + return ret; + } + template< class T > + bool Vector< T >::read( FILE* fp ) + { + int d; + if( fread( &d , sizeof(int) , 1 , fp )!=1 ) return false; + Resize( d ); + if( fread( &(*this)[0] , sizeof( T ) , d , fp )!=d ) return false; + return true; + } + template< class T > + bool Vector< T >::write( FILE* fp ) const + { + if( fwrite( &m_N , sizeof( int ) , 1 , fp )!=1 ) return false; + if( fwrite( &(*this)[0] , sizeof( T ) , m_N , fp )!=m_N ) return false; + return true; + } + + + ///////////// + // NVector // + ///////////// + template + NVector::NVector() + { + m_N = 0; + m_pV = 0; + } + template + NVector::NVector( const NVector& V ) + { + m_N = 0; + m_pV = 0; + Resize(V.m_N); + memcpy( m_pV, V.m_pV, m_N*sizeof(T)*Dim ); + } + template + NVector::NVector( std::size_t N ) + { + m_N=0; + m_pV=0; + Resize(N); + } + template + void NVector::Resize( std::size_t N ) + { + if(m_N!=N){ + if(m_N){delete[] m_pV;} + m_pV=NULL; + m_N = N; + if(N){m_pV = new T[Dim*N];} + } + memset( m_pV, 0, N*sizeof(T)*Dim ); + } + template + NVector::NVector( std::size_t N, T* pV ) + { + Resize(N); + memcpy( m_pV, pV, N*sizeof(T)*Dim ); + } + template + NVector::~NVector(){Resize(0);} + template + NVector& NVector::operator = (const NVector& V) + { + Resize(V.m_N); + memcpy( m_pV, V.m_pV, m_N*sizeof(T)*Dim ); + return *this; + } + template + std::size_t NVector::Dimensions() const{return m_N;} + template + void NVector::SetZero(void){for (std::size_t i=0; i + const T* NVector::operator () (std::size_t i) const + { + Assert( i < m_N ); + return &m_pV[i*Dim]; + } + template + T* NVector::operator () (std::size_t i) + { + return &m_pV[i*Dim]; + } + template + const T* NVector::operator [] (std::size_t i) const + { + return &m_pV[i*Dim]; + } + template + T* NVector::operator [] (std::size_t i) + { + return &m_pV[i*Dim]; + } + template + NVector NVector::operator * (const T& A) const + { + NVector V(*this); + for (std::size_t i=0; i + NVector& NVector::operator *= (const T& A) + { + for (std::size_t i=0; i + NVector NVector::operator / (const T& A) const + { + NVector V(*this); + for (std::size_t i=0; i + NVector& NVector::operator /= (const T& A) + { + for (std::size_t i=0; i + NVector NVector::operator + (const NVector& V0) const + { + NVector V(m_N); + for (std::size_t i=0; i + NVector& NVector::AddScaled(const NVector& V,const T& scale) + { + for (std::size_t i=0; i + NVector& NVector::SubtractScaled(const NVector& V,const T& scale) + { + for (std::size_t i=0; i + void NVector::Add(const NVector& V1,const T& scale1,const NVector& V2,const T& scale2,NVector& Out){ + for (std::size_t i=0; i + void NVector::Add(const NVector& V1,const T& scale1,const NVector& V2,NVector& Out){ + for (std::size_t i=0; i + NVector& NVector::operator += (const NVector& V) + { + for (std::size_t i=0; i + NVector NVector::operator - (const NVector& V0) const + { + NVector V(m_N); + for (std::size_t i=0; i + NVector NVector::operator - (void) const + { + NVector V(m_N); + + for (std::size_t i=0; i + NVector& NVector::operator -= (const NVector& V) + { + for (std::size_t i=0; i + T NVector::Norm( std::size_t Ln ) const + { + T N = T(); + for (std::size_t i = 0; i + void NVector::Normalize() + { + T N = 1.0f/Norm(2); + for (std::size_t i = 0; i + T NVector::Length() const + { + T N = T(); + for (std::size_t i = 0; i + T NVector::Dot( const NVector& V ) const + { + T V0 = T(); + for (std::size_t i=0; i +#include + +namespace pcl +{ + + /** \brief Bilateral filtering implementation, based on the following paper: + * * Kopf, Johannes and Cohen, Michael F. and Lischinski, Dani and Uyttendaele, Matt - Joint Bilateral Upsampling, + * * ACM Transactions in Graphics, July 2007 + * + * Takes in a colored organized point cloud (i.e. PointXYZRGB or PointXYZRGBA), that might contain nan values for the + * depth information, and it will return an upsampled version of this cloud, based on the formula: + * \f[ + * \tilde{S}_p = \frac{1}{k_p} \sum_{q_d \in \Omega} {S_{q_d} f(||p_d - q_d|| g(||\tilde{I}_p-\tilde{I}_q||}) + * \f] + * + * where S is the depth image, I is the RGB image and f and g are Gaussian functions centered at 0 and with + * standard deviations \f$\sigma_{color}\f$ and \f$\sigma_{depth}\f$ + */ + template + class BilateralUpsampling: public CloudSurfaceProcessing + { + public: + typedef shared_ptr > Ptr; + typedef shared_ptr > ConstPtr; + + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + using CloudSurfaceProcessing::process; + + using PointCloudOut = pcl::PointCloud; + + Eigen::Matrix3f KinectVGAProjectionMatrix, KinectSXGAProjectionMatrix; + + /** \brief Constructor. */ + BilateralUpsampling () + : window_size_ (5) + , sigma_color_ (15.0f) + , sigma_depth_ (0.5f) + { + KinectVGAProjectionMatrix << 525.0f, 0.0f, 320.0f, + 0.0f, 525.0f, 240.0f, + 0.0f, 0.0f, 1.0f; + KinectSXGAProjectionMatrix << 1050.0f, 0.0f, 640.0f, + 0.0f, 1050.0f, 480.0f, + 0.0f, 0.0f, 1.0f; + }; + + /** \brief Method that sets the window size for the filter + * \param[in] window_size the given window size + */ + inline void + setWindowSize (int window_size) { window_size_ = window_size; } + + /** \brief Returns the filter window size */ + inline int + getWindowSize () const { return (window_size_); } + + /** \brief Method that sets the sigma color parameter + * \param[in] sigma_color the new value to be set + */ + inline void + setSigmaColor (const float &sigma_color) { sigma_color_ = sigma_color; } + + /** \brief Returns the current sigma color value */ + inline float + getSigmaColor () const { return (sigma_color_); } + + /** \brief Method that sets the sigma depth parameter + * \param[in] sigma_depth the new value to be set + */ + inline void + setSigmaDepth (const float &sigma_depth) { sigma_depth_ = sigma_depth; } + + /** \brief Returns the current sigma depth value */ + inline float + getSigmaDepth () const { return (sigma_depth_); } + + /** \brief Method that sets the projection matrix to be used when unprojecting the points in the depth image + * back to (x,y,z) positions. + * \note There are 2 matrices already set in the class, used for the 2 modes available for the Kinect. They + * are tuned to be the same as the ones in the OpenNiGrabber + * \param[in] projection_matrix the new projection matrix to be set */ + inline void + setProjectionMatrix (const Eigen::Matrix3f &projection_matrix) { projection_matrix_ = projection_matrix; } + + /** \brief Returns the current projection matrix */ + inline Eigen::Matrix3f + getProjectionMatrix () const { return (projection_matrix_); } + + /** \brief Method that does the actual processing on the input cloud. + * \param[out] output the container of the resulting upsampled cloud */ + void + process (pcl::PointCloud &output) override; + + protected: + void + performProcessing (pcl::PointCloud &output) override; + + /** \brief Computes the distance for depth and RGB. + * \param[out] val_exp_depth distance values for depth + * \param[out] val_exp_rgb distance values for RGB */ + void + computeDistances (Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb); + + private: + int window_size_; + float sigma_color_, sigma_depth_; + Eigen::Matrix3f projection_matrix_, unprojection_matrix_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/boost.h b/deps_install/include/pcl-1.10/pcl/surface/boost.h new file mode 100755 index 0000000..208e3bd --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/boost.h @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include diff --git a/deps_install/include/pcl-1.10/pcl/surface/concave_hull.h b/deps_install/include/pcl-1.10/pcl/surface/concave_hull.h new file mode 100755 index 0000000..b53fd7b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/concave_hull.h @@ -0,0 +1,214 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#ifdef HAVE_QHULL + +#include + +namespace pcl +{ + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ConcaveHull (alpha shapes) using libqhull library. + * \author Aitor Aldoma + * \ingroup surface + */ + template + class ConcaveHull : public MeshConstruction + { + protected: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + using MeshConstruction::reconstruct; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + /** \brief Empty constructor. */ + ConcaveHull () : alpha_ (0), keep_information_ (false), voronoi_centers_ (), dim_(0) + { + }; + + /** \brief Empty destructor */ + ~ConcaveHull () {} + + /** \brief Compute a concave hull for all points given + * + * \param points the resultant points lying on the concave hull + * \param polygons the resultant concave hull polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + */ + void + reconstruct (PointCloud &points, + std::vector &polygons); + + /** \brief Compute a concave hull for all points given + * \param output the resultant concave hull vertices + */ + void + reconstruct (PointCloud &output); + + /** \brief Set the alpha value, which limits the size of the resultant + * hull segments (the smaller the more detailed the hull). + * + * \param alpha positive, non-zero value, defining the maximum length + * from a vertex to the facet center (center of the voronoi cell). + */ + inline void + setAlpha (double alpha) + { + alpha_ = alpha; + } + + /** \brief Returns the alpha parameter, see setAlpha(). */ + inline double + getAlpha () + { + return (alpha_); + } + + /** \brief If set, the voronoi cells center will be saved in _voronoi_centers_ + * \param voronoi_centers + */ + inline void + setVoronoiCenters (PointCloudPtr voronoi_centers) + { + voronoi_centers_ = voronoi_centers; + } + + /** \brief If keep_information_is set to true the convex hull + * points keep other information like rgb, normals, ... + * \param value where to keep the information or not, default is false + */ + void + setKeepInformation (bool value) + { + keep_information_ = value; + } + + /** \brief Returns the dimensionality (2 or 3) of the calculated hull. */ + inline int + getDimension () const + { + return (dim_); + } + + /** \brief Sets the dimension on the input data, 2D or 3D. + * \param[in] dimension The dimension of the input data. If not set, this will be determined automatically. + */ + void + setDimension (int dimension) + { + if ((dimension == 2) || (dimension == 3)) + dim_ = dimension; + else + PCL_ERROR ("[pcl::%s::setDimension] Invalid input dimension specified!\n", getClassName ().c_str ()); + } + + /** \brief Retrieve the indices of the input point cloud that for the convex hull. + * + * \note Should only be called after reconstruction was performed and if the ConcaveHull is + * set to preserve information via setKeepInformation (). + * + * \param[out] hull_point_indices The indices of the points forming the point cloud + */ + void + getHullPointIndices (pcl::PointIndices &hull_point_indices) const; + + protected: + /** \brief Class get name method. */ + std::string + getClassName () const override + { + return ("ConcaveHull"); + } + + protected: + /** \brief The actual reconstruction method. + * + * \param points the resultant points lying on the concave hull + * \param polygons the resultant concave hull polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + */ + void + performReconstruction (PointCloud &points, + std::vector &polygons); + + void + performReconstruction (PolygonMesh &output) override; + + void + performReconstruction (std::vector &polygons) override; + + /** \brief The method accepts facets only if the distance from any vertex to the facet->center + * (center of the voronoi cell) is smaller than alpha + */ + double alpha_; + + /** \brief If set to true, the reconstructed point cloud describing the hull is obtained from + * the original input cloud by performing a nearest neighbor search from Qhull output. + */ + bool keep_information_; + + /** \brief the centers of the voronoi cells */ + PointCloudPtr voronoi_centers_; + + /** \brief the dimensionality of the concave hull */ + int dim_; + + /** \brief vector containing the point cloud indices of the convex hull points. */ + pcl::PointIndices hull_indices_; + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/convex_hull.h b/deps_install/include/pcl-1.10/pcl/surface/convex_hull.h new file mode 100755 index 0000000..4bad4b8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/convex_hull.h @@ -0,0 +1,278 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#ifdef HAVE_QHULL + +// PCL includes +#include +#include +#include + +namespace pcl +{ + /** \brief Sort 2D points in a vector structure + * \param p1 the first point + * \param p2 the second point + * \ingroup surface + */ + inline bool + comparePoints2D (const std::pair & p1, const std::pair & p2) + { + double angle1 = std::atan2 (p1.second[1], p1.second[0]) + M_PI; + double angle2 = std::atan2 (p2.second[1], p2.second[0]) + M_PI; + return (angle1 > angle2); + } + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ConvexHull using libqhull library. + * \author Aitor Aldoma, Alex Trevor + * \ingroup surface + */ + template + class ConvexHull : public MeshConstruction + { + protected: + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using MeshConstruction::reconstruct; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + /** \brief Empty constructor. */ + ConvexHull () : compute_area_ (false), total_area_ (0), total_volume_ (0), dimension_ (0), + projection_angle_thresh_ (std::cos (0.174532925) ), qhull_flags ("qhull "), + x_axis_ (1.0, 0.0, 0.0), y_axis_ (0.0, 1.0, 0.0), z_axis_ (0.0, 0.0, 1.0) + { + }; + + /** \brief Empty destructor */ + ~ConvexHull () {} + + /** \brief Compute a convex hull for all points given. + * + * \note In 2D case (i.e. if the input points belong to one plane) + * the \a polygons vector will have a single item, whereas in 3D + * case it will contain one item for each hull facet. + * + * \param[out] points the resultant points lying on the convex hull. + * \param[out] polygons the resultant convex hull polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + */ + void + reconstruct (PointCloud &points, + std::vector &polygons); + + /** \brief Compute a convex hull for all points given. + * \param[out] points the resultant points lying on the convex hull. + */ + void + reconstruct (PointCloud &points); + + /** \brief If set to true, the qhull library is called to compute the total area and volume of the convex hull. + * NOTE: When this option is activated, the qhull library produces output to the console. + * \param[in] value whether to compute the area and the volume, default is false + */ + void + setComputeAreaVolume (bool value) + { + compute_area_ = value; + if (compute_area_) + qhull_flags = std::string ("qhull FA"); + else + qhull_flags = std::string ("qhull "); + } + + /** \brief Returns the total area of the convex hull. */ + double + getTotalArea () const + { + return (total_area_); + } + + /** \brief Returns the total volume of the convex hull. Only valid for 3-dimensional sets. + * For 2D-sets volume is zero. + */ + double + getTotalVolume () const + { + return (total_volume_); + } + + /** \brief Sets the dimension on the input data, 2D or 3D. + * \param[in] dimension The dimension of the input data. If not set, this will be determined automatically. + */ + void + setDimension (int dimension) + { + if ((dimension == 2) || (dimension == 3)) + dimension_ = dimension; + else + PCL_ERROR ("[pcl::%s::setDimension] Invalid input dimension specified!\n", getClassName ().c_str ()); + } + + /** \brief Returns the dimensionality (2 or 3) of the calculated hull. */ + inline int + getDimension () const + { + return (dimension_); + } + + /** \brief Retrieve the indices of the input point cloud that for the convex hull. + * + * \note Should only be called after reconstruction was performed. + * \param[out] hull_point_indices The indices of the points forming the point cloud + */ + void + getHullPointIndices (pcl::PointIndices &hull_point_indices) const; + + protected: + /** \brief The actual reconstruction method. + * + * \param[out] points the resultant points lying on the convex hull + * \param[out] polygons the resultant convex hull polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + * \param[in] fill_polygon_data true if polygons should be filled, false otherwise + */ + void + performReconstruction (PointCloud &points, + std::vector &polygons, + bool fill_polygon_data = false); + + /** \brief The reconstruction method for 2D data. Does not require dimension to be set. + * + * \param[out] points the resultant points lying on the convex hull + * \param[out] polygons the resultant convex hull polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + * \param[in] fill_polygon_data true if polygons should be filled, false otherwise + */ + void + performReconstruction2D (PointCloud &points, + std::vector &polygons, + bool fill_polygon_data = false); + + /** \brief The reconstruction method for 3D data. Does not require dimension to be set. + * + * \param[out] points the resultant points lying on the convex hull + * \param[out] polygons the resultant convex hull polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + * \param[in] fill_polygon_data true if polygons should be filled, false otherwise + */ + void + performReconstruction3D (PointCloud &points, + std::vector &polygons, + bool fill_polygon_data = false); + + /** \brief A reconstruction method that returns a polygonmesh. + * + * \param[out] output a PolygonMesh representing the convex hull of the input data. + */ + void + performReconstruction (PolygonMesh &output) override; + + /** \brief A reconstruction method that returns the polygon of the convex hull. + * + * \param[out] polygons the polygon(s) representing the convex hull of the input data. + */ + void + performReconstruction (std::vector &polygons) override; + + /** \brief Automatically determines the dimension of input data - 2D or 3D. */ + void + calculateInputDimension (); + + /** \brief Class get name method. */ + std::string + getClassName () const override + { + return ("ConvexHull"); + } + + /* \brief True if we should compute the area and volume of the convex hull. */ + bool compute_area_; + + /* \brief The area of the convex hull. */ + double total_area_; + + /* \brief The volume of the convex hull (only for 3D hulls, zero for 2D). */ + double total_volume_; + + /** \brief The dimensionality of the concave hull (2D or 3D). */ + int dimension_; + + /** \brief How close can a 2D plane's normal be to an axis to make projection problematic. */ + double projection_angle_thresh_; + + /** \brief Option flag string to be used calling qhull. */ + std::string qhull_flags; + + /* \brief x-axis - for checking valid projections. */ + const Eigen::Vector3d x_axis_; + + /* \brief y-axis - for checking valid projections. */ + const Eigen::Vector3d y_axis_; + + /* \brief z-axis - for checking valid projections. */ + const Eigen::Vector3d z_axis_; + + /* \brief vector containing the point cloud indices of the convex hull points. */ + pcl::PointIndices hull_indices_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/ear_clipping.h b/deps_install/include/pcl-1.10/pcl/surface/ear_clipping.h new file mode 100755 index 0000000..c6a54e8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/ear_clipping.h @@ -0,0 +1,125 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + + /** \brief The ear clipping triangulation algorithm. + * The code is inspired by Flavien Brebion implementation, which is + * in n^3 and does not handle holes. + * \author Nicolas Burrus + * \ingroup surface + */ + class PCL_EXPORTS EarClipping : public MeshProcessing + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using MeshProcessing::input_mesh_; + using MeshProcessing::initCompute; + /** \brief Empty constructor */ + EarClipping () + { + }; + + protected: + /** \brief a Pointer to the point cloud data. */ + pcl::PointCloud::Ptr points_; + + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief The actual surface reconstruction method. + * \param[out] output the output polygonal mesh + */ + void + performProcessing (pcl::PolygonMesh &output) override; + + /** \brief Triangulate one polygon. + * \param[in] vertices the set of vertices + * \param[out] output the resultant polygonal mesh + */ + void + triangulate (const Vertices& vertices, PolygonMesh& output); + + /** \brief Compute the signed area of a polygon. + * \param[in] vertices the vertices representing the polygon + */ + float + area (const std::vector& vertices); + + /** \brief Check if the triangle (u,v,w) is an ear. + * \param[in] u the first triangle vertex + * \param[in] v the second triangle vertex + * \param[in] w the third triangle vertex + * \param[in] vertices a set of input vertices + */ + bool + isEar (int u, int v, int w, const std::vector& vertices); + + /** \brief Check if p is inside the triangle (u,v,w). + * \param[in] u the first triangle vertex + * \param[in] v the second triangle vertex + * \param[in] w the third triangle vertex + * \param[in] p the point to check + */ + bool + isInsideTriangle (const Eigen::Vector3f& u, + const Eigen::Vector3f& v, + const Eigen::Vector3f& w, + const Eigen::Vector3f& p); + + /** \brief Compute the cross product between 2D vectors. + * \param[in] p1 the first 2D vector + * \param[in] p2 the first 2D vector + */ + float + crossProduct (const Eigen::Vector2f& p1, const Eigen::Vector2f& p2) const + { + return p1[0]*p2[1] - p1[1]*p2[0]; + } + + }; + +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/eigen.h b/deps_install/include/pcl-1.10/pcl/surface/eigen.h new file mode 100755 index 0000000..c1cad11 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/eigen.h @@ -0,0 +1,46 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: lmeds.h 1370 2011-06-19 01:06:01Z jspricke $ + * + */ + +#pragma once + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include diff --git a/deps_install/include/pcl-1.10/pcl/surface/gp3.h b/deps_install/include/pcl-1.10/pcl/surface/gp3.h new file mode 100755 index 0000000..6b7d8f4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/gp3.h @@ -0,0 +1,522 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +// PCL includes +#include +#include + +#include +#include +#include + +#include +#include + + + +namespace pcl +{ + /** \brief Returns if a point X is visible from point R (or the origin) + * when taking into account the segment between the points S1 and S2 + * \param X 2D coordinate of the point + * \param S1 2D coordinate of the segment's first point + * \param S2 2D coordinate of the segment's second point + * \param R 2D coordinate of the reference point (defaults to 0,0) + * \ingroup surface + */ + inline bool + isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2, + const Eigen::Vector2f &R = Eigen::Vector2f::Zero ()) + { + double a0 = S1[1] - S2[1]; + double b0 = S2[0] - S1[0]; + double c0 = S1[0]*S2[1] - S2[0]*S1[1]; + double a1 = -X[1]; + double b1 = X[0]; + double c1 = 0; + if (R != Eigen::Vector2f::Zero()) + { + a1 += R[1]; + b1 -= R[0]; + c1 = R[0]*X[1] - X[0]*R[1]; + } + double div = a0*b1 - b0*a1; + double x = (b0*c1 - b1*c0) / div; + double y = (a1*c0 - a0*c1) / div; + + bool intersection_outside_XR; + if (R == Eigen::Vector2f::Zero()) + { + if (X[0] > 0) + intersection_outside_XR = (x <= 0) || (x >= X[0]); + else if (X[0] < 0) + intersection_outside_XR = (x >= 0) || (x <= X[0]); + else if (X[1] > 0) + intersection_outside_XR = (y <= 0) || (y >= X[1]); + else if (X[1] < 0) + intersection_outside_XR = (y >= 0) || (y <= X[1]); + else + intersection_outside_XR = true; + } + else + { + if (X[0] > R[0]) + intersection_outside_XR = (x <= R[0]) || (x >= X[0]); + else if (X[0] < R[0]) + intersection_outside_XR = (x >= R[0]) || (x <= X[0]); + else if (X[1] > R[1]) + intersection_outside_XR = (y <= R[1]) || (y >= X[1]); + else if (X[1] < R[1]) + intersection_outside_XR = (y >= R[1]) || (y <= X[1]); + else + intersection_outside_XR = true; + } + if (intersection_outside_XR) + return true; + if (S1[0] > S2[0]) + return (x <= S2[0]) || (x >= S1[0]); + if (S1[0] < S2[0]) + return (x >= S2[0]) || (x <= S1[0]); + if (S1[1] > S2[1]) + return (y <= S2[1]) || (y >= S1[1]); + if (S1[1] < S2[1]) + return (y >= S2[1]) || (y <= S1[1]); + return false; + } + + /** \brief GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points + * based on local 2D projections. It assumes locally smooth surfaces and relatively smooth transitions between + * areas with different point densities. + * \author Zoltan Csaba Marton + * \ingroup surface + */ + template + class GreedyProjectionTriangulation : public MeshConstruction + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using MeshConstruction::tree_; + using MeshConstruction::input_; + using MeshConstruction::indices_; + + using KdTree = pcl::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + enum GP3Type + { + NONE = -1, // not-defined + FREE = 0, + FRINGE = 1, + BOUNDARY = 2, + COMPLETED = 3 + }; + + /** \brief Empty constructor. */ + GreedyProjectionTriangulation () : + mu_ (0), + search_radius_ (0), // must be set by user + nnn_ (100), + minimum_angle_ (M_PI/18), // 10 degrees + maximum_angle_ (2*M_PI/3), // 120 degrees + eps_angle_(M_PI/4), //45 degrees, + consistent_(false), + consistent_ordering_ (false), + angles_ (), + R_ (), + is_current_free_ (false), + current_index_ (), + prev_is_ffn_ (false), + prev_is_sfn_ (false), + next_is_ffn_ (false), + next_is_sfn_ (false), + changed_1st_fn_ (false), + changed_2nd_fn_ (false), + new2boundary_ (), + already_connected_ (false) + {}; + + /** \brief Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point + * (this will make the algorithm adapt to different point densities in the cloud). + * \param[in] mu the multiplier + */ + inline void + setMu (double mu) { mu_ = mu; } + + /** \brief Get the nearest neighbor distance multiplier. */ + inline double + getMu () const { return (mu_); } + + /** \brief Set the maximum number of nearest neighbors to be searched for. + * \param[in] nnn the maximum number of nearest neighbors + */ + inline void + setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; } + + /** \brief Get the maximum number of nearest neighbors to be searched for. */ + inline int + getMaximumNearestNeighbors () const { return (nnn_); } + + /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for triangulating. + * \param[in] radius the sphere radius that is to contain all k-nearest neighbors + * \note This distance limits the maximum edge length! + */ + inline void + setSearchRadius (double radius) { search_radius_ = radius; } + + /** \brief Get the sphere radius used for determining the k-nearest neighbors. */ + inline double + getSearchRadius () const { return (search_radius_); } + + /** \brief Set the minimum angle each triangle should have. + * \param[in] minimum_angle the minimum angle each triangle should have + * \note As this is a greedy approach, this will have to be violated from time to time + */ + inline void + setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; } + + /** \brief Get the parameter for distance based weighting of neighbors. */ + inline double + getMinimumAngle () const { return (minimum_angle_); } + + /** \brief Set the maximum angle each triangle can have. + * \param[in] maximum_angle the maximum angle each triangle can have + * \note For best results, its value should be around 120 degrees + */ + inline void + setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; } + + /** \brief Get the parameter for distance based weighting of neighbors. */ + inline double + getMaximumAngle () const { return (maximum_angle_); } + + /** \brief Don't consider points for triangulation if their normal deviates more than this value from the query point's normal. + * \param[in] eps_angle maximum surface angle + * \note As normal estimation methods usually give smooth transitions at sharp edges, this ensures correct triangulation + * by avoiding connecting points from one side to points from the other through forcing the use of the edge points. + */ + inline void + setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; } + + /** \brief Get the maximum surface angle. */ + inline double + getMaximumSurfaceAngle () const { return (eps_angle_); } + + /** \brief Set the flag if the input normals are oriented consistently. + * \param[in] consistent set it to true if the normals are consistently oriented + */ + inline void + setNormalConsistency (bool consistent) { consistent_ = consistent; } + + /** \brief Get the flag for consistently oriented normals. */ + inline bool + getNormalConsistency () const { return (consistent_); } + + /** \brief Set the flag to order the resulting triangle vertices consistently (positive direction around normal). + * @note Assumes consistently oriented normals (towards the viewpoint) -- see setNormalConsistency () + * \param[in] consistent_ordering set it to true if triangle vertices should be ordered consistently + */ + inline void + setConsistentVertexOrdering (bool consistent_ordering) { consistent_ordering_ = consistent_ordering; } + + /** \brief Get the flag signaling consistently ordered triangle vertices. */ + inline bool + getConsistentVertexOrdering () const { return (consistent_ordering_); } + + /** \brief Get the state of each point after reconstruction. + * \note Options are defined as constants: FREE, FRINGE, COMPLETED, BOUNDARY and NONE + */ + inline std::vector + getPointStates () const { return (state_); } + + /** \brief Get the ID of each point after reconstruction. + * \note parts are numbered from 0, a -1 denotes unconnected points + */ + inline std::vector + getPartIDs () const { return (part_); } + + + /** \brief Get the sfn list. */ + inline std::vector + getSFN () const { return (sfn_); } + + /** \brief Get the ffn list. */ + inline std::vector + getFFN () const { return (ffn_); } + + protected: + /** \brief The nearest neighbor distance multiplier to obtain the final search radius. */ + double mu_; + + /** \brief The nearest neighbors search radius for each point and the maximum edge length. */ + double search_radius_; + + /** \brief The maximum number of nearest neighbors accepted by searching. */ + int nnn_; + + /** \brief The preferred minimum angle for the triangles. */ + double minimum_angle_; + + /** \brief The maximum angle for the triangles. */ + double maximum_angle_; + + /** \brief Maximum surface angle. */ + double eps_angle_; + + /** \brief Set this to true if the normals of the input are consistently oriented. */ + bool consistent_; + + /** \brief Set this to true if the output triangle vertices should be consistently oriented. */ + bool consistent_ordering_; + + private: + /** \brief Struct for storing the angles to nearest neighbors **/ + struct nnAngle + { + double angle; + int index; + int nnIndex; + bool visible; + }; + + /** \brief Struct for storing the edges starting from a fringe point **/ + struct doubleEdge + { + doubleEdge () : index (0) {} + int index; + Eigen::Vector2f first; + Eigen::Vector2f second; + }; + + // Variables made global to decrease the number of parameters to helper functions + + /** \brief Temporary variable to store a triangle (as a set of point indices) **/ + pcl::Vertices triangle_; + /** \brief Temporary variable to store point coordinates **/ + std::vector > coords_; + + /** \brief A list of angles to neighbors **/ + std::vector angles_; + /** \brief Index of the current query point **/ + int R_; + /** \brief List of point states **/ + std::vector state_; + /** \brief List of sources **/ + std::vector source_; + /** \brief List of fringe neighbors in one direction **/ + std::vector ffn_; + /** \brief List of fringe neighbors in other direction **/ + std::vector sfn_; + /** \brief Connected component labels for each point **/ + std::vector part_; + /** \brief Points on the outer edge from which the mesh has to be grown **/ + std::vector fringe_queue_; + + /** \brief Flag to set if the current point is free **/ + bool is_current_free_; + /** \brief Current point's index **/ + int current_index_; + /** \brief Flag to set if the previous point is the first fringe neighbor **/ + bool prev_is_ffn_; + /** \brief Flag to set if the next point is the second fringe neighbor **/ + bool prev_is_sfn_; + /** \brief Flag to set if the next point is the first fringe neighbor **/ + bool next_is_ffn_; + /** \brief Flag to set if the next point is the second fringe neighbor **/ + bool next_is_sfn_; + /** \brief Flag to set if the first fringe neighbor was changed **/ + bool changed_1st_fn_; + /** \brief Flag to set if the second fringe neighbor was changed **/ + bool changed_2nd_fn_; + /** \brief New boundary point **/ + int new2boundary_; + + /** \brief Flag to set if the next neighbor was already connected in the previous step. + * To avoid inconsistency it should not be connected again. + */ + bool already_connected_; + + /** \brief Point coordinates projected onto the plane defined by the point normal **/ + Eigen::Vector3f proj_qp_; + /** \brief First coordinate vector of the 2D coordinate frame **/ + Eigen::Vector3f u_; + /** \brief Second coordinate vector of the 2D coordinate frame **/ + Eigen::Vector3f v_; + /** \brief 2D coordinates of the first fringe neighbor **/ + Eigen::Vector2f uvn_ffn_; + /** \brief 2D coordinates of the second fringe neighbor **/ + Eigen::Vector2f uvn_sfn_; + /** \brief 2D coordinates of the first fringe neighbor of the next point **/ + Eigen::Vector2f uvn_next_ffn_; + /** \brief 2D coordinates of the second fringe neighbor of the next point **/ + Eigen::Vector2f uvn_next_sfn_; + + /** \brief Temporary variable to store 3 coordinates **/ + Eigen::Vector3f tmp_; + + /** \brief The actual surface reconstruction method. + * \param[out] output the resultant polygonal mesh + */ + void + performReconstruction (pcl::PolygonMesh &output) override; + + /** \brief The actual surface reconstruction method. + * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. + */ + void + performReconstruction (std::vector &polygons) override; + + /** \brief The actual surface reconstruction method. + * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. + */ + bool + reconstructPolygons (std::vector &polygons); + + /** \brief Class get name method. */ + std::string + getClassName () const override { return ("GreedyProjectionTriangulation"); } + + /** \brief Forms a new triangle by connecting the current neighbor to the query point + * and the previous neighbor + * \param[out] polygons the polygon mesh to be updated + * \param[in] prev_index index of the previous point + * \param[in] next_index index of the next point + * \param[in] next_next_index index of the point after the next one + * \param[in] uvn_current 2D coordinate of the current point + * \param[in] uvn_prev 2D coordinates of the previous point + * \param[in] uvn_next 2D coordinates of the next point + */ + void + connectPoint (std::vector &polygons, + const int prev_index, + const int next_index, + const int next_next_index, + const Eigen::Vector2f &uvn_current, + const Eigen::Vector2f &uvn_prev, + const Eigen::Vector2f &uvn_next); + + /** \brief Whenever a query point is part of a boundary loop containing 3 points, that triangle is created + * (called if angle constraints make it possible) + * \param[out] polygons the polygon mesh to be updated + */ + void + closeTriangle (std::vector &polygons); + + /** \brief Get the list of containing triangles for each vertex in a PolygonMesh + * \param[in] polygonMesh the input polygon mesh + */ + std::vector > + getTriangleList (const pcl::PolygonMesh &input); + + /** \brief Add a new triangle to the current polygon mesh + * \param[in] a index of the first vertex + * \param[in] b index of the second vertex + * \param[in] c index of the third vertex + * \param[out] polygons the polygon mesh to be updated + */ + inline void + addTriangle (int a, int b, int c, std::vector &polygons) + { + triangle_.vertices.resize (3); + if (consistent_ordering_) + { + const PointInT p = input_->at (indices_->at (a)); + const Eigen::Vector3f pv = p.getVector3fMap (); + if (p.getNormalVector3fMap ().dot ( + (pv - input_->at (indices_->at (b)).getVector3fMap ()).cross ( + pv - input_->at (indices_->at (c)).getVector3fMap ()) ) > 0) + { + triangle_.vertices[0] = a; + triangle_.vertices[1] = b; + triangle_.vertices[2] = c; + } + else + { + triangle_.vertices[0] = a; + triangle_.vertices[1] = c; + triangle_.vertices[2] = b; + } + } + else + { + triangle_.vertices[0] = a; + triangle_.vertices[1] = b; + triangle_.vertices[2] = c; + } + polygons.push_back (triangle_); + } + + /** \brief Add a new vertex to the advancing edge front and set its source point + * \param[in] v index of the vertex that was connected + * \param[in] s index of the source point + */ + inline void + addFringePoint (int v, int s) + { + source_[v] = s; + part_[v] = part_[s]; + fringe_queue_.push_back(v); + } + + /** \brief Function for ascending sort of nnAngle, taking visibility into account + * (angles to visible neighbors will be first, to the invisible ones after). + * \param[in] a1 the first angle + * \param[in] a2 the second angle + */ + static inline bool + nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2) + { + if (a1.visible == a2.visible) + return (a1.angle < a2.angle); + return a1.visible; + } + }; + +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/grid_projection.h b/deps_install/include/pcl-1.10/pcl/surface/grid_projection.h new file mode 100755 index 0000000..94fe432 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/grid_projection.h @@ -0,0 +1,505 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +#include + +namespace pcl +{ + /** \brief The 12 edges of a cell. */ + const int I_SHIFT_EP[12][2] = { + {0, 4}, {1, 5}, {2, 6}, {3, 7}, + {0, 1}, {1, 2}, {2, 3}, {3, 0}, + {4, 5}, {5, 6}, {6, 7}, {7, 4} + }; + + const int I_SHIFT_PT[4] = { + 0, 4, 5, 7 + }; + + const int I_SHIFT_EDGE[3][2] = { + {0,1}, {1,3}, {1,2} + }; + + + /** \brief Grid projection surface reconstruction method. + * \author Rosie Li + * + * \note If you use this code in any academic work, please cite: + * - Ruosi Li, Lu Liu, Ly Phan, Sasakthi Abeysinghe, Cindy Grimm, Tao Ju. + * Polygonizing extremal surfaces with manifold guarantees. + * In Proceedings of the 14th ACM Symposium on Solid and Physical Modeling, 2010. + * \ingroup surface + */ + template + class GridProjection : public SurfaceReconstruction + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SurfaceReconstruction::input_; + using SurfaceReconstruction::tree_; + + using PointCloudPtr = typename pcl::PointCloud::Ptr; + + using KdTree = pcl::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + /** \brief Data leaf. */ + struct Leaf + { + Leaf () {} + + std::vector data_indices; + Eigen::Vector4f pt_on_surface; + Eigen::Vector3f vect_at_grid_pt; + }; + + typedef std::unordered_map, std::equal_to<>, Eigen::aligned_allocator>> HashMap; + + /** \brief Constructor. */ + GridProjection (); + + /** \brief Constructor. + * \param in_resolution set the resolution of the grid + */ + GridProjection (double in_resolution); + + /** \brief Destructor. */ + ~GridProjection (); + + /** \brief Set the size of the grid cell + * \param resolution the size of the grid cell + */ + inline void + setResolution (double resolution) + { + leaf_size_ = resolution; + } + + inline double + getResolution () const + { + return (leaf_size_); + } + + /** \brief When averaging the vectors, we find the union of all the input data + * points within the padding area,and do a weighted average. Say if the padding + * size is 1, when we process cell (x,y,z), we will find union of input data points + * from (x-1) to (x+1), (y-1) to (y+1), (z-1) to (z+1)(in total, 27 cells). In this + * way, even the cells itself doesn't contain any data points, we will still process it + * because there are data points in the padding area. This can help us fix holes which + * is smaller than the padding size. + * \param padding_size The num of padding cells we want to create + */ + inline void + setPaddingSize (int padding_size) + { + padding_size_ = padding_size; + } + inline int + getPaddingSize () const + { + return (padding_size_); + } + + /** \brief Set this only when using the k nearest neighbors search + * instead of finding the point union + * \param k The number of nearest neighbors we are looking for + */ + inline void + setNearestNeighborNum (int k) + { + k_ = k; + } + inline int + getNearestNeighborNum () const + { + return (k_); + } + + /** \brief Binary search is used in projection. given a point x, we find another point + * which is 3*cell_size_ far away from x. Then we do a binary search between these + * two points to find where the projected point should be. + */ + inline void + setMaxBinarySearchLevel (int max_binary_search_level) + { + max_binary_search_level_ = max_binary_search_level; + } + inline int + getMaxBinarySearchLevel () const + { + return (max_binary_search_level_); + } + + /////////////////////////////////////////////////////////// + inline const HashMap& + getCellHashMap () const + { + return (cell_hash_map_); + } + + inline const std::vector >& + getVectorAtDataPoint () const + { + return (vector_at_data_point_); + } + + inline const std::vector >& + getSurface () const + { + return (surface_); + } + + protected: + /** \brief Get the bounding box for the input data points, also calculating the + * cell size, and the gaussian scale factor + */ + void + getBoundingBox (); + + /** \brief The actual surface reconstruction method. + * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. + */ + bool + reconstructPolygons (std::vector &polygons); + + /** \brief Create the surface. + * + * The 1st step is filling the padding, so that all the cells in the padding + * area are in the hash map. The 2nd step is store the vector, and projected + * point. The 3rd step is finding all the edges intersects the surface, and + * creating surface. + * + * \param[out] output the resultant polygonal mesh + */ + void + performReconstruction (pcl::PolygonMesh &output) override; + + /** \brief Create the surface. + * + * The 1st step is filling the padding, so that all the cells in the padding + * area are in the hash map. The 2nd step is store the vector, and projected + * point. The 3rd step is finding all the edges intersects the surface, and + * creating surface. + * + * \param[out] points the resultant points lying on the surface + * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. + */ + void + performReconstruction (pcl::PointCloud &points, + std::vector &polygons) override; + + /** \brief When the input data points don't fill into the 1*1*1 box, + * scale them so that they can be filled in the unit box. Otherwise, + * it will be some drawing problem when doing visulization + * \param scale_factor scale all the input data point by scale_factor + */ + void + scaleInputDataPoint (double scale_factor); + + /** \brief Get the 3d index (x,y,z) of the cell based on the location of + * the cell + * \param p the coordinate of the input point + * \param index the output 3d index + */ + inline void + getCellIndex (const Eigen::Vector4f &p, Eigen::Vector3i& index) const + { + for (int i = 0; i < 3; ++i) + index[i] = static_cast ((p[i] - min_p_(i)) / leaf_size_); + } + + /** \brief Given the 3d index (x, y, z) of the cell, get the + * coordinates of the cell center + * \param index the output 3d index + * \param center the resultant cell center + */ + inline void + getCellCenterFromIndex (const Eigen::Vector3i &index, Eigen::Vector4f ¢er) const + { + for (int i = 0; i < 3; ++i) + center[i] = + min_p_[i] + static_cast (index[i]) * + static_cast (leaf_size_) + + static_cast (leaf_size_) / 2.0f; + } + + /** \brief Given cell center, caluate the coordinates of the eight vertices of the cell + * \param cell_center the coordinates of the cell center + * \param pts the coordinates of the 8 vertices + */ + void + getVertexFromCellCenter (const Eigen::Vector4f &cell_center, + std::vector > &pts) const; + + /** \brief Given an index (x, y, z) in 3d, translate it into the index + * in 1d + * \param index the index of the cell in (x,y,z) 3d format + */ + inline int + getIndexIn1D (const Eigen::Vector3i &index) const + { + //assert(data_size_ > 0); + return (index[0] * data_size_ * data_size_ + + index[1] * data_size_ + index[2]); + } + + /** \brief Given an index in 1d, translate it into the index (x, y, z) + * in 3d + * \param index_1d the input 1d index + * \param index_3d the output 3d index + */ + inline void + getIndexIn3D (int index_1d, Eigen::Vector3i& index_3d) const + { + //assert(data_size_ > 0); + index_3d[0] = index_1d / (data_size_ * data_size_); + index_1d -= index_3d[0] * data_size_ * data_size_; + index_3d[1] = index_1d / data_size_; + index_1d -= index_3d[1] * data_size_; + index_3d[2] = index_1d; + } + + /** \brief For a given 3d index of a cell, test whether the cells within its + * padding area exist in the hash table, if no, create an entry for that cell. + * \param index the index of the cell in (x,y,z) format + */ + void + fillPad (const Eigen::Vector3i &index); + + /** \brief Obtain the index of a cell and the pad size. + * \param index the input index + * \param pt_union_indices the union of input data points within the cell and padding cells + */ + void + getDataPtsUnion (const Eigen::Vector3i &index, std::vector &pt_union_indices); + + /** \brief Given the index of a cell, exam it's up, left, front edges, and add + * the vectices to m_surface list.the up, left, front edges only share 4 + * points, we first get the vectors at these 4 points and exam whether those + * three edges are intersected by the surface \param index the input index + * \param pt_union_indices the union of input data points within the cell and padding cells + */ + void + createSurfaceForCell (const Eigen::Vector3i &index, std::vector &pt_union_indices); + + + /** \brief Given the coordinates of one point, project it onto the surface, + * return the projected point. Do a binary search between p and p+projection_distance + * to find the projected point + * \param p the coordinates of the input point + * \param pt_union_indices the union of input data points within the cell and padding cells + * \param projection the resultant point projected + */ + void + getProjection (const Eigen::Vector4f &p, std::vector &pt_union_indices, Eigen::Vector4f &projection); + + /** \brief Given the coordinates of one point, project it onto the surface, + * return the projected point. Find the plane which fits all the points in + * pt_union_indices, projected p to the plane to get the projected point. + * \param p the coordinates of the input point + * \param pt_union_indices the union of input data points within the cell and padding cells + * \param projection the resultant point projected + */ + void + getProjectionWithPlaneFit (const Eigen::Vector4f &p, + std::vector &pt_union_indices, + Eigen::Vector4f &projection); + + + /** \brief Given the location of a point, get it's vector + * \param p the coordinates of the input point + * \param pt_union_indices the union of input data points within the cell and padding cells + * \param vo the resultant vector + */ + void + getVectorAtPoint (const Eigen::Vector4f &p, + std::vector &pt_union_indices, Eigen::Vector3f &vo); + + /** \brief Given the location of a point, get it's vector + * \param p the coordinates of the input point + * \param k_indices the k nearest neighbors of the query point + * \param k_squared_distances the squared distances of the k nearest + * neighbors to the query point + * \param vo the resultant vector + */ + void + getVectorAtPointKNN (const Eigen::Vector4f &p, + std::vector &k_indices, + std::vector &k_squared_distances, + Eigen::Vector3f &vo); + + /** \brief Get the magnitude of the vector by summing up the distance. + * \param p the coordinate of the input point + * \param pt_union_indices the union of input data points within the cell and padding cells + */ + double + getMagAtPoint (const Eigen::Vector4f &p, const std::vector &pt_union_indices); + + /** \brief Get the 1st derivative + * \param p the coordinate of the input point + * \param vec the vector at point p + * \param pt_union_indices the union of input data points within the cell and padding cells + */ + double + getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, + const std::vector &pt_union_indices); + + /** \brief Get the 2nd derivative + * \param p the coordinate of the input point + * \param vec the vector at point p + * \param pt_union_indices the union of input data points within the cell and padding cells + */ + double + getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, + const std::vector &pt_union_indices); + + /** \brief Test whether the edge is intersected by the surface by + * doing the dot product of the vector at two end points. Also test + * whether the edge is intersected by the maximum surface by examing + * the 2nd derivative of the intersection point + * \param end_pts the two points of the edge + * \param vect_at_end_pts + * \param pt_union_indices the union of input data points within the cell and padding cells + */ + bool + isIntersected (const std::vector > &end_pts, + std::vector > &vect_at_end_pts, + std::vector &pt_union_indices); + + /** \brief Find point where the edge intersects the surface. + * \param level binary search level + * \param end_pts the two end points on the edge + * \param vect_at_end_pts the vectors at the two end points + * \param start_pt the starting point we use for binary search + * \param pt_union_indices the union of input data points within the cell and padding cells + * \param intersection the resultant intersection point + */ + void + findIntersection (int level, + const std::vector > &end_pts, + const std::vector > &vect_at_end_pts, + const Eigen::Vector4f &start_pt, + std::vector &pt_union_indices, + Eigen::Vector4f &intersection); + + /** \brief Go through all the entries in the hash table and update the + * cellData. + * + * When creating the hash table, the pt_on_surface field store the center + * point of the cell.After calling this function, the projection operator will + * project the center point onto the surface, and the pt_on_surface field will + * be updated using the projected point.Also the vect_at_grid_pt field will be + * updated using the vector at the upper left front vertex of the cell. + * + * \param index_1d the index of the cell after flatting it's 3d index into a 1d array + * \param index_3d the index of the cell in (x,y,z) 3d format + * \param pt_union_indices the union of input data points within the cell and pads + * \param cell_data information stored in the cell + */ + void + storeVectAndSurfacePoint (int index_1d, const Eigen::Vector3i &index_3d, + std::vector &pt_union_indices, const Leaf &cell_data); + + /** \brief Go through all the entries in the hash table and update the cellData. + * When creating the hash table, the pt_on_surface field store the center point + * of the cell.After calling this function, the projection operator will project the + * center point onto the surface, and the pt_on_surface field will be updated + * using the projected point.Also the vect_at_grid_pt field will be updated using + * the vector at the upper left front vertex of the cell. When projecting the point + * and calculating the vector, using K nearest neighbors instead of using the + * union of input data point within the cell and pads. + * + * \param index_1d the index of the cell after flatting it's 3d index into a 1d array + * \param index_3d the index of the cell in (x,y,z) 3d format + * \param cell_data information stored in the cell + */ + void + storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &index_3d, const Leaf &cell_data); + + private: + /** \brief Map containing the set of leaves. */ + HashMap cell_hash_map_; + + /** \brief Min and max data points. */ + Eigen::Vector4f min_p_, max_p_; + + /** \brief The size of a leaf. */ + double leaf_size_; + + /** \brief Gaussian scale. */ + double gaussian_scale_; + + /** \brief Data size. */ + int data_size_; + + /** \brief Max binary search level. */ + int max_binary_search_level_; + + /** \brief Number of neighbors (k) to use. */ + int k_; + + /** \brief Padding size. */ + int padding_size_; + + /** \brief The point cloud input (XYZ+Normals). */ + PointCloudPtr data_; + + /** \brief Store the surface normal(vector) at the each input data point. */ + std::vector > vector_at_data_point_; + + /** \brief An array of points which lay on the output surface. */ + std::vector > surface_; + + /** \brief Bit map which tells if there is any input data point in the cell. */ + boost::dynamic_bitset<> occupied_cell_list_; + + /** \brief Class get name method. */ + std::string getClassName () const override { return ("GridProjection"); } + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/bilateral_upsampling.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/bilateral_upsampling.hpp new file mode 100755 index 0000000..d7d9e4a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/bilateral_upsampling.hpp @@ -0,0 +1,183 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + + +#ifndef PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_ +#define PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BilateralUpsampling::process (pcl::PointCloud &output) +{ + // Copy the header + output.header = input_->header; + + if (!initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + + if (input_->isOrganized () == false) + { + PCL_ERROR ("Input cloud is not organized.\n"); + return; + } + + // Invert projection matrix + unprojection_matrix_ = projection_matrix_.inverse (); + + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + printf ("%f ", unprojection_matrix_(i, j)); + + printf ("\n"); + } + + + // Perform the actual surface reconstruction + performProcessing (output); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BilateralUpsampling::performProcessing (PointCloudOut &output) +{ + output.resize (input_->size ()); + float nan = std::numeric_limits::quiet_NaN (); + + Eigen::MatrixXf val_exp_depth_matrix; + Eigen::VectorXf val_exp_rgb_vector; + computeDistances (val_exp_depth_matrix, val_exp_rgb_vector); + + for (int x = 0; x < static_cast (input_->width); ++x) + for (int y = 0; y < static_cast (input_->height); ++y) + { + int start_window_x = std::max (x - window_size_, 0), + start_window_y = std::max (y - window_size_, 0), + end_window_x = std::min (x + window_size_, static_cast (input_->width)), + end_window_y = std::min (y + window_size_, static_cast (input_->height)); + + float sum = 0.0f, + norm_sum = 0.0f; + + for (int x_w = start_window_x; x_w < end_window_x; ++ x_w) + for (int y_w = start_window_y; y_w < end_window_y; ++ y_w) + { + float val_exp_depth = val_exp_depth_matrix (static_cast (x - x_w + window_size_), + static_cast (y - y_w + window_size_)); + + Eigen::VectorXf::Index d_color = static_cast ( + std::abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) + + std::abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) + + std::abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b)); + + float val_exp_rgb = val_exp_rgb_vector (d_color); + + if (std::isfinite (input_->points[y_w*input_->width + x_w].z)) + { + sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z; + norm_sum += val_exp_depth * val_exp_rgb; + } + } + + output.points[y*input_->width + x].r = input_->points[y*input_->width + x].r; + output.points[y*input_->width + x].g = input_->points[y*input_->width + x].g; + output.points[y*input_->width + x].b = input_->points[y*input_->width + x].b; + + if (norm_sum != 0.0f) + { + float depth = sum / norm_sum; + Eigen::Vector3f pc (static_cast (x) * depth, static_cast (y) * depth, depth); + Eigen::Vector3f pw (unprojection_matrix_ * pc); + output.points[y*input_->width + x].x = pw[0]; + output.points[y*input_->width + x].y = pw[1]; + output.points[y*input_->width + x].z = pw[2]; + } + else + { + output.points[y*input_->width + x].x = nan; + output.points[y*input_->width + x].y = nan; + output.points[y*input_->width + x].z = nan; + } + } + + output.header = input_->header; + output.width = input_->width; + output.height = input_->height; +} + + +template void +pcl::BilateralUpsampling::computeDistances (Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb) +{ + val_exp_depth.resize (2*window_size_+1,2*window_size_+1); + val_exp_rgb.resize (3*255+1); + + int j = 0; + for (int dx = -window_size_; dx < window_size_+1; ++dx) + { + int i = 0; + for (int dy = -window_size_; dy < window_size_+1; ++dy) + { + float val_exp = std::exp (- (dx*dx + dy*dy) / (2.0f * static_cast (sigma_depth_ * sigma_depth_))); + val_exp_depth(i,j) = val_exp; + i++; + } + j++; + } + + for (int d_color = 0; d_color < 3*255+1; d_color++) + { + float val_exp = std::exp (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_)); + val_exp_rgb(d_color) = val_exp; + } +} + + +#define PCL_INSTANTIATE_BilateralUpsampling(T,OutT) template class PCL_EXPORTS pcl::BilateralUpsampling; + + +#endif /* PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/concave_hull.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/concave_hull.hpp new file mode 100755 index 0000000..f8f945f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/concave_hull.hpp @@ -0,0 +1,618 @@ +/** + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include +#ifdef HAVE_QHULL + +#ifndef PCL_SURFACE_IMPL_CONCAVE_HULL_H_ +#define PCL_SURFACE_IMPL_CONCAVE_HULL_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConcaveHull::reconstruct (PointCloud &output) +{ + output.header = input_->header; + if (alpha_ <= 0) + { + PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ()); + output.points.clear (); + return; + } + + if (!initCompute ()) + { + output.points.clear (); + return; + } + + // Perform the actual surface reconstruction + std::vector polygons; + performReconstruction (output, polygons); + + output.width = static_cast (output.points.size ()); + output.height = 1; + output.is_dense = true; + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConcaveHull::reconstruct (PointCloud &output, std::vector &polygons) +{ + output.header = input_->header; + if (alpha_ <= 0) + { + PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ()); + output.points.clear (); + return; + } + + if (!initCompute ()) + { + output.points.clear (); + return; + } + + // Perform the actual surface reconstruction + performReconstruction (output, polygons); + + output.width = static_cast (output.points.size ()); + output.height = 1; + output.is_dense = true; + + deinitCompute (); +} + +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wold-style-cast" +#endif +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std::vector &polygons) +{ + Eigen::Vector4d xyz_centroid; + compute3DCentroid (*input_, *indices_, xyz_centroid); + EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero (); + computeCovarianceMatrixNormalized (*input_, *indices_, xyz_centroid, covariance_matrix); + + // Check if the covariance matrix is finite or not. + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + if (!std::isfinite (covariance_matrix.coeffRef (i, j))) + return; + + EIGEN_ALIGN16 Eigen::Vector3d eigen_values; + EIGEN_ALIGN16 Eigen::Matrix3d eigen_vectors; + pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); + + Eigen::Affine3d transform1; + transform1.setIdentity (); + + // If no input dimension is specified, determine automatically + if (dim_ == 0) + { + PCL_DEBUG ("[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ()); + if (std::abs (eigen_values[0]) < std::numeric_limits::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3) + dim_ = 2; + else + dim_ = 3; + } + + if (dim_ == 2) + { + // we have points laying on a plane, using 2d convex hull + // compute transformation bring eigen_vectors.col(i) to z-axis + + transform1 (2, 0) = eigen_vectors (0, 0); + transform1 (2, 1) = eigen_vectors (1, 0); + transform1 (2, 2) = eigen_vectors (2, 0); + + transform1 (1, 0) = eigen_vectors (0, 1); + transform1 (1, 1) = eigen_vectors (1, 1); + transform1 (1, 2) = eigen_vectors (2, 1); + transform1 (0, 0) = eigen_vectors (0, 2); + transform1 (0, 1) = eigen_vectors (1, 2); + transform1 (0, 2) = eigen_vectors (2, 2); + } + else + { + transform1.setIdentity (); + } + + PointCloud cloud_transformed; + pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed); + pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1); + + // True if qhull should free points in qh_freeqhull() or reallocation + boolT ismalloc = True; + // option flags for qhull, see qh_opt.htm + char flags[] = "qhull d QJ"; + // output from qh_produce_output(), use NULL to skip qh_produce_output() + FILE *outfile = nullptr; + // error messages from qhull code + FILE *errfile = stderr; + // 0 if no error from qhull + int exitcode; + + // Array of coordinates for each point + coordT *points = reinterpret_cast (calloc (cloud_transformed.points.size () * dim_, sizeof(coordT))); + + for (std::size_t i = 0; i < cloud_transformed.points.size (); ++i) + { + points[i * dim_ + 0] = static_cast (cloud_transformed.points[i].x); + points[i * dim_ + 1] = static_cast (cloud_transformed.points[i].y); + + if (dim_ > 2) + points[i * dim_ + 2] = static_cast (cloud_transformed.points[i].z); + } + + // Compute concave hull + exitcode = qh_new_qhull (dim_, static_cast (cloud_transformed.points.size ()), points, ismalloc, flags, outfile, errfile); + + if (exitcode != 0) + { + PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a concave hull for the given point cloud (%lu)!\n", getClassName ().c_str (), cloud_transformed.points.size ()); + + //check if it fails because of NaN values... + if (!cloud_transformed.is_dense) + { + bool NaNvalues = false; + for (std::size_t i = 0; i < cloud_transformed.size (); ++i) + { + if (!std::isfinite (cloud_transformed.points[i].x) || + !std::isfinite (cloud_transformed.points[i].y) || + !std::isfinite (cloud_transformed.points[i].z)) + { + NaNvalues = true; + break; + } + } + + if (NaNvalues) + PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ()); + } + + alpha_shape.points.resize (0); + alpha_shape.width = alpha_shape.height = 0; + polygons.resize (0); + + qh_freeqhull (!qh_ALL); + int curlong, totlong; + qh_memfreeshort (&curlong, &totlong); + + return; + } + + qh_setvoronoi_all (); + + int num_vertices = qh num_vertices; + alpha_shape.points.resize (num_vertices); + + vertexT *vertex; + // Max vertex id + int max_vertex_id = 0; + FORALLvertices + { + if (vertex->id + 1 > unsigned (max_vertex_id)) + max_vertex_id = vertex->id + 1; + } + + facetT *facet; // set by FORALLfacets + + ++max_vertex_id; + std::vector qhid_to_pcidx (max_vertex_id); + + int num_facets = qh num_facets; + + if (dim_ == 3) + { + setT *triangles_set = qh_settemp (4 * num_facets); + if (voronoi_centers_) + voronoi_centers_->points.resize (num_facets); + + int non_upper = 0; + FORALLfacets + { + // Facets are tetrahedrons (3d) + if (!facet->upperdelaunay) + { + vertexT *anyVertex = static_cast (facet->vertices->e[0].p); + double *center = facet->center; + double r = qh_pointdist (anyVertex->point,center,dim_); + + if (voronoi_centers_) + { + voronoi_centers_->points[non_upper].x = static_cast (facet->center[0]); + voronoi_centers_->points[non_upper].y = static_cast (facet->center[1]); + voronoi_centers_->points[non_upper].z = static_cast (facet->center[2]); + } + + non_upper++; + + if (r <= alpha_) + { + // all triangles in tetrahedron are good, add them all to the alpha shape (triangles_set) + qh_makeridges (facet); + facet->good = true; + facet->visitid = qh visit_id; + ridgeT *ridge, **ridgep; + FOREACHridge_ (facet->ridges) + { + facetT *neighb = otherfacet_ (ridge, facet); + if ((neighb->visitid != qh visit_id)) + qh_setappend (&triangles_set, ridge); + } + } + else + { + // consider individual triangles from the tetrahedron... + facet->good = false; + facet->visitid = qh visit_id; + qh_makeridges (facet); + ridgeT *ridge, **ridgep; + FOREACHridge_ (facet->ridges) + { + facetT *neighb; + neighb = otherfacet_ (ridge, facet); + if ((neighb->visitid != qh visit_id)) + { + // check if individual triangle is good and add it to triangles_set + + PointInT a, b, c; + a.x = static_cast ((static_cast(ridge->vertices->e[0].p))->point[0]); + a.y = static_cast ((static_cast(ridge->vertices->e[0].p))->point[1]); + a.z = static_cast ((static_cast(ridge->vertices->e[0].p))->point[2]); + b.x = static_cast ((static_cast(ridge->vertices->e[1].p))->point[0]); + b.y = static_cast ((static_cast(ridge->vertices->e[1].p))->point[1]); + b.z = static_cast ((static_cast(ridge->vertices->e[1].p))->point[2]); + c.x = static_cast ((static_cast(ridge->vertices->e[2].p))->point[0]); + c.y = static_cast ((static_cast(ridge->vertices->e[2].p))->point[1]); + c.z = static_cast ((static_cast(ridge->vertices->e[2].p))->point[2]); + + double r = pcl::getCircumcircleRadius (a, b, c); + if (r <= alpha_) + qh_setappend (&triangles_set, ridge); + } + } + } + } + } + + if (voronoi_centers_) + voronoi_centers_->points.resize (non_upper); + + // filter, add points to alpha_shape and create polygon structure + + int num_good_triangles = 0; + ridgeT *ridge, **ridgep; + FOREACHridge_ (triangles_set) + { + if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good) + num_good_triangles++; + } + + polygons.resize (num_good_triangles); + + int vertices = 0; + std::vector added_vertices (max_vertex_id, false); + + int triangles = 0; + FOREACHridge_ (triangles_set) + { + if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good) + { + polygons[triangles].vertices.resize (3); + int vertex_n, vertex_i; + FOREACHvertex_i_ ((*ridge).vertices) //3 vertices per ridge! + { + if (!added_vertices[vertex->id]) + { + alpha_shape.points[vertices].x = static_cast (vertex->point[0]); + alpha_shape.points[vertices].y = static_cast (vertex->point[1]); + alpha_shape.points[vertices].z = static_cast (vertex->point[2]); + + qhid_to_pcidx[vertex->id] = vertices; //map the vertex id of qhull to the point cloud index + added_vertices[vertex->id] = true; + vertices++; + } + + polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id]; + + } + + triangles++; + } + } + + alpha_shape.points.resize (vertices); + alpha_shape.width = static_cast (alpha_shape.points.size ()); + alpha_shape.height = 1; + } + else + { + // Compute the alpha complex for the set of points + // Filters the delaunay triangles + setT *edges_set = qh_settemp (3 * num_facets); + if (voronoi_centers_) + voronoi_centers_->points.resize (num_facets); + + int dd = 0; + FORALLfacets + { + // Facets are the delaunay triangles (2d) + if (!facet->upperdelaunay) + { + // Check if the distance from any vertex to the facet->center + // (center of the voronoi cell) is smaller than alpha + vertexT *anyVertex = static_cast(facet->vertices->e[0].p); + double r = (sqrt ((anyVertex->point[0] - facet->center[0]) * + (anyVertex->point[0] - facet->center[0]) + + (anyVertex->point[1] - facet->center[1]) * + (anyVertex->point[1] - facet->center[1]))); + if (r <= alpha_) + { + pcl::Vertices facet_vertices; //TODO: is not used!! + qh_makeridges (facet); + facet->good = true; + + ridgeT *ridge, **ridgep; + FOREACHridge_ (facet->ridges) + qh_setappend (&edges_set, ridge); + + if (voronoi_centers_) + { + voronoi_centers_->points[dd].x = static_cast (facet->center[0]); + voronoi_centers_->points[dd].y = static_cast (facet->center[1]); + voronoi_centers_->points[dd].z = 0.0f; + } + + ++dd; + } + else + facet->good = false; + } + } + + int vertices = 0; + std::vector added_vertices (max_vertex_id, false); + std::map > edges; + + ridgeT *ridge, **ridgep; + FOREACHridge_ (edges_set) + { + if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good) + { + int vertex_n, vertex_i; + int vertices_in_ridge=0; + std::vector pcd_indices; + pcd_indices.resize (2); + + FOREACHvertex_i_ ((*ridge).vertices) //in 2-dim, 2 vertices per ridge! + { + if (!added_vertices[vertex->id]) + { + alpha_shape.points[vertices].x = static_cast (vertex->point[0]); + alpha_shape.points[vertices].y = static_cast (vertex->point[1]); + + if (dim_ > 2) + alpha_shape.points[vertices].z = static_cast (vertex->point[2]); + else + alpha_shape.points[vertices].z = 0; + + qhid_to_pcidx[vertex->id] = vertices; //map the vertex id of qhull to the point cloud index + added_vertices[vertex->id] = true; + pcd_indices[vertices_in_ridge] = vertices; + vertices++; + } + else + { + pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id]; + } + + vertices_in_ridge++; + } + + // make edges bidirectional and pointing to alpha_shape pointcloud... + edges[pcd_indices[0]].push_back (pcd_indices[1]); + edges[pcd_indices[1]].push_back (pcd_indices[0]); + } + } + + alpha_shape.points.resize (vertices); + + PointCloud alpha_shape_sorted; + alpha_shape_sorted.points.resize (vertices); + + // iterate over edges until they are empty! + std::map >::iterator curr = edges.begin (); + int next = - 1; + std::vector used (vertices, false); // used to decide which direction should we take! + std::vector pcd_idx_start_polygons; + pcd_idx_start_polygons.push_back (0); + + // start following edges and removing elements + int sorted_idx = 0; + while (!edges.empty ()) + { + alpha_shape_sorted.points[sorted_idx] = alpha_shape.points[(*curr).first]; + // check where we can go from (*curr).first + for (const int &i : (*curr).second) + { + if (!used[i]) + { + // we can go there + next = i; + break; + } + } + + used[(*curr).first] = true; + edges.erase (curr); // remove edges starting from curr + + sorted_idx++; + + if (edges.empty ()) + break; + + // reassign current + curr = edges.find (next); // if next is not found, then we have unconnected polygons. + if (curr == edges.end ()) + { + // set current to any of the remaining in edge! + curr = edges.begin (); + pcd_idx_start_polygons.push_back (sorted_idx); + } + } + + pcd_idx_start_polygons.push_back (sorted_idx); + + alpha_shape.points = alpha_shape_sorted.points; + + polygons.reserve (pcd_idx_start_polygons.size () - 1); + + for (std::size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++) + { + // Check if we actually have a polygon, and not some degenerated output from QHull + if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3) + { + pcl::Vertices vertices; + vertices.vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]); + // populate points in the corresponding polygon + for (int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j) + vertices.vertices[j - pcd_idx_start_polygons[poly_id]] = static_cast (j); + + polygons.push_back (vertices); + } + } + + if (voronoi_centers_) + voronoi_centers_->points.resize (dd); + } + + qh_freeqhull (!qh_ALL); + int curlong, totlong; + qh_memfreeshort (&curlong, &totlong); + + Eigen::Affine3d transInverse = transform1.inverse (); + pcl::transformPointCloud (alpha_shape, alpha_shape, transInverse); + xyz_centroid[0] = - xyz_centroid[0]; + xyz_centroid[1] = - xyz_centroid[1]; + xyz_centroid[2] = - xyz_centroid[2]; + pcl::demeanPointCloud (alpha_shape, xyz_centroid, alpha_shape); + + // also transform voronoi_centers_... + if (voronoi_centers_) + { + pcl::transformPointCloud (*voronoi_centers_, *voronoi_centers_, transInverse); + pcl::demeanPointCloud (*voronoi_centers_, xyz_centroid, *voronoi_centers_); + } + + if (keep_information_) + { + // build a tree with the original points + pcl::KdTreeFLANN tree (true); + tree.setInputCloud (input_, indices_); + + std::vector neighbor; + std::vector distances; + neighbor.resize (1); + distances.resize (1); + + // for each point in the concave hull, search for the nearest neighbor in the original point cloud + hull_indices_.header = input_->header; + hull_indices_.indices.clear (); + hull_indices_.indices.reserve (alpha_shape.points.size ()); + + for (std::size_t i = 0; i < alpha_shape.points.size (); i++) + { + tree.nearestKSearch (alpha_shape.points[i], 1, neighbor, distances); + hull_indices_.indices.push_back (neighbor[0]); + } + + // replace point with the closest neighbor in the original point cloud + pcl::copyPointCloud (*input_, hull_indices_.indices, alpha_shape); + } +} +#ifdef __GNUC__ +#pragma GCC diagnostic warning "-Wold-style-cast" +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ConcaveHull::performReconstruction (PolygonMesh &output) +{ + // Perform reconstruction + pcl::PointCloud hull_points; + performReconstruction (hull_points, output.polygons); + + // Convert the PointCloud into a PCLPointCloud2 + pcl::toPCLPointCloud2 (hull_points, output.cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ConcaveHull::performReconstruction (std::vector &polygons) +{ + pcl::PointCloud hull_points; + performReconstruction (hull_points, polygons); +} + +////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ConcaveHull::getHullPointIndices (pcl::PointIndices &hull_point_indices) const +{ + hull_point_indices = hull_indices_; +} + +#define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull; + +#endif // PCL_SURFACE_IMPL_CONCAVE_HULL_H_ +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/convex_hull.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/convex_hull.hpp new file mode 100755 index 0000000..a81798f --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/convex_hull.hpp @@ -0,0 +1,507 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#include +#ifdef HAVE_QHULL + +#ifndef PCL_SURFACE_IMPL_CONVEX_HULL_H_ +#define PCL_SURFACE_IMPL_CONVEX_HULL_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::calculateInputDimension () +{ + PCL_DEBUG ("[pcl::%s::calculateInputDimension] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ()); + Eigen::Vector4d xyz_centroid; + compute3DCentroid (*input_, *indices_, xyz_centroid); + EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero (); + computeCovarianceMatrixNormalized (*input_, *indices_, xyz_centroid, covariance_matrix); + + EIGEN_ALIGN16 Eigen::Vector3d eigen_values; + pcl::eigen33 (covariance_matrix, eigen_values); + + if (std::abs (eigen_values[0]) < std::numeric_limits::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3) + dimension_ = 2; + else + dimension_ = 3; +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::performReconstruction2D (PointCloud &hull, std::vector &polygons, + bool) +{ + int dimension = 2; + bool xy_proj_safe = true; + bool yz_proj_safe = true; + bool xz_proj_safe = true; + + // Check the input's normal to see which projection to use + PointInT p0 = input_->points[(*indices_)[0]]; + PointInT p1 = input_->points[(*indices_)[indices_->size () - 1]]; + PointInT p2 = input_->points[(*indices_)[indices_->size () / 2]]; + Eigen::Array4f dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ()); + while (!( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ) ) + { + p0 = input_->points[(*indices_)[rand () % indices_->size ()]]; + p1 = input_->points[(*indices_)[rand () % indices_->size ()]]; + p2 = input_->points[(*indices_)[rand () % indices_->size ()]]; + dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ()); + } + + pcl::PointCloud normal_calc_cloud; + normal_calc_cloud.points.resize (3); + normal_calc_cloud.points[0] = p0; + normal_calc_cloud.points[1] = p1; + normal_calc_cloud.points[2] = p2; + + Eigen::Vector4d normal_calc_centroid; + Eigen::Matrix3d normal_calc_covariance; + pcl::compute3DCentroid (normal_calc_cloud, normal_calc_centroid); + pcl::computeCovarianceMatrixNormalized (normal_calc_cloud, normal_calc_centroid, normal_calc_covariance); + + // Need to set -1 here. See eigen33 for explanations. + Eigen::Vector3d::Scalar eigen_value; + Eigen::Vector3d plane_params; + pcl::eigen33 (normal_calc_covariance, eigen_value, plane_params); + float theta_x = std::abs (static_cast (plane_params.dot (x_axis_))); + float theta_y = std::abs (static_cast (plane_params.dot (y_axis_))); + float theta_z = std::abs (static_cast (plane_params.dot (z_axis_))); + + // Check for degenerate cases of each projection + // We must avoid projections in which the plane projects as a line + if (theta_z > projection_angle_thresh_) + { + xz_proj_safe = false; + yz_proj_safe = false; + } + if (theta_x > projection_angle_thresh_) + { + xz_proj_safe = false; + xy_proj_safe = false; + } + if (theta_y > projection_angle_thresh_) + { + xy_proj_safe = false; + yz_proj_safe = false; + } + + // True if qhull should free points in qh_freeqhull() or reallocation + boolT ismalloc = True; + // output from qh_produce_output(), use NULL to skip qh_produce_output() + FILE *outfile = nullptr; + +#ifndef HAVE_QHULL_2011 + if (compute_area_) + outfile = stderr; +#endif + + // option flags for qhull, see qh_opt.htm + const char* flags = qhull_flags.c_str (); + // error messages from qhull code + FILE *errfile = stderr; + + // Array of coordinates for each point + coordT *points = reinterpret_cast (calloc (indices_->size () * dimension, sizeof (coordT))); + + // Build input data, using appropriate projection + int j = 0; + if (xy_proj_safe) + { + for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension) + { + points[j + 0] = static_cast (input_->points[(*indices_)[i]].x); + points[j + 1] = static_cast (input_->points[(*indices_)[i]].y); + } + } + else if (yz_proj_safe) + { + for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension) + { + points[j + 0] = static_cast (input_->points[(*indices_)[i]].y); + points[j + 1] = static_cast (input_->points[(*indices_)[i]].z); + } + } + else if (xz_proj_safe) + { + for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension) + { + points[j + 0] = static_cast (input_->points[(*indices_)[i]].x); + points[j + 1] = static_cast (input_->points[(*indices_)[i]].z); + } + } + else + { + // This should only happen if we had invalid input + PCL_ERROR ("[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ()); + } + + // Compute convex hull + int exitcode = qh_new_qhull (dimension, static_cast (indices_->size ()), points, ismalloc, const_cast (flags), outfile, errfile); +#ifdef HAVE_QHULL_2011 + if (compute_area_) + { + qh_prepare_output(); + } +#endif + + // 0 if no error from qhull or it doesn't find any vertices + if (exitcode != 0 || qh num_vertices == 0) + { + PCL_ERROR ("[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), indices_->size ()); + + hull.points.resize (0); + hull.width = hull.height = 0; + polygons.resize (0); + + qh_freeqhull (!qh_ALL); + int curlong, totlong; + qh_memfreeshort (&curlong, &totlong); + + return; + } + + // Qhull returns the area in volume for 2D + if (compute_area_) + { + total_area_ = qh totvol; + total_volume_ = 0.0; + } + + int num_vertices = qh num_vertices; + hull.points.resize (num_vertices); + memset (&hull.points[0], static_cast (hull.points.size ()), sizeof (PointInT)); + + vertexT * vertex; + int i = 0; + + std::vector, Eigen::aligned_allocator > > idx_points (num_vertices); + idx_points.resize (hull.points.size ()); + memset (&idx_points[0], static_cast (hull.points.size ()), sizeof (std::pair)); + + FORALLvertices + { + hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]]; + idx_points[i].first = qh_pointid (vertex->point); + ++i; + } + + // Sort + Eigen::Vector4f centroid; + pcl::compute3DCentroid (hull, centroid); + if (xy_proj_safe) + { + for (std::size_t j = 0; j < hull.points.size (); j++) + { + idx_points[j].second[0] = hull.points[j].x - centroid[0]; + idx_points[j].second[1] = hull.points[j].y - centroid[1]; + } + } + else if (yz_proj_safe) + { + for (std::size_t j = 0; j < hull.points.size (); j++) + { + idx_points[j].second[0] = hull.points[j].y - centroid[1]; + idx_points[j].second[1] = hull.points[j].z - centroid[2]; + } + } + else if (xz_proj_safe) + { + for (std::size_t j = 0; j < hull.points.size (); j++) + { + idx_points[j].second[0] = hull.points[j].x - centroid[0]; + idx_points[j].second[1] = hull.points[j].z - centroid[2]; + } + } + std::sort (idx_points.begin (), idx_points.end (), comparePoints2D); + + polygons.resize (1); + polygons[0].vertices.resize (hull.points.size ()); + + hull_indices_.header = input_->header; + hull_indices_.indices.clear (); + hull_indices_.indices.reserve (hull.points.size ()); + + for (int j = 0; j < static_cast (hull.points.size ()); j++) + { + hull_indices_.indices.push_back ((*indices_)[idx_points[j].first]); + hull.points[j] = input_->points[(*indices_)[idx_points[j].first]]; + polygons[0].vertices[j] = static_cast (j); + } + + qh_freeqhull (!qh_ALL); + int curlong, totlong; + qh_memfreeshort (&curlong, &totlong); + + hull.width = static_cast (hull.points.size ()); + hull.height = 1; + hull.is_dense = false; + return; +} + +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wold-style-cast" +#endif +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::performReconstruction3D ( + PointCloud &hull, std::vector &polygons, bool fill_polygon_data) +{ + int dimension = 3; + + // True if qhull should free points in qh_freeqhull() or reallocation + boolT ismalloc = True; + // output from qh_produce_output(), use NULL to skip qh_produce_output() + FILE *outfile = nullptr; + +#ifndef HAVE_QHULL_2011 + if (compute_area_) + outfile = stderr; +#endif + + // option flags for qhull, see qh_opt.htm + const char *flags = qhull_flags.c_str (); + // error messages from qhull code + FILE *errfile = stderr; + + // Array of coordinates for each point + coordT *points = reinterpret_cast (calloc (indices_->size () * dimension, sizeof (coordT))); + + int j = 0; + for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension) + { + points[j + 0] = static_cast (input_->points[(*indices_)[i]].x); + points[j + 1] = static_cast (input_->points[(*indices_)[i]].y); + points[j + 2] = static_cast (input_->points[(*indices_)[i]].z); + } + + // Compute convex hull + int exitcode = qh_new_qhull (dimension, static_cast (indices_->size ()), points, ismalloc, const_cast (flags), outfile, errfile); +#ifdef HAVE_QHULL_2011 + if (compute_area_) + { + qh_prepare_output(); + } +#endif + + // 0 if no error from qhull + if (exitcode != 0) + { + PCL_ERROR ("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), input_->points.size ()); + + hull.points.resize (0); + hull.width = hull.height = 0; + polygons.resize (0); + + qh_freeqhull (!qh_ALL); + int curlong, totlong; + qh_memfreeshort (&curlong, &totlong); + + return; + } + + qh_triangulate (); + + int num_facets = qh num_facets; + + int num_vertices = qh num_vertices; + hull.points.resize (num_vertices); + + vertexT * vertex; + int i = 0; + // Max vertex id + unsigned int max_vertex_id = 0; + FORALLvertices + { + if (vertex->id + 1 > max_vertex_id) + max_vertex_id = vertex->id + 1; + } + + ++max_vertex_id; + std::vector qhid_to_pcidx (max_vertex_id); + + hull_indices_.header = input_->header; + hull_indices_.indices.clear (); + hull_indices_.indices.reserve (num_vertices); + + FORALLvertices + { + // Add vertices to hull point_cloud and store index + hull_indices_.indices.push_back ((*indices_)[qh_pointid (vertex->point)]); + hull.points[i] = input_->points[hull_indices_.indices.back ()]; + + qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index + ++i; + } + + if (compute_area_) + { + total_area_ = qh totarea; + total_volume_ = qh totvol; + } + + if (fill_polygon_data) + { + polygons.resize (num_facets); + int dd = 0; + + facetT * facet; + FORALLfacets + { + polygons[dd].vertices.resize (3); + + // Needed by FOREACHvertex_i_ + int vertex_n, vertex_i; + FOREACHvertex_i_ ((*facet).vertices) + //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]); + polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id]; + ++dd; + } + } + // Deallocates memory (also the points) + qh_freeqhull (!qh_ALL); + int curlong, totlong; + qh_memfreeshort (&curlong, &totlong); + + hull.width = static_cast (hull.points.size ()); + hull.height = 1; + hull.is_dense = false; +} +#ifdef __GNUC__ +#pragma GCC diagnostic warning "-Wold-style-cast" +#endif + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::performReconstruction (PointCloud &hull, std::vector &polygons, + bool fill_polygon_data) +{ + if (dimension_ == 0) + calculateInputDimension (); + if (dimension_ == 2) + performReconstruction2D (hull, polygons, fill_polygon_data); + else if (dimension_ == 3) + performReconstruction3D (hull, polygons, fill_polygon_data); + else + PCL_ERROR ("[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::reconstruct (PointCloud &points) +{ + points.header = input_->header; + if (!initCompute () || input_->points.empty () || indices_->empty ()) + { + points.points.clear (); + return; + } + + // Perform the actual surface reconstruction + std::vector polygons; + performReconstruction (points, polygons, false); + + points.width = static_cast (points.points.size ()); + points.height = 1; + points.is_dense = true; + + deinitCompute (); +} + + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::performReconstruction (PolygonMesh &output) +{ + // Perform reconstruction + pcl::PointCloud hull_points; + performReconstruction (hull_points, output.polygons, true); + + // Convert the PointCloud into a PCLPointCloud2 + pcl::toPCLPointCloud2 (hull_points, output.cloud); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::performReconstruction (std::vector &polygons) +{ + pcl::PointCloud hull_points; + performReconstruction (hull_points, polygons, true); +} + +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::reconstruct (PointCloud &points, std::vector &polygons) +{ + points.header = input_->header; + if (!initCompute () || input_->points.empty () || indices_->empty ()) + { + points.points.clear (); + return; + } + + // Perform the actual surface reconstruction + performReconstruction (points, polygons, true); + + points.width = static_cast (points.points.size ()); + points.height = 1; + points.is_dense = true; + + deinitCompute (); +} +////////////////////////////////////////////////////////////////////////// +template void +pcl::ConvexHull::getHullPointIndices (pcl::PointIndices &hull_point_indices) const +{ + hull_point_indices = hull_indices_; +} + +#define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull; + +#endif // PCL_SURFACE_IMPL_CONVEX_HULL_H_ +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/gp3.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/gp3.hpp new file mode 100755 index 0000000..728e5a1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/gp3.hpp @@ -0,0 +1,1676 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SURFACE_IMPL_GP3_H_ +#define PCL_SURFACE_IMPL_GP3_H_ + +#include + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GreedyProjectionTriangulation::performReconstruction (pcl::PolygonMesh &output) +{ + output.polygons.clear (); + output.polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices + if (!reconstructPolygons (output.polygons)) + { + PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_); + output.cloud.width = output.cloud.height = 0; + output.cloud.data.clear (); + return; + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GreedyProjectionTriangulation::performReconstruction (std::vector &polygons) +{ + polygons.clear (); + polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices + if (!reconstructPolygons (polygons)) + { + PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_); + return; + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector &polygons) +{ + if (search_radius_ <= 0 || mu_ <= 0) + { + polygons.clear (); + return (false); + } + const double sqr_mu = mu_*mu_; + const double sqr_max_edge = search_radius_*search_radius_; + if (nnn_ > static_cast (indices_->size ())) + nnn_ = static_cast (indices_->size ()); + + // Variables to hold the results of nearest neighbor searches + std::vector nnIdx (nnn_); + std::vector sqrDists (nnn_); + + // current number of connected components + int part_index = 0; + + // 2D coordinates of points + const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero(); + Eigen::Vector2f uvn_current; + Eigen::Vector2f uvn_prev; + Eigen::Vector2f uvn_next; + + // initializing fields + already_connected_ = false; // see declaration for comments :P + + // initializing states and fringe neighbors + part_.clear (); + state_.clear (); + source_.clear (); + ffn_.clear (); + sfn_.clear (); + part_.resize(indices_->size (), -1); // indices of point's part + state_.resize(indices_->size (), FREE); + source_.resize(indices_->size (), NONE); + ffn_.resize(indices_->size (), NONE); + sfn_.resize(indices_->size (), NONE); + fringe_queue_.clear (); + int fqIdx = 0; // current fringe's index in the queue to be processed + + // Avoiding NaN coordinates if needed + if (!input_->is_dense) + { + // Skip invalid points from the indices list + for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + if (!std::isfinite (input_->points[*it].x) || + !std::isfinite (input_->points[*it].y) || + !std::isfinite (input_->points[*it].z)) + state_[*it] = NONE; + } + + // Saving coordinates and point to index mapping + coords_.clear (); + coords_.reserve (indices_->size ()); + std::vector point2index (input_->points.size (), -1); + for (int cp = 0; cp < static_cast (indices_->size ()); ++cp) + { + coords_.push_back(input_->points[(*indices_)[cp]].getVector3fMap()); + point2index[(*indices_)[cp]] = cp; + } + + // Initializing + int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0; + angles_.resize(nnn_); + std::vector > uvn_nn (nnn_); + Eigen::Vector2f uvn_s; + + // iterating through fringe points and finishing them until everything is done + while (is_free != NONE) + { + R_ = is_free; + if (state_[R_] == FREE) + { + state_[R_] = NONE; + part_[R_] = part_index++; + + // creating starting triangle + //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); + //tree_->nearestKSearch (input_->points[(*indices_)[R_]], nnn_, nnIdx, sqrDists); + tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists); + double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); + + // Search tree returns indices into the original cloud, but we are working with indices. TODO: make that optional! + for (int i = 1; i < nnn_; i++) + { + //if (point2index[nnIdx[i]] == -1) + // std::cerr << R_ << " [" << indices_->at (R_) << "] " << i << ": " << nnIdx[i] << " / " << point2index[nnIdx[i]] << std::endl; + nnIdx[i] = point2index[nnIdx[i]]; + } + + // Get the normal estimate at the current point + const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); + + // Get a coordinate system that lies on a plane defined by its normal + v_ = nc.unitOrthogonal (); + u_ = nc.cross (v_); + + // Projecting point onto the surface + float dist = nc.dot (coords_[R_]); + proj_qp_ = coords_[R_] - dist * nc; + + // Converting coords, calculating angles and saving the projected near boundary edges + int nr_edge = 0; + std::vector doubleEdges; + for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself + { + // Transforming coordinates + tmp_ = coords_[nnIdx[i]] - proj_qp_; + uvn_nn[i][0] = tmp_.dot(u_); + uvn_nn[i][1] = tmp_.dot(v_); + // Computing the angle between each neighboring point and the query point itself + angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]); + // initializing angle descriptors + angles_[i].index = nnIdx[i]; + if ( + (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) + || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1) /// NOTE: discarding NaN points and those that are not in indices_ + || (sqrDists[i] > sqr_dist_threshold) + ) + angles_[i].visible = false; + else + angles_[i].visible = true; + // Saving the edges between nearby boundary points + if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)) + { + doubleEdge e; + e.index = i; + nr_edge++; + tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; + e.first[0] = tmp_.dot(u_); + e.first[1] = tmp_.dot(v_); + tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; + e.second[0] = tmp_.dot(u_); + e.second[1] = tmp_.dot(v_); + doubleEdges.push_back(e); + } + } + angles_[0].visible = false; + + // Verify the visibility of each potential new vertex + for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself + if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) + { + bool visibility = true; + for (int j = 0; j < nr_edge; j++) + { + if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) + visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); + if (!visibility) + break; + if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i]) + visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); + if (!visibility) + break; + } + angles_[i].visible = visibility; + } + + // Selecting first two visible free neighbors + bool not_found = true; + int left = 1; + do + { + while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++; + if (left >= nnn_) + break; + int right = left+1; + do + { + while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++; + if (right >= nnn_) + break; + if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge) + right++; + else + { + addFringePoint (nnIdx[right], R_); + addFringePoint (nnIdx[left], nnIdx[right]); + addFringePoint (R_, nnIdx[left]); + state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE; + ffn_[R_] = nnIdx[left]; + sfn_[R_] = nnIdx[right]; + ffn_[nnIdx[left]] = nnIdx[right]; + sfn_[nnIdx[left]] = R_; + ffn_[nnIdx[right]] = R_; + sfn_[nnIdx[right]] = nnIdx[left]; + addTriangle (R_, nnIdx[left], nnIdx[right], polygons); + nr_parts++; + not_found = false; + break; + } + } + while (true); + left++; + } + while (not_found); + } + + is_free = NONE; + for (std::size_t temp = 0; temp < indices_->size (); temp++) + { + if (state_[temp] == FREE) + { + is_free = temp; + break; + } + } + + bool is_fringe = true; + while (is_fringe) + { + is_fringe = false; + + int fqSize = static_cast (fringe_queue_.size ()); + while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE)) + fqIdx++; + + // an unfinished fringe point is found + if (fqIdx >= fqSize) + { + continue; + } + + R_ = fringe_queue_[fqIdx]; + is_fringe = true; + + if (ffn_[R_] == sfn_[R_]) + { + state_[R_] = COMPLETED; + continue; + } + //searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists); + //tree_->nearestKSearch (input_->points[(*indices_)[R_]], nnn_, nnIdx, sqrDists); + tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists); + + // Search tree returns indices into the original cloud, but we are working with indices TODO: make that optional! + for (int i = 1; i < nnn_; i++) + { + //if (point2index[nnIdx[i]] == -1) + // std::cerr << R_ << " [" << indices_->at (R_) << "] " << i << ": " << nnIdx[i] << " / " << point2index[nnIdx[i]] << std::endl; + nnIdx[i] = point2index[nnIdx[i]]; + } + + // Locating FFN and SFN to adapt distance threshold + double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm (); + double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm (); + double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm (); + double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist); + double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); //sqr_mu * sqr_avg_conn_dist); + if (max_sqr_fn_dist > sqrDists[nnn_-1]) + { + if (0 == increase_nnn4fn) + PCL_WARN("Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_); + increase_nnn4fn++; + state_[R_] = BOUNDARY; + continue; + } + double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist); + if (max_sqr_fns_dist > sqrDists[nnn_-1]) + { + if (0 == increase_nnn4s) + PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_); + increase_nnn4s++; + } + + // Get the normal estimate at the current point + const Eigen::Vector3f nc = input_->points[(*indices_)[R_]].getNormalVector3fMap (); + + // Get a coordinate system that lies on a plane defined by its normal + v_ = nc.unitOrthogonal (); + u_ = nc.cross (v_); + + // Projecting point onto the surface + float dist = nc.dot (coords_[R_]); + proj_qp_ = coords_[R_] - dist * nc; + + // Converting coords, calculating angles and saving the projected near boundary edges + int nr_edge = 0; + std::vector doubleEdges; + for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself + { + tmp_ = coords_[nnIdx[i]] - proj_qp_; + uvn_nn[i][0] = tmp_.dot(u_); + uvn_nn[i][1] = tmp_.dot(v_); + + // Computing the angle between each neighboring point and the query point itself + angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]); + // initializing angle descriptors + angles_[i].index = nnIdx[i]; + angles_[i].nnIndex = i; + if ( + (state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY) + || (state_[nnIdx[i]] == NONE) || (nnIdx[i] == -1) /// NOTE: discarding NaN points and those that are not in indices_ + || (sqrDists[i] > sqr_dist_threshold) + ) + angles_[i].visible = false; + else + angles_[i].visible = true; + if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i])) + angles_[i].visible = true; + bool same_side = true; + const Eigen::Vector3f neighbor_normal = input_->points[(*indices_)[nnIdx[i]]].getNormalVector3fMap (); /// NOTE: nnIdx was reset + double cosine = nc.dot (neighbor_normal); + if (cosine > 1) cosine = 1; + if (cosine < -1) cosine = -1; + double angle = std::acos (cosine); + if ((!consistent_) && (angle > M_PI/2)) + angle = M_PI - angle; + if (angle > eps_angle_) + { + angles_[i].visible = false; + same_side = false; + } + // Saving the edges between nearby boundary points + if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))) + { + doubleEdge e; + e.index = i; + nr_edge++; + tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_; + e.first[0] = tmp_.dot(u_); + e.first[1] = tmp_.dot(v_); + tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_; + e.second[0] = tmp_.dot(u_); + e.second[1] = tmp_.dot(v_); + doubleEdges.push_back(e); + // Pruning by visibility criterion + if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) + { + double angle1 = std::atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]); + double angle2 = std::atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]); + double angleMin, angleMax; + if (angle1 < angle2) + { + angleMin = angle1; + angleMax = angle2; + } + else + { + angleMin = angle2; + angleMax = angle1; + } + double angleR = angles_[i].angle + M_PI; + if (angleR >= M_PI) + angleR -= 2*M_PI; + if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]])) + { + if ((angleMax - angleMin) < M_PI) + { + if ((angleMin < angleR) && (angleR < angleMax)) + angles_[i].visible = false; + } + else + { + if ((angleR < angleMin) || (angleMax < angleR)) + angles_[i].visible = false; + } + } + else + { + tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_; + uvn_s[0] = tmp_.dot(u_); + uvn_s[1] = tmp_.dot(v_); + double angleS = std::atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]); + if ((angleMin < angleS) && (angleS < angleMax)) + { + if ((angleMin < angleR) && (angleR < angleMax)) + angles_[i].visible = false; + } + else + { + if ((angleR < angleMin) || (angleMax < angleR)) + angles_[i].visible = false; + } + } + } + } + } + angles_[0].visible = false; + + // Verify the visibility of each potential new vertex + for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself + if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i])) + { + bool visibility = true; + for (int j = 0; j < nr_edge; j++) + { + if (doubleEdges[j].index != i) + { + int f = ffn_[nnIdx[doubleEdges[j].index]]; + if ((f != nnIdx[i]) && (f != R_)) + visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero()); + if (!visibility) + break; + + int s = sfn_[nnIdx[doubleEdges[j].index]]; + if ((s != nnIdx[i]) && (s != R_)) + visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero()); + if (!visibility) + break; + } + } + angles_[i].visible = visibility; + } + + // Sorting angles + std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation::nnAngleSortAsc); + + // Triangulating + if (angles_[2].visible == false) + { + if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) ) + { + state_[R_] = BOUNDARY; + } + else + { + if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index)) + state_[R_] = BOUNDARY; + else + { + if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ()) + { + state_[R_] = BOUNDARY; + } + else + { + tmp_ = coords_[source_[R_]] - proj_qp_; + uvn_s[0] = tmp_.dot(u_); + uvn_s[1] = tmp_.dot(v_); + double angleS = std::atan2(uvn_s[1], uvn_s[0]); + double dif = angles_[1].angle - angles_[0].angle; + if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle)) + { + if (dif < 2*M_PI - maximum_angle_) + state_[R_] = BOUNDARY; + else + closeTriangle (polygons); + } + else + { + if (dif >= maximum_angle_) + state_[R_] = BOUNDARY; + else + closeTriangle (polygons); + } + } + } + } + continue; + } + + // Finding the FFN and SFN + int start = -1, end = -1; + for (int i=0; i end)) + need_invert = true; + } + else + { + if (nCB != NONE) + { + if ((nCB == start) || (nCB == end)) + { + bool inside_CB = false; + bool outside_CB = false; + for (int i=0; i angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle)) + need_invert = true; + } + } + else + { + if (start == end-1) + need_invert = true; + } + } + } + else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle)) + need_invert = true; + } + + // switching start and end if necessary + if (need_invert) + { + int tmp = start; + start = end; + end = tmp; + } + + // Arranging visible nnAngles in the order they need to be connected and + // compute the maximal angle difference between two consecutive visible angles + bool is_boundary = false, is_skinny = false; + std::vector gaps (nnn_, false); + std::vector skinny (nnn_, false); + std::vector dif (nnn_); + std::vector angleIdx; angleIdx.reserve (nnn_); + if (start > end) + { + for (int j=start; j::iterator first_gap_after = angleIdx.end (); + std::vector::iterator last_gap_before = angleIdx.begin (); + int nr_gaps = 0; + for (std::vector::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; ++it) + { + if (gaps[*it]) + { + nr_gaps++; + if (first_gap_after == angleIdx.end()) + first_gap_after = it; + last_gap_before = it+1; + } + } + if (nr_gaps > 1) + { + angleIdx.erase(first_gap_after+1, last_gap_before); + } + + // Neglecting points that would form skinny triangles (if possible) + if (is_skinny) + { + double angle_so_far = 0, angle_would_be; + double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_); + Eigen::Vector2f X; + Eigen::Vector2f S1; + Eigen::Vector2f S2; + std::vector to_erase; + for (std::vector::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it) + { + if (gaps[*(it-1)]) + angle_so_far = 0; + else + angle_so_far += dif[*(it-1)]; + if (gaps[*it]) + angle_would_be = angle_so_far; + else + angle_would_be = angle_so_far + dif[*it]; + if ( + (skinny[*it] || skinny[*(it-1)]) && + ((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) && + ((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) && + ((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) && + (angle_would_be < max_combined_angle) + ) + { + if (gaps[*(it-1)]) + { + gaps[*it] = true; + to_erase.push_back(*it); + } + else if (gaps[*it]) + { + gaps[*(it-1)] = true; + to_erase.push_back(*it); + } + else + { + std::vector::iterator prev_it; + int erased_idx = static_cast (to_erase.size ()) -1; + for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); --it) + if (*it == to_erase[erased_idx]) + erased_idx--; + else + break; + bool can_delete = true; + for (std::vector::iterator curr_it = prev_it+1; curr_it != it+1; ++curr_it) + { + tmp_ = coords_[angles_[*curr_it].index] - proj_qp_; + X[0] = tmp_.dot(u_); + X[1] = tmp_.dot(v_); + tmp_ = coords_[angles_[*prev_it].index] - proj_qp_; + S1[0] = tmp_.dot(u_); + S1[1] = tmp_.dot(v_); + tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_; + S2[0] = tmp_.dot(u_); + S2[1] = tmp_.dot(v_); + // check for inclusions + if (isVisible(X,S1,S2)) + { + can_delete = false; + angle_so_far = 0; + break; + } + } + if (can_delete) + { + to_erase.push_back(*it); + } + } + } + else + angle_so_far = 0; + } + for (const int &it : to_erase) + { + for (std::vector::iterator iter = angleIdx.begin(); iter != angleIdx.end(); ++iter) + if (it == *iter) + { + angleIdx.erase(iter); + break; + } + } + } + + // Writing edges and updating edge-front + changed_1st_fn_ = false; + changed_2nd_fn_ = false; + new2boundary_ = NONE; + for (std::vector::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it) + { + current_index_ = angles_[*it].index; + + is_current_free_ = false; + if (state_[current_index_] <= FREE) + { + state_[current_index_] = FRINGE; + is_current_free_ = true; + } + else if (!already_connected_) + { + prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); + prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]); + next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); + next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]); + if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_) + { + nr_touched++; + } + } + + if (gaps[*it]) + if (gaps[*(it-1)]) + { + if (is_current_free_) + state_[current_index_] = NONE; /// TODO: document! + } + + else // (gaps[*it]) && ^(gaps[*(it-1)]) + { + addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons); + addFringePoint (current_index_, R_); + new2boundary_ = current_index_; + if (!already_connected_) + connectPoint (polygons, angles_[*(it-1)].index, R_, + angles_[*(it+1)].index, + uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero); + else already_connected_ = false; + if (ffn_[R_] == angles_[*(angleIdx.begin())].index) + { + ffn_[R_] = new2boundary_; + } + else if (sfn_[R_] == angles_[*(angleIdx.begin())].index) + { + sfn_[R_] = new2boundary_; + } + } + + else // ^(gaps[*it]) + if (gaps[*(it-1)]) + { + addFringePoint (current_index_, R_); + new2boundary_ = current_index_; + if (!already_connected_) connectPoint (polygons, R_, angles_[*(it+1)].index, + (it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index, + uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero, + uvn_nn[angles_[*(it+1)].nnIndex]); + else already_connected_ = false; + if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index) + { + ffn_[R_] = new2boundary_; + } + else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index) + { + sfn_[R_] = new2boundary_; + } + } + + else // ^(gaps[*it]) && ^(gaps[*(it-1)]) + { + addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons); + addFringePoint (current_index_, R_); + if (!already_connected_) connectPoint (polygons, angles_[*(it-1)].index, angles_[*(it+1)].index, + (it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index, + uvn_nn[angles_[*it].nnIndex], + uvn_nn[angles_[*(it-1)].nnIndex], + uvn_nn[angles_[*(it+1)].nnIndex]); + else already_connected_ = false; + } + } + + // Finishing up R_ + if (ffn_[R_] == sfn_[R_]) + { + state_[R_] = COMPLETED; + } + if (!gaps[*(angleIdx.end()-2)]) + { + addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, polygons); + addFringePoint (angles_[*(angleIdx.end()-2)].index, R_); + if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index]) + { + if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index]) + { + state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; + } + else + { + ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; + } + } + else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index]) + { + if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index]) + { + state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED; + } + else + { + sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index; + } + } + } + if (!gaps[*(angleIdx.begin())]) + { + if (R_ == ffn_[angles_[*(angleIdx.begin())].index]) + { + if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index]) + { + state_[angles_[*(angleIdx.begin())].index] = COMPLETED; + } + else + { + ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; + } + } + else if (R_ == sfn_[angles_[*(angleIdx.begin())].index]) + { + if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index]) + { + state_[angles_[*(angleIdx.begin())].index] = COMPLETED; + } + else + { + sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index; + } + } + } + } + } + PCL_DEBUG ("Number of triangles: %lu\n", polygons.size()); + PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts); + if (increase_nnn4fn > 0) + PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn); + if (increase_nnn4s > 0) + PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s); + if (increase_dist > 0) + PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist); + + // sorting and removing doubles from fringe queue + std::sort (fringe_queue_.begin (), fringe_queue_.end ()); + fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ()); + PCL_DEBUG ("Number of processed points: %lu / %lu\n", fringe_queue_.size(), indices_->size ()); + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GreedyProjectionTriangulation::closeTriangle (std::vector &polygons) +{ + state_[R_] = COMPLETED; + addTriangle (angles_[0].index, angles_[1].index, R_, polygons); + for (int aIdx=0; aIdx<2; aIdx++) + { + if (ffn_[angles_[aIdx].index] == R_) + { + if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index) + { + state_[angles_[aIdx].index] = COMPLETED; + } + else + { + ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index; + } + } + else if (sfn_[angles_[aIdx].index] == R_) + { + if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index) + { + state_[angles_[aIdx].index] = COMPLETED; + } + else + { + sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index; + } + } + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GreedyProjectionTriangulation::connectPoint ( + std::vector &polygons, + const int prev_index, const int next_index, const int next_next_index, + const Eigen::Vector2f &uvn_current, + const Eigen::Vector2f &uvn_prev, + const Eigen::Vector2f &uvn_next) +{ + if (is_current_free_) + { + ffn_[current_index_] = prev_index; + sfn_[current_index_] = next_index; + } + else + { + if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_)) + state_[current_index_] = COMPLETED; + else if (prev_is_ffn_ && !next_is_sfn_) + ffn_[current_index_] = next_index; + else if (next_is_ffn_ && !prev_is_sfn_) + ffn_[current_index_] = prev_index; + else if (prev_is_sfn_ && !next_is_ffn_) + sfn_[current_index_] = next_index; + else if (next_is_sfn_ && !prev_is_ffn_) + sfn_[current_index_] = prev_index; + else + { + bool found_triangle = false; + if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index]))) + { + found_triangle = true; + addTriangle (current_index_, ffn_[current_index_], prev_index, polygons); + state_[prev_index] = COMPLETED; + state_[ffn_[current_index_]] = COMPLETED; + ffn_[current_index_] = next_index; + } + else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index]))) + { + found_triangle = true; + addTriangle (current_index_, sfn_[current_index_], prev_index, polygons); + state_[prev_index] = COMPLETED; + state_[sfn_[current_index_]] = COMPLETED; + sfn_[current_index_] = next_index; + } + else if (state_[next_index] > FREE) + { + if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index])) + { + found_triangle = true; + addTriangle (current_index_, ffn_[current_index_], next_index, polygons); + + if (ffn_[current_index_] == ffn_[next_index]) + { + ffn_[next_index] = current_index_; + } + else + { + sfn_[next_index] = current_index_; + } + state_[ffn_[current_index_]] = COMPLETED; + ffn_[current_index_] = prev_index; + } + else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index])) + { + found_triangle = true; + addTriangle (current_index_, sfn_[current_index_], next_index, polygons); + + if (sfn_[current_index_] == ffn_[next_index]) + { + ffn_[next_index] = current_index_; + } + else + { + sfn_[next_index] = current_index_; + } + state_[sfn_[current_index_]] = COMPLETED; + sfn_[current_index_] = prev_index; + } + } + + if (found_triangle) + { + } + else + { + tmp_ = coords_[ffn_[current_index_]] - proj_qp_; + uvn_ffn_[0] = tmp_.dot(u_); + uvn_ffn_[1] = tmp_.dot(v_); + tmp_ = coords_[sfn_[current_index_]] - proj_qp_; + uvn_sfn_[0] = tmp_.dot(u_); + uvn_sfn_[1] = tmp_.dot(v_); + bool prev_ffn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_); + bool prev_sfn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_); + bool next_ffn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) && isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_); + bool next_sfn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) && isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_); + int min_dist = -1; + if (prev_ffn && next_sfn && prev_sfn && next_ffn) + { + /* should be never the case */ + double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm (); + double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm (); + double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); + double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); + if (prev2f < prev2s) + { + if (prev2f < next2f) + { + if (prev2f < next2s) + min_dist = 0; + else + min_dist = 3; + } + else + { + if (next2f < next2s) + min_dist = 2; + else + min_dist = 3; + } + } + else + { + if (prev2s < next2f) + { + if (prev2s < next2s) + min_dist = 1; + else + min_dist = 3; + } + else + { + if (next2f < next2s) + min_dist = 2; + else + min_dist = 3; + } + } + } + else if (prev_ffn && next_sfn) + { + /* a clear case */ + double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm (); + double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm (); + if (prev2f < next2s) + min_dist = 0; + else + min_dist = 3; + } + else if (prev_sfn && next_ffn) + { + /* a clear case */ + double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); + double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); + if (prev2s < next2f) + min_dist = 1; + else + min_dist = 2; + } + /* straightforward cases */ + else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn) + min_dist = 0; + else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn) + min_dist = 1; + else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn) + min_dist = 2; + else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn) + min_dist = 3; + /* messed up cases */ + else if (prev_ffn) + { + double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm (); + if (prev_sfn) + { + double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); + if (prev2s < prev2f) + min_dist = 1; + else + min_dist = 0; + } + else if (next_ffn) + { + double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); + if (next2f < prev2f) + min_dist = 2; + else + min_dist = 0; + } + } + else if (next_sfn) + { + double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm (); + if (prev_sfn) + { + double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm (); + if (prev2s < next2s) + min_dist = 1; + else + min_dist = 3; + } + else if (next_ffn) + { + double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm (); + if (next2f < next2s) + min_dist = 2; + else + min_dist = 3; + } + } + switch (min_dist) + { + case 0://prev2f: + { + addTriangle (current_index_, ffn_[current_index_], prev_index, polygons); + + /* updating prev_index */ + if (ffn_[prev_index] == current_index_) + { + ffn_[prev_index] = ffn_[current_index_]; + } + else if (sfn_[prev_index] == current_index_) + { + sfn_[prev_index] = ffn_[current_index_]; + } + else if (ffn_[prev_index] == R_) + { + changed_1st_fn_ = true; + ffn_[prev_index] = ffn_[current_index_]; + } + else if (sfn_[prev_index] == R_) + { + changed_1st_fn_ = true; + sfn_[prev_index] = ffn_[current_index_]; + } + else if (prev_index == R_) + { + new2boundary_ = ffn_[current_index_]; + } + + /* updating ffn */ + if (ffn_[ffn_[current_index_]] == current_index_) + { + ffn_[ffn_[current_index_]] = prev_index; + } + else if (sfn_[ffn_[current_index_]] == current_index_) + { + sfn_[ffn_[current_index_]] = prev_index; + } + + /* updating current */ + ffn_[current_index_] = next_index; + + break; + } + case 1://prev2s: + { + addTriangle (current_index_, sfn_[current_index_], prev_index, polygons); + + /* updating prev_index */ + if (ffn_[prev_index] == current_index_) + { + ffn_[prev_index] = sfn_[current_index_]; + } + else if (sfn_[prev_index] == current_index_) + { + sfn_[prev_index] = sfn_[current_index_]; + } + else if (ffn_[prev_index] == R_) + { + changed_1st_fn_ = true; + ffn_[prev_index] = sfn_[current_index_]; + } + else if (sfn_[prev_index] == R_) + { + changed_1st_fn_ = true; + sfn_[prev_index] = sfn_[current_index_]; + } + else if (prev_index == R_) + { + new2boundary_ = sfn_[current_index_]; + } + + /* updating sfn */ + if (ffn_[sfn_[current_index_]] == current_index_) + { + ffn_[sfn_[current_index_]] = prev_index; + } + else if (sfn_[sfn_[current_index_]] == current_index_) + { + sfn_[sfn_[current_index_]] = prev_index; + } + + /* updating current */ + sfn_[current_index_] = next_index; + + break; + } + case 2://next2f: + { + addTriangle (current_index_, ffn_[current_index_], next_index, polygons); + int neighbor_update = next_index; + + /* updating next_index */ + if (state_[next_index] <= FREE) + { + state_[next_index] = FRINGE; + ffn_[next_index] = current_index_; + sfn_[next_index] = ffn_[current_index_]; + } + else + { + if (ffn_[next_index] == R_) + { + changed_2nd_fn_ = true; + ffn_[next_index] = ffn_[current_index_]; + } + else if (sfn_[next_index] == R_) + { + changed_2nd_fn_ = true; + sfn_[next_index] = ffn_[current_index_]; + } + else if (next_index == R_) + { + new2boundary_ = ffn_[current_index_]; + if (next_next_index == new2boundary_) + already_connected_ = true; + } + else if (ffn_[next_index] == next_next_index) + { + already_connected_ = true; + ffn_[next_index] = ffn_[current_index_]; + } + else if (sfn_[next_index] == next_next_index) + { + already_connected_ = true; + sfn_[next_index] = ffn_[current_index_]; + } + else + { + tmp_ = coords_[ffn_[next_index]] - proj_qp_; + uvn_next_ffn_[0] = tmp_.dot(u_); + uvn_next_ffn_[1] = tmp_.dot(v_); + tmp_ = coords_[sfn_[next_index]] - proj_qp_; + uvn_next_sfn_[0] = tmp_.dot(u_); + uvn_next_sfn_[1] = tmp_.dot(v_); + + bool ffn_next_ffn = isVisible(uvn_next_ffn_, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_next_ffn_, uvn_next, uvn_next_sfn_, uvn_ffn_); + bool sfn_next_ffn = isVisible(uvn_next_sfn_, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_next_sfn_, uvn_next, uvn_next_ffn_, uvn_ffn_); + + int connect2ffn = -1; + if (ffn_next_ffn && sfn_next_ffn) + { + double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm (); + double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm (); + if (fn2f < sn2f) connect2ffn = 0; + else connect2ffn = 1; + } + else if (ffn_next_ffn) connect2ffn = 0; + else if (sfn_next_ffn) connect2ffn = 1; + + switch (connect2ffn) + { + case 0: // ffn[next] + { + addTriangle (next_index, ffn_[current_index_], ffn_[next_index], polygons); + neighbor_update = ffn_[next_index]; + + /* ffn[next_index] */ + if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_])) + { + state_[ffn_[next_index]] = COMPLETED; + } + else if (ffn_[ffn_[next_index]] == next_index) + { + ffn_[ffn_[next_index]] = ffn_[current_index_]; + } + else if (sfn_[ffn_[next_index]] == next_index) + { + sfn_[ffn_[next_index]] = ffn_[current_index_]; + } + + ffn_[next_index] = current_index_; + + break; + } + case 1: // sfn[next] + { + addTriangle (next_index, ffn_[current_index_], sfn_[next_index], polygons); + neighbor_update = sfn_[next_index]; + + /* sfn[next_index] */ + if ((ffn_[sfn_[next_index]] == ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_])) + { + state_[sfn_[next_index]] = COMPLETED; + } + else if (ffn_[sfn_[next_index]] == next_index) + { + ffn_[sfn_[next_index]] = ffn_[current_index_]; + } + else if (sfn_[sfn_[next_index]] == next_index) + { + sfn_[sfn_[next_index]] = ffn_[current_index_]; + } + + sfn_[next_index] = current_index_; + + break; + } + default:; + } + } + } + + /* updating ffn */ + if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update)) + { + state_[ffn_[current_index_]] = COMPLETED; + } + else if (ffn_[ffn_[current_index_]] == current_index_) + { + ffn_[ffn_[current_index_]] = neighbor_update; + } + else if (sfn_[ffn_[current_index_]] == current_index_) + { + sfn_[ffn_[current_index_]] = neighbor_update; + } + + /* updating current */ + ffn_[current_index_] = prev_index; + + break; + } + case 3://next2s: + { + addTriangle (current_index_, sfn_[current_index_], next_index, polygons); + int neighbor_update = next_index; + + /* updating next_index */ + if (state_[next_index] <= FREE) + { + state_[next_index] = FRINGE; + ffn_[next_index] = current_index_; + sfn_[next_index] = sfn_[current_index_]; + } + else + { + if (ffn_[next_index] == R_) + { + changed_2nd_fn_ = true; + ffn_[next_index] = sfn_[current_index_]; + } + else if (sfn_[next_index] == R_) + { + changed_2nd_fn_ = true; + sfn_[next_index] = sfn_[current_index_]; + } + else if (next_index == R_) + { + new2boundary_ = sfn_[current_index_]; + if (next_next_index == new2boundary_) + already_connected_ = true; + } + else if (ffn_[next_index] == next_next_index) + { + already_connected_ = true; + ffn_[next_index] = sfn_[current_index_]; + } + else if (sfn_[next_index] == next_next_index) + { + already_connected_ = true; + sfn_[next_index] = sfn_[current_index_]; + } + else + { + tmp_ = coords_[ffn_[next_index]] - proj_qp_; + uvn_next_ffn_[0] = tmp_.dot(u_); + uvn_next_ffn_[1] = tmp_.dot(v_); + tmp_ = coords_[sfn_[next_index]] - proj_qp_; + uvn_next_sfn_[0] = tmp_.dot(u_); + uvn_next_sfn_[1] = tmp_.dot(v_); + + bool ffn_next_sfn = isVisible(uvn_next_ffn_, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_next_ffn_, uvn_next, uvn_next_sfn_, uvn_sfn_); + bool sfn_next_sfn = isVisible(uvn_next_sfn_, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_next_sfn_, uvn_next, uvn_next_ffn_, uvn_sfn_); + + int connect2sfn = -1; + if (ffn_next_sfn && sfn_next_sfn) + { + double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm (); + double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm (); + if (fn2s < sn2s) connect2sfn = 0; + else connect2sfn = 1; + } + else if (ffn_next_sfn) connect2sfn = 0; + else if (sfn_next_sfn) connect2sfn = 1; + + switch (connect2sfn) + { + case 0: // ffn[next] + { + addTriangle (next_index, sfn_[current_index_], ffn_[next_index], polygons); + neighbor_update = ffn_[next_index]; + + /* ffn[next_index] */ + if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_])) + { + state_[ffn_[next_index]] = COMPLETED; + } + else if (ffn_[ffn_[next_index]] == next_index) + { + ffn_[ffn_[next_index]] = sfn_[current_index_]; + } + else if (sfn_[ffn_[next_index]] == next_index) + { + sfn_[ffn_[next_index]] = sfn_[current_index_]; + } + + ffn_[next_index] = current_index_; + + break; + } + case 1: // sfn[next] + { + addTriangle (next_index, sfn_[current_index_], sfn_[next_index], polygons); + neighbor_update = sfn_[next_index]; + + /* sfn[next_index] */ + if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_])) + { + state_[sfn_[next_index]] = COMPLETED; + } + else if (ffn_[sfn_[next_index]] == next_index) + { + ffn_[sfn_[next_index]] = sfn_[current_index_]; + } + else if (sfn_[sfn_[next_index]] == next_index) + { + sfn_[sfn_[next_index]] = sfn_[current_index_]; + } + + sfn_[next_index] = current_index_; + + break; + } + default:; + } + } + } + + /* updating sfn */ + if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update)) + { + state_[sfn_[current_index_]] = COMPLETED; + } + else if (ffn_[sfn_[current_index_]] == current_index_) + { + ffn_[sfn_[current_index_]] = neighbor_update; + } + else if (sfn_[sfn_[current_index_]] == current_index_) + { + sfn_[sfn_[current_index_]] = neighbor_update; + } + + sfn_[current_index_] = prev_index; + + break; + } + default:; + } + } + } + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template std::vector > +pcl::GreedyProjectionTriangulation::getTriangleList (const pcl::PolygonMesh &input) +{ + std::vector > triangleList (input.cloud.width * input.cloud.height); + + for (std::size_t i=0; i < input.polygons.size (); ++i) + for (std::size_t j=0; j < input.polygons[i].vertices.size (); ++j) + triangleList[input.polygons[i].vertices[j]].push_back (i); + return (triangleList); +} + +#define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \ + template class PCL_EXPORTS pcl::GreedyProjectionTriangulation; + +#endif // PCL_SURFACE_IMPL_GP3_H_ + + diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/grid_projection.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/grid_projection.hpp new file mode 100755 index 0000000..864b6ab --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/grid_projection.hpp @@ -0,0 +1,774 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SURFACE_IMPL_GRID_PROJECTION_H_ +#define PCL_SURFACE_IMPL_GRID_PROJECTION_H_ + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::GridProjection::GridProjection () : + cell_hash_map_ (), leaf_size_ (0.001), gaussian_scale_ (), + data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ () +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::GridProjection::GridProjection (double resolution) : + cell_hash_map_ (), leaf_size_ (resolution), gaussian_scale_ (), + data_size_ (0), max_binary_search_level_ (10), k_ (50), padding_size_ (3), data_ () +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::GridProjection::~GridProjection () +{ + vector_at_data_point_.clear (); + surface_.clear (); + cell_hash_map_.clear (); + occupied_cell_list_.clear (); + data_.reset (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::scaleInputDataPoint (double scale_factor) +{ + for (std::size_t i = 0; i < data_->points.size(); ++i) + { + data_->points[i].x /= static_cast (scale_factor); + data_->points[i].y /= static_cast (scale_factor); + data_->points[i].z /= static_cast (scale_factor); + } + max_p_ /= static_cast (scale_factor); + min_p_ /= static_cast (scale_factor); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::getBoundingBox () +{ + pcl::getMinMax3D (*data_, min_p_, max_p_); + + Eigen::Vector4f bounding_box_size = max_p_ - min_p_; + double scale_factor = (std::max)((std::max)(bounding_box_size.x (), + bounding_box_size.y ()), + bounding_box_size.z ()); + if (scale_factor > 1) + scaleInputDataPoint (scale_factor); + + // Round the max_p_, min_p_ so that they are aligned with the cells vertices + int upper_right_index[3]; + int lower_left_index[3]; + for (std::size_t i = 0; i < 3; ++i) + { + upper_right_index[i] = static_cast (max_p_(i) / leaf_size_ + 5); + lower_left_index[i] = static_cast (min_p_(i) / leaf_size_ - 5); + max_p_(i) = static_cast (upper_right_index[i] * leaf_size_); + min_p_(i) = static_cast (lower_left_index[i] * leaf_size_); + } + bounding_box_size = max_p_ - min_p_; + PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n", + bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ()); + double max_size = + (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), + bounding_box_size.z ()); + + data_size_ = static_cast (max_size / leaf_size_); + PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Lower left point is [%f, %f, %f]\n", + min_p_.x (), min_p_.y (), min_p_.z ()); + PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Upper left point is [%f, %f, %f]\n", + max_p_.x (), max_p_.y (), max_p_.z ()); + PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Padding size: %d\n", padding_size_); + PCL_DEBUG ("[pcl::GridProjection::getBoundingBox] Leaf size: %f\n", leaf_size_); + occupied_cell_list_.resize (data_size_ * data_size_ * data_size_); + gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::getVertexFromCellCenter ( + const Eigen::Vector4f &cell_center, + std::vector > &pts) const +{ + assert (pts.size () == 8); + + float sz = static_cast (leaf_size_) / 2.0f; + + pts[0] = cell_center + Eigen::Vector4f (-sz, sz, -sz, 0); + pts[1] = cell_center + Eigen::Vector4f (-sz, -sz, -sz, 0); + pts[2] = cell_center + Eigen::Vector4f (sz, -sz, -sz, 0); + pts[3] = cell_center + Eigen::Vector4f (sz, sz, -sz, 0); + pts[4] = cell_center + Eigen::Vector4f (-sz, sz, sz, 0); + pts[5] = cell_center + Eigen::Vector4f (-sz, -sz, sz, 0); + pts[6] = cell_center + Eigen::Vector4f (sz, -sz, sz, 0); + pts[7] = cell_center + Eigen::Vector4f (sz, sz, sz, 0); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::getDataPtsUnion (const Eigen::Vector3i &index, + std::vector &pt_union_indices) +{ + for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i) + { + for (int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j) + { + for (int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k) + { + Eigen::Vector3i cell_index_3d (i, j, k); + int cell_index_1d = getIndexIn1D (cell_index_3d); + if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ()) + { + // Get the indices of the input points within the cell(i,j,k), which + // is stored in the hash map + pt_union_indices.insert (pt_union_indices.end (), + cell_hash_map_.at (cell_index_1d).data_indices.begin (), + cell_hash_map_.at (cell_index_1d).data_indices.end ()); + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::createSurfaceForCell (const Eigen::Vector3i &index, + std::vector &pt_union_indices) +{ + // 8 vertices of the cell + std::vector > vertices (8); + + // 4 end points that shared by 3 edges connecting the upper left front points + Eigen::Vector4f pts[4]; + Eigen::Vector3f vector_at_pts[4]; + + // Given the index of cell, caluate the coordinates of the eight vertices of the cell + // index the index of the cell in (x,y,z) 3d format + Eigen::Vector4f cell_center = Eigen::Vector4f::Zero (); + getCellCenterFromIndex (index, cell_center); + getVertexFromCellCenter (cell_center, vertices); + + // Get the indices of the cells which stores the 4 end points. + Eigen::Vector3i indices[4]; + indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1); + indices[1] = Eigen::Vector3i (index[0], index[1], index[2]); + indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]); + indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]); + + // Get the coordinate of the 4 end points, and the corresponding vectors + for (std::size_t i = 0; i < 4; ++i) + { + pts[i] = vertices[I_SHIFT_PT[i]]; + unsigned int index_1d = getIndexIn1D (indices[i]); + if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () || + occupied_cell_list_[index_1d] == 0) + return; + vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt; + } + + // Go through the 3 edges, test whether they are intersected by the surface + for (std::size_t i = 0; i < 3; ++i) + { + std::vector > end_pts (2); + std::vector > vect_at_end_pts (2); + for (std::size_t j = 0; j < 2; ++j) + { + end_pts[j] = pts[I_SHIFT_EDGE[i][j]]; + vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]]; + } + + if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices)) + { + // Indices of cells that contains points which will be connected to + // create a polygon + Eigen::Vector3i polygon[4]; + Eigen::Vector4f polygon_pts[4]; + int polygon_indices_1d[4]; + bool is_all_in_hash_map = true; + switch (i) + { + case 0: + polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]); + polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]); + polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]); + polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]); + break; + case 1: + polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1); + polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]); + polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]); + polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1); + break; + case 2: + polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1); + polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]); + polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]); + polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1); + break; + default: + break; + } + for (std::size_t k = 0; k < 4; k++) + { + polygon_indices_1d[k] = getIndexIn1D (polygon[k]); + if (!occupied_cell_list_[polygon_indices_1d[k]]) + { + is_all_in_hash_map = false; + break; + } + } + if (is_all_in_hash_map) + { + for (std::size_t k = 0; k < 4; k++) + { + polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface; + surface_.push_back (polygon_pts[k]); + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::getProjection (const Eigen::Vector4f &p, + std::vector &pt_union_indices, Eigen::Vector4f &projection) +{ + const double projection_distance = leaf_size_ * 3; + std::vector > end_pt (2); + std::vector > end_pt_vect (2); + end_pt[0] = p; + getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]); + end_pt_vect[0].normalize(); + + double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices); + + // Find another point which is projection_distance away from the p, do a + // binary search between these two points, to find the projected point on the + // surface + if (dSecond > 0) + end_pt[1] = end_pt[0] + Eigen::Vector4f ( + end_pt_vect[0][0] * static_cast (projection_distance), + end_pt_vect[0][1] * static_cast (projection_distance), + end_pt_vect[0][2] * static_cast (projection_distance), + 0.0f); + else + end_pt[1] = end_pt[0] - Eigen::Vector4f ( + end_pt_vect[0][0] * static_cast (projection_distance), + end_pt_vect[0][1] * static_cast (projection_distance), + end_pt_vect[0][2] * static_cast (projection_distance), + 0.0f); + getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]); + if (end_pt_vect[1].dot (end_pt_vect[0]) < 0) + { + Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5; + findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection); + } + else + projection = p; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::getProjectionWithPlaneFit (const Eigen::Vector4f &p, + std::vector (&pt_union_indices), + Eigen::Vector4f &projection) +{ + // Compute the plane coefficients + Eigen::Vector4f model_coefficients; + /// @remark iterative weighted least squares or sac might give better results + Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f xyz_centroid; + + computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid); + + // Get the plane normal + EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + + // The normalization is not necessary, since the eigenvectors from libeigen are already normalized + model_coefficients[0] = eigen_vector [0]; + model_coefficients[1] = eigen_vector [1]; + model_coefficients[2] = eigen_vector [2]; + model_coefficients[3] = 0; + // Hessian form (D = nc . p_plane (centroid here) + p) + model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); + + // Projected point + Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output.points[cp].x, 3); + float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3]; + point -= distance * model_coefficients.head < 3 > (); + + projection = Eigen::Vector4f (point[0], point[1], point[2], 0); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::getVectorAtPoint (const Eigen::Vector4f &p, + std::vector &pt_union_indices, + Eigen::Vector3f &vo) +{ + std::vector pt_union_dist (pt_union_indices.size ()); + std::vector pt_union_weight (pt_union_indices.size ()); + Eigen::Vector3f out_vector (0, 0, 0); + double sum = 0.0; + double mag = 0.0; + + for (std::size_t i = 0; i < pt_union_indices.size (); ++i) + { + Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0); + pt_union_dist[i] = (pp - p).squaredNorm (); + pt_union_weight[i] = pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_); + mag += pow (M_E, -pow (sqrt (pt_union_dist[i]), 2.0) / gaussian_scale_); + sum += pt_union_weight[i]; + } + + pcl::VectorAverage3f vector_average; + + Eigen::Vector3f v ( + data_->points[pt_union_indices[0]].normal[0], + data_->points[pt_union_indices[0]].normal[1], + data_->points[pt_union_indices[0]].normal[2]); + + for (std::size_t i = 0; i < pt_union_weight.size (); ++i) + { + pt_union_weight[i] /= sum; + Eigen::Vector3f vec (data_->points[pt_union_indices[i]].normal[0], + data_->points[pt_union_indices[i]].normal[1], + data_->points[pt_union_indices[i]].normal[2]); + if (vec.dot (v) < 0) + vec = -vec; + vector_average.add (vec, static_cast (pt_union_weight[i])); + } + out_vector = vector_average.getMean (); + // vector_average.getEigenVector1(out_vector); + + out_vector.normalize (); + double d1 = getD1AtPoint (p, out_vector, pt_union_indices); + out_vector *= static_cast (sum); + vo = ((d1 > 0) ? -1 : 1) * out_vector; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::getVectorAtPointKNN (const Eigen::Vector4f &p, + std::vector &k_indices, + std::vector &k_squared_distances, + Eigen::Vector3f &vo) +{ + Eigen::Vector3f out_vector (0, 0, 0); + std::vector k_weight; + k_weight.resize (k_); + float sum = 0.0; + for (int i = 0; i < k_; i++) + { + //k_weight[i] = pow (M_E, -pow (k_squared_distances[i], 2.0) / gaussian_scale_); + k_weight[i] = std::pow (static_cast(M_E), static_cast(-std::pow (static_cast(k_squared_distances[i]), 2.0f) / gaussian_scale_)); + sum += k_weight[i]; + } + pcl::VectorAverage3f vector_average; + for (int i = 0; i < k_; i++) + { + k_weight[i] /= sum; + Eigen::Vector3f vec (data_->points[k_indices[i]].normal[0], + data_->points[k_indices[i]].normal[1], + data_->points[k_indices[i]].normal[2]); + vector_average.add (vec, k_weight[i]); + } + vector_average.getEigenVector1 (out_vector); + out_vector.normalize (); + double d1 = getD1AtPoint (p, out_vector, k_indices); + out_vector *= sum; + vo = ((d1 > 0) ? -1 : 1) * out_vector; + +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::GridProjection::getMagAtPoint (const Eigen::Vector4f &p, + const std::vector &pt_union_indices) +{ + std::vector pt_union_dist (pt_union_indices.size ()); + std::vector pt_union_weight (pt_union_indices.size ()); + double sum = 0.0; + for (std::size_t i = 0; i < pt_union_indices.size (); ++i) + { + Eigen::Vector4f pp (data_->points[pt_union_indices[i]].x, data_->points[pt_union_indices[i]].y, data_->points[pt_union_indices[i]].z, 0); + pt_union_dist[i] = (pp - p).norm (); + sum += pow (M_E, -pow (pt_union_dist[i], 2.0) / gaussian_scale_); + } + return (sum); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::GridProjection::getD1AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, + const std::vector &pt_union_indices) +{ + double sz = 0.01 * leaf_size_; + Eigen::Vector3f v = vec * static_cast (sz); + + double forward = getMagAtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices); + double backward = getMagAtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), pt_union_indices); + return ((forward - backward) / (0.02 * leaf_size_)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::GridProjection::getD2AtPoint (const Eigen::Vector4f &p, const Eigen::Vector3f &vec, + const std::vector &pt_union_indices) +{ + double sz = 0.01 * leaf_size_; + Eigen::Vector3f v = vec * static_cast (sz); + + double forward = getD1AtPoint (p + Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices); + double backward = getD1AtPoint (p - Eigen::Vector4f (v[0], v[1], v[2], 0), vec, pt_union_indices); + return ((forward - backward) / (0.02 * leaf_size_)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::GridProjection::isIntersected (const std::vector > &end_pts, + std::vector > &vect_at_end_pts, + std::vector &pt_union_indices) +{ + assert (end_pts.size () == 2); + assert (vect_at_end_pts.size () == 2); + + double length[2]; + for (std::size_t i = 0; i < 2; ++i) + { + length[i] = vect_at_end_pts[i].norm (); + vect_at_end_pts[i].normalize (); + } + double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]); + if (dot_prod < 0) + { + double ratio = length[0] / (length[0] + length[1]); + Eigen::Vector4f start_pt = + end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast (ratio); + Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero (); + findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt); + + Eigen::Vector3f vec; + getVectorAtPoint (intersection_pt, pt_union_indices, vec); + vec.normalize (); + + double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices); + if (d2 < 0) + return (true); + } + return (false); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::findIntersection (int level, + const std::vector > &end_pts, + const std::vector > &vect_at_end_pts, + const Eigen::Vector4f &start_pt, + std::vector &pt_union_indices, + Eigen::Vector4f &intersection) +{ + assert (end_pts.size () == 2); + assert (vect_at_end_pts.size () == 2); + + Eigen::Vector3f vec; + getVectorAtPoint (start_pt, pt_union_indices, vec); + double d1 = getD1AtPoint (start_pt, vec, pt_union_indices); + std::vector > new_end_pts (2); + std::vector > new_vect_at_end_pts (2); + if ((std::abs (d1) < 10e-3) || (level == max_binary_search_level_)) + { + intersection = start_pt; + return; + } + vec.normalize (); + if (vec.dot (vect_at_end_pts[0]) < 0) + { + Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5; + new_end_pts[0] = end_pts[0]; + new_end_pts[1] = start_pt; + new_vect_at_end_pts[0] = vect_at_end_pts[0]; + new_vect_at_end_pts[1] = vec; + findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection); + return; + } + if (vec.dot (vect_at_end_pts[1]) < 0) + { + Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5; + new_end_pts[0] = start_pt; + new_end_pts[1] = end_pts[1]; + new_vect_at_end_pts[0] = vec; + new_vect_at_end_pts[1] = vect_at_end_pts[1]; + findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection); + return; + } + intersection = start_pt; + return; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::fillPad (const Eigen::Vector3i &index) +{ + for (int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i) + { + for (int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j) + { + for (int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k) + { + Eigen::Vector3i cell_index_3d (i, j, k); + unsigned int cell_index_1d = getIndexIn1D (cell_index_3d); + if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ()) + { + cell_hash_map_[cell_index_1d].data_indices.resize (0); + getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface); + } + } + } + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::storeVectAndSurfacePoint (int index_1d, + const Eigen::Vector3i &, + std::vector &pt_union_indices, + const Leaf &cell_data) +{ + // Get point on grid + Eigen::Vector4f grid_pt ( + cell_data.pt_on_surface.x () - static_cast (leaf_size_) / 2.0f, + cell_data.pt_on_surface.y () + static_cast (leaf_size_) / 2.0f, + cell_data.pt_on_surface.z () + static_cast (leaf_size_) / 2.0f, 0.0f); + + // Save the vector and the point on the surface + getVectorAtPoint (grid_pt, pt_union_indices, cell_hash_map_[index_1d].vect_at_grid_pt); + getProjection (cell_data.pt_on_surface, pt_union_indices, cell_hash_map_[index_1d].pt_on_surface); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::storeVectAndSurfacePointKNN (int index_1d, const Eigen::Vector3i &, + const Leaf &cell_data) +{ + Eigen::Vector4f cell_center = cell_data.pt_on_surface; + Eigen::Vector4f grid_pt ( + cell_center.x () - static_cast (leaf_size_) / 2.0f, + cell_center.y () + static_cast (leaf_size_) / 2.0f, + cell_center.z () + static_cast (leaf_size_) / 2.0f, 0.0f); + + std::vector k_indices; + k_indices.resize (k_); + std::vector k_squared_distances; + k_squared_distances.resize (k_); + + PointNT pt; pt.x = grid_pt.x (); pt.y = grid_pt.y (); pt.z = grid_pt.z (); + tree_->nearestKSearch (pt, k_, k_indices, k_squared_distances); + + getVectorAtPointKNN (grid_pt, k_indices, k_squared_distances, cell_hash_map_[index_1d].vect_at_grid_pt); + getProjectionWithPlaneFit (cell_center, k_indices, cell_hash_map_[index_1d].pt_on_surface); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::GridProjection::reconstructPolygons (std::vector &polygons) +{ + data_.reset (new pcl::PointCloud (*input_)); + getBoundingBox (); + + // Store the point cloud data into the voxel grid, and at the same time + // create a hash map to store the information for each cell + cell_hash_map_.max_load_factor (2.0); + cell_hash_map_.rehash (data_->points.size () / static_cast (cell_hash_map_.max_load_factor ())); + + // Go over all points and insert them into the right leaf + for (int cp = 0; cp < static_cast (data_->points.size ()); ++cp) + { + // Check if the point is invalid + if (!std::isfinite (data_->points[cp].x) || + !std::isfinite (data_->points[cp].y) || + !std::isfinite (data_->points[cp].z)) + continue; + + Eigen::Vector3i index_3d; + getCellIndex (data_->points[cp].getVector4fMap (), index_3d); + int index_1d = getIndexIn1D (index_3d); + if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ()) + { + Leaf cell_data; + cell_data.data_indices.push_back (cp); + getCellCenterFromIndex (index_3d, cell_data.pt_on_surface); + cell_hash_map_[index_1d] = cell_data; + occupied_cell_list_[index_1d] = 1; + } + else + { + Leaf cell_data = cell_hash_map_.at (index_1d); + cell_data.data_indices.push_back (cp); + cell_hash_map_[index_1d] = cell_data; + } + } + + Eigen::Vector3i index; + int numOfFilledPad = 0; + + for (int i = 0; i < data_size_; ++i) + { + for (int j = 0; j < data_size_; ++j) + { + for (int k = 0; k < data_size_; ++k) + { + index[0] = i; + index[1] = j; + index[2] = k; + if (occupied_cell_list_[getIndexIn1D (index)]) + { + fillPad (index); + numOfFilledPad++; + } + } + } + } + + // Update the hashtable and store the vector and point + for (const auto &entry : cell_hash_map_) + { + getIndexIn3D (entry.first, index); + std::vector pt_union_indices; + getDataPtsUnion (index, pt_union_indices); + + // Needs at least 10 points (?) + // NOTE: set as parameter later + if (pt_union_indices.size () > 10) + { + storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second); + //storeVectAndSurfacePointKNN(entry.first, index, entry.second); + occupied_cell_list_[entry.first] = 1; + } + } + + // Go through the hash table another time to extract surface + for (const auto &entry : cell_hash_map_) + { + getIndexIn3D (entry.first, index); + std::vector pt_union_indices; + getDataPtsUnion (index, pt_union_indices); + + // Needs at least 10 points (?) + // NOTE: set as parameter later + if (pt_union_indices.size () > 10) + createSurfaceForCell (index, pt_union_indices); + } + + polygons.resize (surface_.size () / 4); + // Copy the data from surface_ to polygons + for (int i = 0; i < static_cast (polygons.size ()); ++i) + { + pcl::Vertices v; + v.vertices.resize (4); + for (int j = 0; j < 4; ++j) + v.vertices[j] = i*4+j; + polygons[i] = v; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::performReconstruction (pcl::PolygonMesh &output) +{ + if (!reconstructPolygons (output.polygons)) + return; + + // The mesh surface is held in surface_. Copy it to the output format + output.header = input_->header; + + pcl::PointCloud cloud; + cloud.width = static_cast (surface_.size ()); + cloud.height = 1; + cloud.is_dense = true; + + cloud.points.resize (surface_.size ()); + // Copy the data from surface_ to cloud + for (std::size_t i = 0; i < cloud.points.size (); ++i) + { + cloud.points[i].x = surface_[i].x (); + cloud.points[i].y = surface_[i].y (); + cloud.points[i].z = surface_[i].z (); + } + pcl::toPCLPointCloud2 (cloud, output.cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GridProjection::performReconstruction (pcl::PointCloud &points, + std::vector &polygons) +{ + if (!reconstructPolygons (polygons)) + return; + + // The mesh surface is held in surface_. Copy it to the output format + points.header = input_->header; + points.width = static_cast (surface_.size ()); + points.height = 1; + points.is_dense = true; + + points.resize (surface_.size ()); + // Copy the data from surface_ to cloud + for (std::size_t i = 0; i < points.size (); ++i) + { + points[i].x = surface_[i].x (); + points[i].y = surface_[i].y (); + points[i].z = surface_[i].z (); + } +} + +#define PCL_INSTANTIATE_GridProjection(T) template class PCL_EXPORTS pcl::GridProjection; + +#endif // PCL_SURFACE_IMPL_GRID_PROJECTION_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes.hpp new file mode 100755 index 0000000..39dbca0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes.hpp @@ -0,0 +1,280 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_H_ +#define PCL_SURFACE_IMPL_MARCHING_CUBES_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MarchingCubes::~MarchingCubes () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubes::getBoundingBox () +{ + PointNT max_pt, min_pt; + pcl::getMinMax3D (*input_, min_pt, max_pt); + + lower_boundary_ = min_pt.getArray3fMap (); + upper_boundary_ = max_pt.getArray3fMap (); + + const Eigen::Array3f size3_extend = 0.5f * percentage_extend_grid_ + * (upper_boundary_ - lower_boundary_); + + lower_boundary_ -= size3_extend; + upper_boundary_ += size3_extend; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubes::interpolateEdge (Eigen::Vector3f &p1, + Eigen::Vector3f &p2, + float val_p1, + float val_p2, + Eigen::Vector3f &output) +{ + const float mu = (iso_level_ - val_p1) / (val_p2 - val_p1); + output = p1 + mu * (p2 - p1); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubes::createSurface (const std::vector &leaf_node, + const Eigen::Vector3i &index_3d, + pcl::PointCloud &cloud) +{ + int cubeindex = 0; + if (leaf_node[0] < iso_level_) cubeindex |= 1; + if (leaf_node[1] < iso_level_) cubeindex |= 2; + if (leaf_node[2] < iso_level_) cubeindex |= 4; + if (leaf_node[3] < iso_level_) cubeindex |= 8; + if (leaf_node[4] < iso_level_) cubeindex |= 16; + if (leaf_node[5] < iso_level_) cubeindex |= 32; + if (leaf_node[6] < iso_level_) cubeindex |= 64; + if (leaf_node[7] < iso_level_) cubeindex |= 128; + + // Cube is entirely in/out of the surface + if (edgeTable[cubeindex] == 0) + return; + + const Eigen::Vector3f center = lower_boundary_ + + size_voxel_ * index_3d.cast ().array (); + + std::vector > p; + p.resize (8); + for (int i = 0; i < 8; ++i) + { + Eigen::Vector3f point = center; + if (i & 0x4) + point[1] = static_cast (center[1] + size_voxel_[1]); + + if (i & 0x2) + point[2] = static_cast (center[2] + size_voxel_[2]); + + if ((i & 0x1) ^ ((i >> 1) & 0x1)) + point[0] = static_cast (center[0] + size_voxel_[0]); + + p[i] = point; + } + + // Find the vertices where the surface intersects the cube + std::vector > vertex_list; + vertex_list.resize (12); + if (edgeTable[cubeindex] & 1) + interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]); + if (edgeTable[cubeindex] & 2) + interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]); + if (edgeTable[cubeindex] & 4) + interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]); + if (edgeTable[cubeindex] & 8) + interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]); + if (edgeTable[cubeindex] & 16) + interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]); + if (edgeTable[cubeindex] & 32) + interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]); + if (edgeTable[cubeindex] & 64) + interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]); + if (edgeTable[cubeindex] & 128) + interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]); + if (edgeTable[cubeindex] & 256) + interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]); + if (edgeTable[cubeindex] & 512) + interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]); + if (edgeTable[cubeindex] & 1024) + interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]); + if (edgeTable[cubeindex] & 2048) + interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]); + + // Create the triangle + for (int i = 0; triTable[cubeindex][i] != -1; i += 3) + { + PointNT p1, p2, p3; + p1.getVector3fMap () = vertex_list[triTable[cubeindex][i]]; + cloud.push_back (p1); + p2.getVector3fMap () = vertex_list[triTable[cubeindex][i+1]]; + cloud.push_back (p2); + p3.getVector3fMap () = vertex_list[triTable[cubeindex][i+2]]; + cloud.push_back (p3); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubes::getNeighborList1D (std::vector &leaf, + Eigen::Vector3i &index3d) +{ + leaf.resize (8); + + leaf[0] = getGridValue (index3d); + leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0)); + leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1)); + leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1)); + leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0)); + leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0)); + leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1)); + leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1)); + + for (int i = 0; i < 8; ++i) + { + if (std::isnan (leaf[i])) + { + leaf.clear (); + break; + } + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::MarchingCubes::getGridValue (Eigen::Vector3i pos) +{ + /// TODO what to return? + if (pos[0] < 0 || pos[0] >= res_x_) + return -1.0f; + if (pos[1] < 0 || pos[1] >= res_y_) + return -1.0f; + if (pos[2] < 0 || pos[2] >= res_z_) + return -1.0f; + + return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]]; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubes::performReconstruction (pcl::PolygonMesh &output) +{ + pcl::PointCloud points; + + performReconstruction (points, output.polygons); + + pcl::toPCLPointCloud2 (points, output.cloud); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubes::performReconstruction (pcl::PointCloud &points, + std::vector &polygons) +{ + if (!(iso_level_ >= 0 && iso_level_ < 1)) + { + PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", + getClassName ().c_str (), iso_level_); + points.width = points.height = 0; + points.points.clear (); + polygons.clear (); + return; + } + + // the point cloud really generated from Marching Cubes, prev intermediate_cloud_ + pcl::PointCloud intermediate_cloud; + + // Create grid + grid_ = std::vector (res_x_*res_y_*res_z_, NAN); + + // Compute bounding box and voxel size + getBoundingBox (); + size_voxel_ = (upper_boundary_ - lower_boundary_) + * Eigen::Array3f (res_x_, res_y_, res_z_).inverse (); + + // Transform the point cloud into a voxel grid + // This needs to be implemented in a child class + voxelizeData (); + + // preallocate memory assuming a hull. suppose 6 point per voxel + double size_reserve = std::min((double) intermediate_cloud.points.max_size (), + 2.0 * 6.0 * (double) (res_y_*res_z_ + res_x_*res_z_ + res_x_*res_y_)); + intermediate_cloud.reserve ((std::size_t) size_reserve); + + for (int x = 1; x < res_x_-1; ++x) + for (int y = 1; y < res_y_-1; ++y) + for (int z = 1; z < res_z_-1; ++z) + { + Eigen::Vector3i index_3d (x, y, z); + std::vector leaf_node; + getNeighborList1D (leaf_node, index_3d); + if (!leaf_node.empty ()) + createSurface (leaf_node, index_3d, intermediate_cloud); + } + + points.swap (intermediate_cloud); + + polygons.resize (points.size () / 3); + for (std::size_t i = 0; i < polygons.size (); ++i) + { + pcl::Vertices v; + v.vertices.resize (3); + for (int j = 0; j < 3; ++j) + v.vertices[j] = static_cast (i) * 3 + j; + polygons[i] = v; + } +} + +#define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes; + +#endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes_hoppe.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes_hoppe.hpp new file mode 100755 index 0000000..adcf6f1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes_hoppe.hpp @@ -0,0 +1,94 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_HOPPE_H_ +#define PCL_SURFACE_IMPL_MARCHING_CUBES_HOPPE_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MarchingCubesHoppe::~MarchingCubesHoppe () +{ +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubesHoppe::voxelizeData () +{ + const bool is_far_ignored = dist_ignore_ > 0.0f; + + for (int x = 0; x < res_x_; ++x) + { + const int y_start = x * res_y_ * res_z_; + + for (int y = 0; y < res_y_; ++y) + { + const int z_start = y_start + y * res_z_; + + for (int z = 0; z < res_z_; ++z) + { + std::vector nn_indices (1, 0); + std::vector nn_sqr_dists (1, 0.0f); + const Eigen::Vector3f point = (lower_boundary_ + size_voxel_ * Eigen::Array3f (x, y, z)).matrix (); + PointNT p; + + p.getVector3fMap () = point; + + tree_->nearestKSearch (p, 1, nn_indices, nn_sqr_dists); + + if (!is_far_ignored || nn_sqr_dists[0] < dist_ignore_) + { + const Eigen::Vector3f normal = input_->points[nn_indices[0]].getNormalVector3fMap (); + + if (!std::isnan (normal (0)) && normal.norm () > 0.5f) + grid_[z_start + z] = normal.dot ( + point - input_->points[nn_indices[0]].getVector3fMap ()); + } + } + } + } +} + + + +#define PCL_INSTANTIATE_MarchingCubesHoppe(T) template class PCL_EXPORTS pcl::MarchingCubesHoppe; + +#endif // PCL_SURFACE_IMPL_MARCHING_CUBES_HOPPE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes_rbf.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes_rbf.hpp new file mode 100755 index 0000000..a33d7d8 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/marching_cubes_rbf.hpp @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_RBF_H_ +#define PCL_SURFACE_IMPL_MARCHING_CUBES_RBF_H_ + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MarchingCubesRBF::~MarchingCubesRBF () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MarchingCubesRBF::voxelizeData () +{ + // Initialize data structures + const unsigned int N = static_cast (input_->size ()); + Eigen::MatrixXd M (2*N, 2*N), + d (2*N, 1); + + for (unsigned int row_i = 0; row_i < 2*N; ++row_i) + { + // boolean variable to determine whether we are in the off_surface domain for the rows + bool row_off = (row_i >= N); + for (unsigned int col_i = 0; col_i < 2*N; ++col_i) + { + // boolean variable to determine whether we are in the off_surface domain for the columns + bool col_off = (col_i >= N); + M (row_i, col_i) = kernel (Eigen::Vector3f (input_->points[col_i%N].getVector3fMap ()).cast () + Eigen::Vector3f (input_->points[col_i%N].getNormalVector3fMap ()).cast () * col_off * off_surface_epsilon_, + Eigen::Vector3f (input_->points[row_i%N].getVector3fMap ()).cast () + Eigen::Vector3f (input_->points[row_i%N].getNormalVector3fMap ()).cast () * row_off * off_surface_epsilon_); + } + + d (row_i, 0) = row_off * off_surface_epsilon_; + } + + // Solve for the weights + Eigen::MatrixXd w (2*N, 1); + + // Solve_linear_system (M, d, w); + w = M.fullPivLu ().solve (d); + + std::vector weights (2*N); + std::vector > centers (2*N); + for (unsigned int i = 0; i < N; ++i) + { + centers[i] = Eigen::Vector3f (input_->points[i].getVector3fMap ()).cast (); + centers[i + N] = Eigen::Vector3f (input_->points[i].getVector3fMap ()).cast () + Eigen::Vector3f (input_->points[i].getNormalVector3fMap ()).cast () * off_surface_epsilon_; + weights[i] = w (i, 0); + weights[i + N] = w (i + N, 0); + } + + for (int x = 0; x < res_x_; ++x) + for (int y = 0; y < res_y_; ++y) + for (int z = 0; z < res_z_; ++z) + { + const Eigen::Vector3f point_f = (size_voxel_ * Eigen::Array3f (x, y, z) + + lower_boundary_).matrix (); + const Eigen::Vector3d point = point_f.cast (); + + double f = 0.0; + std::vector::const_iterator w_it (weights.begin()); + for (std::vector >::const_iterator c_it = centers.begin (); + c_it != centers.end (); ++c_it, ++w_it) + f += *w_it * kernel (*c_it, point); + + grid_[x * res_y_*res_z_ + y * res_z_ + z] = float (f); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template double +pcl::MarchingCubesRBF::kernel (Eigen::Vector3d c, Eigen::Vector3d x) +{ + double r = (x - c).norm (); + return (r * r * r); +} + +#define PCL_INSTANTIATE_MarchingCubesRBF(T) template class PCL_EXPORTS pcl::MarchingCubesRBF; + +#endif // PCL_SURFACE_IMPL_MARCHING_CUBES_HOPPE_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/mls.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/mls.hpp new file mode 100755 index 0000000..028d270 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/mls.hpp @@ -0,0 +1,900 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SURFACE_IMPL_MLS_H_ +#define PCL_SURFACE_IMPL_MLS_H_ + +#include +#include +#include +#include +#include +#include +#include + +#ifdef _OPENMP +#include +#endif + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MovingLeastSquares::process (PointCloudOut &output) +{ + // Reset or initialize the collection of indices + corresponding_input_indices_.reset (new PointIndices); + + // Check if normals have to be computed/saved + if (compute_normals_) + { + normals_.reset (new NormalCloud); + // Copy the header + normals_->header = input_->header; + // Clear the fields in case the method exits before computation + normals_->width = normals_->height = 0; + normals_->points.clear (); + } + + // Copy the header + output.header = input_->header; + output.width = output.height = 0; + output.points.clear (); + + if (search_radius_ <= 0 || sqr_gauss_param_ <= 0) + { + PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_); + return; + } + + // Check if distinct_cloud_ was set + if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_) + { + PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ()); + return; + } + + if (!initCompute ()) + return; + + // Initialize the spatial locator + if (!tree_) + { + KdTreePtr tree; + if (input_->isOrganized ()) + tree.reset (new pcl::search::OrganizedNeighbor ()); + else + tree.reset (new pcl::search::KdTree (false)); + setSearchMethod (tree); + } + + // Send the surface dataset to the spatial locator + tree_->setInputCloud (input_); + + switch (upsample_method_) + { + // Initialize random number generator if necessary + case (RANDOM_UNIFORM_DENSITY): + { + std::random_device rd; + rng_.seed (rd()); + const double tmp = search_radius_ / 2.0; + rng_uniform_distribution_.reset (new std::uniform_real_distribution<> (-tmp, tmp)); + + break; + } + case (VOXEL_GRID_DILATION): + case (DISTINCT_CLOUD): + { + if (!cache_mls_results_) + PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n"); + + cache_mls_results_ = true; + break; + } + default: + break; + } + + if (cache_mls_results_) + { + mls_results_.resize (input_->size ()); + } + else + { + mls_results_.resize (1); // Need to have a reference to a single dummy result. + } + + // Perform the actual surface reconstruction + performProcessing (output); + + if (compute_normals_) + { + normals_->height = 1; + normals_->width = static_cast (normals_->size ()); + + for (std::size_t i = 0; i < output.size (); ++i) + { + using FieldList = typename pcl::traits::fieldList::type; + pcl::for_each_type (SetIfFieldExists (output.points[i], "normal_x", normals_->points[i].normal_x)); + pcl::for_each_type (SetIfFieldExists (output.points[i], "normal_y", normals_->points[i].normal_y)); + pcl::for_each_type (SetIfFieldExists (output.points[i], "normal_z", normals_->points[i].normal_z)); + pcl::for_each_type (SetIfFieldExists (output.points[i], "curvature", normals_->points[i].curvature)); + } + + } + + // Set proper widths and heights for the clouds + output.height = 1; + output.width = static_cast (output.size ()); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MovingLeastSquares::computeMLSPointNormal (int index, + const std::vector &nn_indices, + PointCloudOut &projected_points, + NormalCloud &projected_points_normals, + PointIndices &corresponding_input_indices, + MLSResult &mls_result) const +{ + // Note: this method is const because it needs to be thread-safe + // (MovingLeastSquaresOMP calls it from multiple threads) + + mls_result.computeMLSSurface (*input_, index, nn_indices, search_radius_, order_); + + switch (upsample_method_) + { + case (NONE): + { + const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_); + addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices); + break; + } + + case (SAMPLE_LOCAL_PLANE): + { + // Uniformly sample a circle around the query point using the radius and step parameters + for (float u_disp = -static_cast (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast (upsampling_step_)) + for (float v_disp = -static_cast (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast (upsampling_step_)) + if (u_disp * u_disp + v_disp * v_disp < upsampling_radius_ * upsampling_radius_) + { + MLSResult::MLSProjectionResults proj = mls_result.projectPointSimpleToPolynomialSurface (u_disp, v_disp); + addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices); + } + break; + } + + case (RANDOM_UNIFORM_DENSITY): + { + // Compute the local point density and add more samples if necessary + const int num_points_to_add = static_cast (std::floor (desired_num_points_in_radius_ / 2.0 / static_cast (nn_indices.size ()))); + + // Just add the query point, because the density is good + if (num_points_to_add <= 0) + { + // Just add the current point + const MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint (projection_method_, nr_coeff_); + addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices); + } + else + { + // Sample the local plane + for (int num_added = 0; num_added < num_points_to_add;) + { + const double u = (*rng_uniform_distribution_) (rng_); + const double v = (*rng_uniform_distribution_) (rng_); + + // Check if inside circle; if not, try another coin flip + if (u * u + v * v > search_radius_ * search_radius_ / 4) + continue; + + MLSResult::MLSProjectionResults proj; + if (order_ > 1 && mls_result.num_neighbors >= 5 * nr_coeff_) + proj = mls_result.projectPointSimpleToPolynomialSurface (u, v); + else + proj = mls_result.projectPointToMLSPlane (u, v); + + addProjectedPointNormal (index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices); + + num_added++; + } + } + break; + } + + default: + break; + } +} + +template void +pcl::MovingLeastSquares::addProjectedPointNormal (int index, + const Eigen::Vector3d &point, + const Eigen::Vector3d &normal, + double curvature, + PointCloudOut &projected_points, + NormalCloud &projected_points_normals, + PointIndices &corresponding_input_indices) const +{ + PointOutT aux; + aux.x = static_cast (point[0]); + aux.y = static_cast (point[1]); + aux.z = static_cast (point[2]); + + // Copy additional point information if available + copyMissingFields (input_->points[index], aux); + + projected_points.push_back (aux); + corresponding_input_indices.indices.push_back (index); + + if (compute_normals_) + { + pcl::Normal aux_normal; + aux_normal.normal_x = static_cast (normal[0]); + aux_normal.normal_y = static_cast (normal[1]); + aux_normal.normal_z = static_cast (normal[2]); + aux_normal.curvature = curvature; + projected_points_normals.push_back (aux_normal); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MovingLeastSquares::performProcessing (PointCloudOut &output) +{ + // Compute the number of coefficients + nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; + +#ifdef _OPENMP + // (Maximum) number of threads + const unsigned int threads = threads_ == 0 ? 1 : threads_; + // Create temporaries for each thread in order to avoid synchronization + typename PointCloudOut::CloudVectorType projected_points (threads); + typename NormalCloud::CloudVectorType projected_points_normals (threads); + std::vector corresponding_input_indices (threads); +#endif + + // For all points +#ifdef _OPENMP +#pragma omp parallel for schedule (dynamic,1000) num_threads (threads) +#endif + for (int cp = 0; cp < static_cast (indices_->size ()); ++cp) + { + // Allocate enough space to hold the results of nearest neighbor searches + // \note resize is irrelevant for a radiusSearch (). + std::vector nn_indices; + std::vector nn_sqr_dists; + + // Get the initial estimates of point positions and their neighborhoods + if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) + { + // Check the number of nearest neighbors for normal estimation (and later for polynomial fit as well) + if (nn_indices.size () >= 3) + { + // This thread's ID (range 0 to threads-1) +#ifdef _OPENMP + const int tn = omp_get_thread_num (); + // Size of projected points before computeMLSPointNormal () adds points + std::size_t pp_size = projected_points[tn].size (); +#else + PointCloudOut projected_points; + NormalCloud projected_points_normals; +#endif + + // Get a plane approximating the local surface's tangent and project point onto it + const int index = (*indices_)[cp]; + + std::size_t mls_result_index = 0; + if (cache_mls_results_) + mls_result_index = index; // otherwise we give it a dummy location. + +#ifdef _OPENMP + computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]); + + // Copy all information from the input cloud to the output points (not doing any interpolation) + for (std::size_t pp = pp_size; pp < projected_points[tn].size (); ++pp) + copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]); +#else + computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]); + + // Append projected points to output + output.insert (output.end (), projected_points.begin (), projected_points.end ()); + if (compute_normals_) + normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); +#endif + } + } + } + +#ifdef _OPENMP + // Combine all threads' results into the output vectors + for (unsigned int tn = 0; tn < threads; ++tn) + { + output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ()); + corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (), + corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ()); + if (compute_normals_) + normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ()); + } +#endif + + // Perform the distinct-cloud or voxel-grid upsampling + performUpsampling (output); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MovingLeastSquares::performUpsampling (PointCloudOut &output) +{ + + if (upsample_method_ == DISTINCT_CLOUD) + { + corresponding_input_indices_.reset (new PointIndices); + for (std::size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i + { + // Distinct cloud may have nan points, skip them + if (!std::isfinite (distinct_cloud_->points[dp_i].x)) + continue; + + // Get 3D position of point + //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap (); + std::vector nn_indices; + std::vector nn_dists; + tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists); + int input_index = nn_indices.front (); + + // If the closest point did not have a valid MLS fitting result + // OR if it is too far away from the sampled point + if (mls_results_[input_index].valid == false) + continue; + + Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast (); + MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_); + addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_); + } + } + + // For the voxel grid upsampling method, generate the voxel grid and dilate it + // Then, project the newly obtained points to the MLS surface + if (upsample_method_ == VOXEL_GRID_DILATION) + { + corresponding_input_indices_.reset (new PointIndices); + + MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_); + for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration) + voxel_grid.dilate (); + + for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it) + { + // Get 3D position of point + Eigen::Vector3f pos; + voxel_grid.getPosition (m_it->first, pos); + + PointInT p; + p.x = pos[0]; + p.y = pos[1]; + p.z = pos[2]; + + std::vector nn_indices; + std::vector nn_dists; + tree_->nearestKSearch (p, 1, nn_indices, nn_dists); + int input_index = nn_indices.front (); + + // If the closest point did not have a valid MLS fitting result + // OR if it is too far away from the sampled point + if (mls_results_[input_index].valid == false) + continue; + + Eigen::Vector3d add_point = p.getVector3fMap ().template cast (); + MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_); + addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +pcl::MLSResult::MLSResult (const Eigen::Vector3d &a_query_point, + const Eigen::Vector3d &a_mean, + const Eigen::Vector3d &a_plane_normal, + const Eigen::Vector3d &a_u, + const Eigen::Vector3d &a_v, + const Eigen::VectorXd &a_c_vec, + const int a_num_neighbors, + const float a_curvature, + const int a_order) : + query_point (a_query_point), mean (a_mean), plane_normal (a_plane_normal), u_axis (a_u), v_axis (a_v), c_vec (a_c_vec), num_neighbors (a_num_neighbors), + curvature (a_curvature), order (a_order), valid (true) +{} + +void +pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const +{ + Eigen::Vector3d delta = pt - mean; + u = delta.dot (u_axis); + v = delta.dot (v_axis); + w = delta.dot (plane_normal); +} + +void +pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const +{ + Eigen::Vector3d delta = pt - mean; + u = delta.dot (u_axis); + v = delta.dot (v_axis); +} + +double +pcl::MLSResult::getPolynomialValue (const double u, const double v) const +{ + // Compute the polynomial's terms at the current point + // Example for second order: z = a + b*y + c*y^2 + d*x + e*x*y + f*x^2 + int j = 0; + double u_pow = 1; + double result = 0; + for (int ui = 0; ui <= order; ++ui) + { + double v_pow = 1; + for (int vi = 0; vi <= order - ui; ++vi) + { + result += c_vec[j++] * u_pow * v_pow; + v_pow *= v; + } + u_pow *= u; + } + + return (result); +} + +pcl::MLSResult::PolynomialPartialDerivative +pcl::MLSResult::getPolynomialPartialDerivative (const double u, const double v) const +{ + // Compute the displacement along the normal using the fitted polynomial + // and compute the partial derivatives needed for estimating the normal + PolynomialPartialDerivative d{}; + Eigen::VectorXd u_pow (order + 2), v_pow (order + 2); + int j = 0; + + d.z = d.z_u = d.z_v = d.z_uu = d.z_vv = d.z_uv = 0; + u_pow (0) = v_pow (0) = 1; + for (int ui = 0; ui <= order; ++ui) + { + for (int vi = 0; vi <= order - ui; ++vi) + { + // Compute displacement along normal + d.z += u_pow (ui) * v_pow (vi) * c_vec[j]; + + // Compute partial derivatives + if (ui >= 1) + d.z_u += c_vec[j] * ui * u_pow (ui - 1) * v_pow (vi); + + if (vi >= 1) + d.z_v += c_vec[j] * vi * u_pow (ui) * v_pow (vi - 1); + + if (ui >= 1 && vi >= 1) + d.z_uv += c_vec[j] * ui * u_pow (ui - 1) * vi * v_pow (vi - 1); + + if (ui >= 2) + d.z_uu += c_vec[j] * ui * (ui - 1) * u_pow (ui - 2) * v_pow (vi); + + if (vi >= 2) + d.z_vv += c_vec[j] * vi * (vi - 1) * u_pow (ui) * v_pow (vi - 2); + + if (ui == 0) + v_pow (vi + 1) = v_pow (vi) * v; + + ++j; + } + u_pow (ui + 1) = u_pow (ui) * u; + } + + return (d); +} + +Eigen::Vector2f +pcl::MLSResult::calculatePrincipleCurvatures (const double u, const double v) const +{ + Eigen::Vector2f k (1e-5, 1e-5); + + // Note: this use the Monge Patch to derive the Gaussian curvature and Mean Curvature found here http://mathworld.wolfram.com/MongePatch.html + // Then: + // k1 = H + sqrt(H^2 - K) + // k1 = H - sqrt(H^2 - K) + if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0])) + { + const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v); + const double Z = 1 + d.z_u * d.z_u + d.z_v * d.z_v; + const double Zlen = std::sqrt (Z); + const double K = (d.z_uu * d.z_vv - d.z_uv * d.z_uv) / (Z * Z); + const double H = ((1.0 + d.z_v * d.z_v) * d.z_uu - 2.0 * d.z_u * d.z_v * d.z_uv + (1.0 + d.z_u * d.z_u) * d.z_vv) / (2.0 * Zlen * Zlen * Zlen); + const double disc2 = H * H - K; + assert (disc2 >= 0.0); + const double disc = std::sqrt (disc2); + k[0] = H + disc; + k[1] = H - disc; + + if (std::abs (k[0]) > std::abs (k[1])) std::swap (k[0], k[1]); + } + else + { + PCL_ERROR ("No Polynomial fit data, unable to calculate the principle curvatures!\n"); + } + + return (k); +} + +pcl::MLSResult::MLSProjectionResults +pcl::MLSResult::projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const +{ + double gu = u; + double gv = v; + double gw = 0; + + MLSProjectionResults result; + result.normal = plane_normal; + if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0])) + { + PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv); + gw = d.z; + double err_total; + const double dist1 = std::abs (gw - w); + double dist2; + do + { + double e1 = (gu - u) + d.z_u * gw - d.z_u * w; + double e2 = (gv - v) + d.z_v * gw - d.z_v * w; + + const double F1u = 1 + d.z_uu * gw + d.z_u * d.z_u - d.z_uu * w; + const double F1v = d.z_uv * gw + d.z_u * d.z_v - d.z_uv * w; + + const double F2u = d.z_uv * gw + d.z_v * d.z_u - d.z_uv * w; + const double F2v = 1 + d.z_vv * gw + d.z_v * d.z_v - d.z_vv * w; + + Eigen::MatrixXd J (2, 2); + J (0, 0) = F1u; + J (0, 1) = F1v; + J (1, 0) = F2u; + J (1, 1) = F2v; + + Eigen::Vector2d err (e1, e2); + Eigen::Vector2d update = J.inverse () * err; + gu -= update (0); + gv -= update (1); + + d = getPolynomialPartialDerivative (gu, gv); + gw = d.z; + dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w)); + + err_total = std::sqrt (e1 * e1 + e2 * e2); + + } while (err_total > 1e-8 && dist2 < dist1); + + if (dist2 > dist1) // the optimization was diverging reset the coordinates for simple projection + { + gu = u; + gv = v; + d = getPolynomialPartialDerivative (u, v); + gw = d.z; + } + + result.u = gu; + result.v = gv; + result.normal -= (d.z_u * u_axis + d.z_v * v_axis); + result.normal.normalize (); + } + + result.point = mean + gu * u_axis + gv * v_axis + gw * plane_normal; + + return (result); +} + +pcl::MLSResult::MLSProjectionResults +pcl::MLSResult::projectPointToMLSPlane (const double u, const double v) const +{ + MLSProjectionResults result; + result.u = u; + result.v = v; + result.normal = plane_normal; + result.point = mean + u * u_axis + v * v_axis; + + return (result); +} + +pcl::MLSResult::MLSProjectionResults +pcl::MLSResult::projectPointSimpleToPolynomialSurface (const double u, const double v) const +{ + MLSProjectionResults result; + double w = 0; + + result.u = u; + result.v = v; + result.normal = plane_normal; + + if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0])) + { + const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v); + w = d.z; + result.normal -= (d.z_u * u_axis + d.z_v * v_axis); + result.normal.normalize (); + } + + result.point = mean + u * u_axis + v * v_axis + w * plane_normal; + + return (result); +} + +pcl::MLSResult::MLSProjectionResults +pcl::MLSResult::projectPoint (const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors) const +{ + double u, v, w; + getMLSCoordinates (pt, u, v, w); + + MLSResult::MLSProjectionResults proj; + if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE) + { + if (method == ORTHOGONAL) + proj = projectPointOrthogonalToPolynomialSurface (u, v, w); + else // SIMPLE + proj = projectPointSimpleToPolynomialSurface (u, v); + } + else + { + proj = projectPointToMLSPlane (u, v); + } + + return (proj); +} + +pcl::MLSResult::MLSProjectionResults +pcl::MLSResult::projectQueryPoint (ProjectionMethod method, int required_neighbors) const +{ + MLSResult::MLSProjectionResults proj; + if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE) + { + if (method == ORTHOGONAL) + { + double u, v, w; + getMLSCoordinates (query_point, u, v, w); + proj = projectPointOrthogonalToPolynomialSurface (u, v, w); + } + else // SIMPLE + { + // Projection onto MLS surface along Darboux normal to the height at (0,0) + proj.point = mean + (c_vec[0] * plane_normal); + + // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1] + proj.normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis; + proj.normal.normalize (); + } + } + else + { + proj.normal = plane_normal; + proj.point = mean; + } + + return (proj); +} + +template void +pcl::MLSResult::computeMLSSurface (const pcl::PointCloud &cloud, + int index, + const std::vector &nn_indices, + double search_radius, + int polynomial_order, + std::function weight_func) +{ + // Compute the plane coefficients + EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix; + Eigen::Vector4d xyz_centroid; + + // Estimate the XYZ centroid + pcl::compute3DCentroid (cloud, nn_indices, xyz_centroid); + + // Compute the 3x3 covariance matrix + pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix); + EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value; + EIGEN_ALIGN16 Eigen::Vector3d eigen_vector; + Eigen::Vector4d model_coefficients (0, 0, 0, 0); + pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + model_coefficients.head<3> ().matrix () = eigen_vector; + model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); + + query_point = cloud.points[index].getVector3fMap ().template cast (); + + if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2])) + { + // Invalid plane coefficients, this may happen if the input cloud is non-dense (it contains invalid points). + // Keep the input point and stop here. + valid = false; + mean = query_point; + return; + } + + // Projected query point + valid = true; + const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3]; + mean = query_point - distance * model_coefficients.head<3> (); + + curvature = covariance_matrix.trace (); + // Compute the curvature surface change + if (curvature != 0) + curvature = std::abs (eigen_value / curvature); + + // Get a copy of the plane normal easy access + plane_normal = model_coefficients.head<3> (); + + // Local coordinate system (Darboux frame) + v_axis = plane_normal.unitOrthogonal (); + u_axis = plane_normal.cross (v_axis); + + // Perform polynomial fit to update point and normal + //////////////////////////////////////////////////// + num_neighbors = static_cast (nn_indices.size ()); + order = polynomial_order; + if (order > 1) + { + const int nr_coeff = (order + 1) * (order + 2) / 2; + + if (num_neighbors >= nr_coeff) + { + if (!weight_func) + weight_func = [=] (const double sq_dist) { return this->computeMLSWeight (sq_dist, search_radius * search_radius); }; + + // Allocate matrices and vectors to hold the data used for the polynomial fit + Eigen::VectorXd weight_vec (num_neighbors); + Eigen::MatrixXd P (nr_coeff, num_neighbors); + Eigen::VectorXd f_vec (num_neighbors); + Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff); + + // Update neighborhood, since point was projected, and computing relative + // positions. Note updating only distances for the weights for speed + std::vector > de_meaned (num_neighbors); + for (std::size_t ni = 0; ni < static_cast(num_neighbors); ++ni) + { + de_meaned[ni][0] = cloud.points[nn_indices[ni]].x - mean[0]; + de_meaned[ni][1] = cloud.points[nn_indices[ni]].y - mean[1]; + de_meaned[ni][2] = cloud.points[nn_indices[ni]].z - mean[2]; + weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni])); + } + + // Go through neighbors, transform them in the local coordinate system, + // save height and the evaluation of the polynome's terms + for (std::size_t ni = 0; ni < static_cast(num_neighbors); ++ni) + { + // Transforming coordinates + const double u_coord = de_meaned[ni].dot(u_axis); + const double v_coord = de_meaned[ni].dot(v_axis); + f_vec (ni) = de_meaned[ni].dot (plane_normal); + + // Compute the polynomial's terms at the current point + int j = 0; + double u_pow = 1; + for (int ui = 0; ui <= order; ++ui) + { + double v_pow = 1; + for (int vi = 0; vi <= order - ui; ++vi) + { + P (j++, ni) = u_pow * v_pow; + v_pow *= v_coord; + } + u_pow *= u_coord; + } + } + + // Computing coefficients + const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal(); // size will be (nr_coeff_, nn_indices.size ()); + P_weight_Pt = P_weight * P.transpose (); + c_vec = P_weight * f_vec; + P_weight_Pt.llt ().solveInPlace (c_vec); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::MovingLeastSquares::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud, + IndicesPtr &indices, + float voxel_size) : + voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size) +{ + pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_); + + Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_; + const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ()); + // Put initial cloud in voxel grid + data_size_ = static_cast (1.5 * max_size / voxel_size_); + for (std::size_t i = 0; i < indices->size (); ++i) + if (std::isfinite (cloud->points[(*indices)[i]].x)) + { + Eigen::Vector3i pos; + getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos); + + std::uint64_t index_1d; + getIndexIn1D (pos, index_1d); + Leaf leaf; + voxel_grid_[index_1d] = leaf; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MovingLeastSquares::MLSVoxelGrid::dilate () +{ + HashMap new_voxel_grid = voxel_grid_; + for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it) + { + Eigen::Vector3i index; + getIndexIn3D (m_it->first, index); + + // Now dilate all of its voxels + for (int x = -1; x <= 1; ++x) + for (int y = -1; y <= 1; ++y) + for (int z = -1; z <= 1; ++z) + if (x != 0 || y != 0 || z != 0) + { + Eigen::Vector3i new_index; + new_index = index + Eigen::Vector3i (x, y, z); + + std::uint64_t index_1d; + getIndexIn1D (new_index, index_1d); + Leaf leaf; + new_voxel_grid[index_1d] = leaf; + } + } + voxel_grid_ = new_voxel_grid; +} + + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MovingLeastSquares::copyMissingFields (const PointInT &point_in, + PointOutT &point_out) const +{ + PointOutT temp = point_out; + copyPoint (point_in, point_out); + point_out.x = temp.x; + point_out.y = temp.y; + point_out.z = temp.z; +} + +#define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares; +#define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP; + +#endif // PCL_SURFACE_IMPL_MLS_H_ diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/organized_fast_mesh.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/organized_fast_mesh.hpp new file mode 100755 index 0000000..2d35086 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/organized_fast_mesh.hpp @@ -0,0 +1,283 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Dirk Holz (University of Bonn) + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_ +#define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_ + +#include + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedFastMesh::performReconstruction (pcl::PolygonMesh &output) +{ + reconstructPolygons (output.polygons); + + // Get the field names + int x_idx = pcl::getFieldIndex (output.cloud, "x"); + int y_idx = pcl::getFieldIndex (output.cloud, "y"); + int z_idx = pcl::getFieldIndex (output.cloud, "z"); + if (x_idx == -1 || y_idx == -1 || z_idx == -1) + return; + // correct all measurements, + // (running over complete image since some rows and columns are left out + // depending on triangle_pixel_size) + // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files + for (std::size_t i = 0; i < input_->points.size (); ++i) + if (!isFinite (input_->points[i])) + resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedFastMesh::performReconstruction (std::vector &polygons) +{ + reconstructPolygons (polygons); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedFastMesh::reconstructPolygons (std::vector &polygons) +{ + if (triangulation_type_ == TRIANGLE_RIGHT_CUT) + makeRightCutMesh (polygons); + else if (triangulation_type_ == TRIANGLE_LEFT_CUT) + makeLeftCutMesh (polygons); + else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT) + makeAdaptiveCutMesh (polygons); + else if (triangulation_type_ == QUAD_MESH) + makeQuadMesh (polygons); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedFastMesh::makeQuadMesh (std::vector& polygons) +{ + int last_column = input_->width - triangle_pixel_size_columns_; + int last_row = input_->height - triangle_pixel_size_rows_; + + int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; + int y_big_incr = triangle_pixel_size_rows_ * input_->width, + x_big_incr = y_big_incr + triangle_pixel_size_columns_; + // Reserve enough space + polygons.resize (input_->width * input_->height); + + // Go over the rows first + for (int y = 0; y < last_row; y += triangle_pixel_size_rows_) + { + // Initialize a new row + i = y * input_->width; + index_right = i + triangle_pixel_size_columns_; + index_down = i + y_big_incr; + index_down_right = i + x_big_incr; + + // Go over the columns + for (int x = 0; x < last_column; x += triangle_pixel_size_columns_, + i += triangle_pixel_size_columns_, + index_right += triangle_pixel_size_columns_, + index_down += triangle_pixel_size_columns_, + index_down_right += triangle_pixel_size_columns_) + { + if (isValidQuad (i, index_right, index_down_right, index_down)) + if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down)) + addQuad (i, index_right, index_down_right, index_down, idx++, polygons); + } + } + polygons.resize (idx); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedFastMesh::makeRightCutMesh (std::vector& polygons) +{ + int last_column = input_->width - triangle_pixel_size_columns_; + int last_row = input_->height - triangle_pixel_size_rows_; + + int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; + int y_big_incr = triangle_pixel_size_rows_ * input_->width, + x_big_incr = y_big_incr + triangle_pixel_size_columns_; + // Reserve enough space + polygons.resize (input_->width * input_->height * 2); + + // Go over the rows first + for (int y = 0; y < last_row; y += triangle_pixel_size_rows_) + { + // Initialize a new row + i = y * input_->width; + index_right = i + triangle_pixel_size_columns_; + index_down = i + y_big_incr; + index_down_right = i + x_big_incr; + + // Go over the columns + for (int x = 0; x < last_column; x += triangle_pixel_size_columns_, + i += triangle_pixel_size_columns_, + index_right += triangle_pixel_size_columns_, + index_down += triangle_pixel_size_columns_, + index_down_right += triangle_pixel_size_columns_) + { + if (isValidTriangle (i, index_down_right, index_right)) + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right)) + addTriangle (i, index_down_right, index_right, idx++, polygons); + + if (isValidTriangle (i, index_down, index_down_right)) + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right)) + addTriangle (i, index_down, index_down_right, idx++, polygons); + } + } + polygons.resize (idx); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedFastMesh::makeLeftCutMesh (std::vector& polygons) +{ + int last_column = input_->width - triangle_pixel_size_columns_; + int last_row = input_->height - triangle_pixel_size_rows_; + + int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; + int y_big_incr = triangle_pixel_size_rows_ * input_->width, + x_big_incr = y_big_incr + triangle_pixel_size_columns_; + // Reserve enough space + polygons.resize (input_->width * input_->height * 2); + + // Go over the rows first + for (int y = 0; y < last_row; y += triangle_pixel_size_rows_) + { + // Initialize a new row + i = y * input_->width; + index_right = i + triangle_pixel_size_columns_; + index_down = i + y_big_incr; + index_down_right = i + x_big_incr; + + // Go over the columns + for (int x = 0; x < last_column; x += triangle_pixel_size_columns_, + i += triangle_pixel_size_columns_, + index_right += triangle_pixel_size_columns_, + index_down += triangle_pixel_size_columns_, + index_down_right += triangle_pixel_size_columns_) + { + if (isValidTriangle (i, index_down, index_right)) + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right)) + addTriangle (i, index_down, index_right, idx++, polygons); + + if (isValidTriangle (index_right, index_down, index_down_right)) + if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right)) + addTriangle (index_right, index_down, index_down_right, idx++, polygons); + } + } + polygons.resize (idx); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedFastMesh::makeAdaptiveCutMesh (std::vector& polygons) +{ + int last_column = input_->width - triangle_pixel_size_columns_; + int last_row = input_->height - triangle_pixel_size_rows_; + + int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0; + int y_big_incr = triangle_pixel_size_rows_ * input_->width, + x_big_incr = y_big_incr + triangle_pixel_size_columns_; + // Reserve enough space + polygons.resize (input_->width * input_->height * 2); + + // Go over the rows first + for (int y = 0; y < last_row; y += triangle_pixel_size_rows_) + { + // Initialize a new row + i = y * input_->width; + index_right = i + triangle_pixel_size_columns_; + index_down = i + y_big_incr; + index_down_right = i + x_big_incr; + + // Go over the columns + for (int x = 0; x < last_column; x += triangle_pixel_size_columns_, + i += triangle_pixel_size_columns_, + index_right += triangle_pixel_size_columns_, + index_down += triangle_pixel_size_columns_, + index_down_right += triangle_pixel_size_columns_) + { + const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right); + const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right); + const bool left_cut_upper = isValidTriangle (i, index_down, index_right); + const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right); + + if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower) + { + float dist_right_cut = std::abs (input_->points[index_down].z - input_->points[index_right].z); + float dist_left_cut = std::abs (input_->points[i].z - input_->points[index_down_right].z); + if (dist_right_cut >= dist_left_cut) + { + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right)) + addTriangle (i, index_down_right, index_right, idx++, polygons); + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right)) + addTriangle (i, index_down, index_down_right, idx++, polygons); + } + else + { + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right)) + addTriangle (i, index_down, index_right, idx++, polygons); + if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right)) + addTriangle (index_right, index_down, index_down_right, idx++, polygons); + } + } + else + { + if (right_cut_upper) + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right)) + addTriangle (i, index_down_right, index_right, idx++, polygons); + if (right_cut_lower) + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right)) + addTriangle (i, index_down, index_down_right, idx++, polygons); + if (left_cut_upper) + if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right)) + addTriangle (i, index_down, index_right, idx++, polygons); + if (left_cut_lower) + if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right)) + addTriangle (index_right, index_down, index_down_right, idx++, polygons); + } + } + } + polygons.resize (idx); +} + +#define PCL_INSTANTIATE_OrganizedFastMesh(T) \ + template class PCL_EXPORTS pcl::OrganizedFastMesh; + +#endif // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_ diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/poisson.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/poisson.hpp new file mode 100755 index 0000000..eb52095 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/poisson.hpp @@ -0,0 +1,315 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SURFACE_IMPL_POISSON_H_ +#define PCL_SURFACE_IMPL_POISSON_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12 + +#include +#include + +using namespace pcl; + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::Poisson::Poisson () + : depth_ (8) + , min_depth_ (5) + , point_weight_ (4) + , scale_ (1.1f) + , solver_divide_ (8) + , iso_divide_ (8) + , samples_per_node_ (1.0) + , confidence_ (false) + , output_polygons_ (false) + , no_reset_samples_ (false) + , no_clip_tree_ (false) + , manifold_ (true) + , refine_ (3) + , kernel_depth_ (8) + , degree_ (2) + , non_adaptive_weights_ (false) + , show_residual_ (false) + , min_iterations_ (8) + , solver_accuracy_ (1e-3f) +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::Poisson::~Poisson () +{ +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template template void +pcl::Poisson::execute (poisson::CoredVectorMeshData &mesh, + poisson::Point3D ¢er, + float &scale) +{ + pcl::poisson::Real iso_value = 0; + poisson::TreeNodeData::UseIndex = 1; + poisson::Octree tree; + + /// TODO OPENMP stuff + // tree.threads = Threads.value; + center.coords[0] = center.coords[1] = center.coords[2] = 0; + + + if (solver_divide_ < min_depth_) + { + PCL_WARN ("[pcl::Poisson] solver_divide_ must be at least as large as min_depth_: %d >= %d\n", solver_divide_, min_depth_); + solver_divide_ = min_depth_; + } + if (iso_divide_< min_depth_) + { + PCL_WARN ("[pcl::Poisson] iso_divide_ must be at least as large as min_depth_: %d >= %d\n", iso_divide_, min_depth_); + iso_divide_ = min_depth_; + } + + pcl::poisson::TreeOctNode::SetAllocator (MEMORY_ALLOCATOR_BLOCK_SIZE); + + kernel_depth_ = depth_ - 2; + + tree.setBSplineData (depth_, pcl::poisson::Real (1.0 / (1 << depth_)), true); + + tree.maxMemoryUsage = 0; + + + int point_count = tree.template setTree (input_, depth_, min_depth_, kernel_depth_, samples_per_node_, + scale_, center, scale, confidence_, point_weight_, !non_adaptive_weights_); + + tree.ClipTree (); + tree.finalize (); + tree.RefineBoundary (iso_divide_); + + PCL_DEBUG ("Input Points: %d\n" , point_count ); + PCL_DEBUG ("Leaves/Nodes: %d/%d\n" , tree.tree.leaves() , tree.tree.nodes() ); + + tree.maxMemoryUsage = 0; + tree.SetLaplacianConstraints (); + + tree.maxMemoryUsage = 0; + tree.LaplacianMatrixIteration (solver_divide_, show_residual_, min_iterations_, solver_accuracy_); + + iso_value = tree.GetIsoValue (); + + tree.GetMCIsoTriangles (iso_value, iso_divide_, &mesh, 0, 1, manifold_, output_polygons_); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::Poisson::performReconstruction (PolygonMesh &output) +{ + poisson::CoredVectorMeshData mesh; + poisson::Point3D center; + float scale = 1.0f; + + switch (degree_) + { + case 1: + { + execute<1> (mesh, center, scale); + break; + } + case 2: + { + execute<2> (mesh, center, scale); + break; + } + case 3: + { + execute<3> (mesh, center, scale); + break; + } + case 4: + { + execute<4> (mesh, center, scale); + break; + } + case 5: + { + execute<5> (mesh, center, scale); + break; + } + default: + { + PCL_ERROR (stderr, "Degree %d not supported\n", degree_); + } + } + + // Write output PolygonMesh + pcl::PointCloud cloud; + cloud.points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ())); + poisson::Point3D p; + for (int i = 0; i < int (mesh.inCorePoints.size ()); i++) + { + p = mesh.inCorePoints[i]; + cloud.points[i].x = p.coords[0]*scale+center.coords[0]; + cloud.points[i].y = p.coords[1]*scale+center.coords[1]; + cloud.points[i].z = p.coords[2]*scale+center.coords[2]; + } + for (int i = int (mesh.inCorePoints.size ()); i < int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()); i++) + { + mesh.nextOutOfCorePoint (p); + cloud.points[i].x = p.coords[0]*scale+center.coords[0]; + cloud.points[i].y = p.coords[1]*scale+center.coords[1]; + cloud.points[i].z = p.coords[2]*scale+center.coords[2]; + } + pcl::toPCLPointCloud2 (cloud, output.cloud); + output.polygons.resize (mesh.polygonCount ()); + + // Write faces + std::vector polygon; + for (int p_i = 0; p_i < mesh.polygonCount (); p_i++) + { + pcl::Vertices v; + mesh.nextPolygon (polygon); + v.vertices.resize (polygon.size ()); + + for (int i = 0; i < static_cast (polygon.size ()); ++i) + if (polygon[i].inCore ) + v.vertices[i] = polygon[i].idx; + else + v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ()); + + output.polygons[p_i] = v; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::Poisson::performReconstruction (pcl::PointCloud &points, + std::vector &polygons) +{ + poisson::CoredVectorMeshData mesh; + poisson::Point3D center; + float scale = 1.0f; + + switch (degree_) + { + case 1: + { + execute<1> (mesh, center, scale); + break; + } + case 2: + { + execute<2> (mesh, center, scale); + break; + } + case 3: + { + execute<3> (mesh, center, scale); + break; + } + case 4: + { + execute<4> (mesh, center, scale); + break; + } + case 5: + { + execute<5> (mesh, center, scale); + break; + } + default: + { + PCL_ERROR (stderr, "Degree %d not supported\n", degree_); + } + } + + // Write output PolygonMesh + // Write vertices + points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ())); + poisson::Point3D p; + for (int i = 0; i < int(mesh.inCorePoints.size ()); i++) + { + p = mesh.inCorePoints[i]; + points.points[i].x = p.coords[0]*scale+center.coords[0]; + points.points[i].y = p.coords[1]*scale+center.coords[1]; + points.points[i].z = p.coords[2]*scale+center.coords[2]; + } + for (int i = int(mesh.inCorePoints.size()); i < int (mesh.outOfCorePointCount() + mesh.inCorePoints.size ()); i++) + { + mesh.nextOutOfCorePoint (p); + points.points[i].x = p.coords[0]*scale+center.coords[0]; + points.points[i].y = p.coords[1]*scale+center.coords[1]; + points.points[i].z = p.coords[2]*scale+center.coords[2]; + } + + polygons.resize (mesh.polygonCount ()); + + // Write faces + std::vector polygon; + for (int p_i = 0; p_i < mesh.polygonCount (); p_i++) + { + pcl::Vertices v; + mesh.nextPolygon (polygon); + v.vertices.resize (polygon.size ()); + + for (int i = 0; i < static_cast (polygon.size ()); ++i) + if (polygon[i].inCore ) + v.vertices[i] = polygon[i].idx; + else + v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ()); + + polygons[p_i] = v; + } +} + + +#define PCL_INSTANTIATE_Poisson(T) template class PCL_EXPORTS pcl::Poisson; + +#endif // PCL_SURFACE_IMPL_POISSON_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/processing.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/processing.hpp new file mode 100755 index 0000000..1cdaa82 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/processing.hpp @@ -0,0 +1,59 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::CloudSurfaceProcessing::process (pcl::PointCloud &output) +{ + // Copy the header + output.header = input_->header; + + if (!initCompute ()) + { + output.width = output.height = 0; + output.points.clear (); + return; + } + + // Perform the actual surface reconstruction + performProcessing (output); + + deinitCompute (); +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/reconstruction.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/reconstruction.hpp new file mode 100755 index 0000000..6c35756 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/reconstruction.hpp @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SURFACE_RECONSTRUCTION_IMPL_H_ +#define PCL_SURFACE_RECONSTRUCTION_IMPL_H_ +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceReconstruction::reconstruct (pcl::PolygonMesh &output) +{ + // Copy the header + output.header = input_->header; + + if (!initCompute ()) + { + output.cloud.width = output.cloud.height = 0; + output.cloud.data.clear (); + output.polygons.clear (); + return; + } + + // Check if a space search locator was given + if (check_tree_) + { + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the surface dataset to the spatial locator + tree_->setInputCloud (input_, indices_); + } + + // Set up the output dataset + pcl::toPCLPointCloud2 (*input_, output.cloud); /// NOTE: passing in boost shared pointer with * as const& should be OK here + output.polygons.clear (); + output.polygons.reserve (2*indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices + // Perform the actual surface reconstruction + performReconstruction (output); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfaceReconstruction::reconstruct (pcl::PointCloud &points, + std::vector &polygons) +{ + // Copy the header + points.header = input_->header; + + if (!initCompute ()) + { + points.width = points.height = 0; + points.clear (); + polygons.clear (); + return; + } + + // Check if a space search locator was given + if (check_tree_) + { + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the surface dataset to the spatial locator + tree_->setInputCloud (input_, indices_); + } + + // Set up the output dataset + polygons.clear (); + polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices + // Perform the actual surface reconstruction + performReconstruction (points, polygons); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MeshConstruction::reconstruct (pcl::PolygonMesh &output) +{ + // Copy the header + output.header = input_->header; + + if (!initCompute ()) + { + output.cloud.width = output.cloud.height = 1; + output.cloud.data.clear (); + output.polygons.clear (); + return; + } + + // Check if a space search locator was given + if (check_tree_) + { + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the surface dataset to the spatial locator + tree_->setInputCloud (input_, indices_); + } + + // Set up the output dataset + pcl::toPCLPointCloud2 (*input_, output.cloud); /// NOTE: passing in boost shared pointer with * as const& should be OK here + // output.polygons.clear (); + // output.polygons.reserve (2*indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices + // Perform the actual surface reconstruction + performReconstruction (output); + + deinitCompute (); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MeshConstruction::reconstruct (std::vector &polygons) +{ + if (!initCompute ()) + { + polygons.clear (); + return; + } + + // Check if a space search locator was given + if (check_tree_) + { + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the surface dataset to the spatial locator + tree_->setInputCloud (input_, indices_); + } + + // Set up the output dataset + //polygons.clear (); + //polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices + // Perform the actual surface reconstruction + performReconstruction (polygons); + + deinitCompute (); +} + + +#endif // PCL_SURFACE_RECONSTRUCTION_IMPL_H_ + diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/surfel_smoothing.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/surfel_smoothing.hpp new file mode 100755 index 0000000..263b76e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/surfel_smoothing.hpp @@ -0,0 +1,317 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#ifndef PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ +#define PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SurfelSmoothing::initCompute () +{ + if (!PCLBase::initCompute ()) + return false; + + if (!normals_) + { + PCL_ERROR ("SurfelSmoothing: normal cloud not set\n"); + return false; + } + + if (input_->points.size () != normals_->points.size ()) + { + PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n"); + return false; + } + + // Initialize the spatial locator + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // create internal copies of the input - these will be modified + interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_)); + interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_)); + + return (true); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::SurfelSmoothing::smoothCloudIteration (PointCloudInPtr &output_positions, + NormalCloudPtr &output_normals) +{ +// PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n"); + + output_positions = PointCloudInPtr (new PointCloudIn); + output_positions->points.resize (interm_cloud_->points.size ()); + output_normals = NormalCloudPtr (new NormalCloud); + output_normals->points.resize (interm_cloud_->points.size ()); + + std::vector nn_indices; + std::vector nn_distances; + + std::vector diffs (interm_cloud_->points.size ()); + float total_residual = 0.0f; + + for (std::size_t i = 0; i < interm_cloud_->points.size (); ++i) + { + Eigen::Vector4f smoothed_point = Eigen::Vector4f::Zero (); + Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero (); + + // get neighbors + // @todo using 5x the scale for searching instead of all the points to avoid O(N^2) + tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances); + + float theta_normalization_factor = 0.0; + std::vector theta (nn_indices.size ()); + for (std::size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) + { + float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], input_->points[nn_indices[nn_index_i]]);//interm_cloud_->points[nn_indices[nn_index_i]]); + float theta_i = std::exp ( (-1) * dist / scale_squared_); + theta_normalization_factor += theta_i; + + smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap (); + + theta[nn_index_i] = theta_i; + } + + smoothed_normal /= theta_normalization_factor; + smoothed_normal(3) = 0.0f; + smoothed_normal.normalize (); + + + // find minimum along the normal + float e_residual; + smoothed_point = interm_cloud_->points[i].getVector4fMap (); + while (true) + { + e_residual = 0.0f; + smoothed_point(3) = 0.0f; + for (std::size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) + { + Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();//interm_cloud_->points[nn_indices[nn_index_i]].getVector4fMap (); + neighbor(3) = 0.0f; + float dot_product = smoothed_normal.dot (neighbor - smoothed_point); + e_residual += theta[nn_index_i] * dot_product;// * dot_product; + } + e_residual /= theta_normalization_factor; + if (e_residual < 1e-5) break; + + smoothed_point += e_residual * smoothed_normal; + } + + total_residual += e_residual; + + output_positions->points[i].getVector4fMap () = smoothed_point; + output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();//smoothed_normal; + } + +// std::cerr << "Total residual after iteration: " << total_residual << std::endl; +// PCL_INFO("SurfelSmoothing done iteration\n"); + return total_residual; +} + + +template void +pcl::SurfelSmoothing::smoothPoint (std::size_t &point_index, + PointT &output_point, + PointNT &output_normal) +{ + Eigen::Vector4f average_normal = Eigen::Vector4f::Zero (); + Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap (); + result_point(3) = 0.0f; + + // @todo parameter + float error_residual_threshold_ = 1e-3f; + float error_residual = error_residual_threshold_ + 1; + float last_error_residual = error_residual + 100.0f; + + std::vector nn_indices; + std::vector nn_distances; + + int big_iterations = 0; + int max_big_iterations = 500; + + while (std::fabs (error_residual) < std::fabs (last_error_residual) -error_residual_threshold_ && + big_iterations < max_big_iterations) + { + average_normal = Eigen::Vector4f::Zero (); + big_iterations ++; + PointT aux_point; aux_point.x = result_point(0); aux_point.y = result_point(1); aux_point.z = result_point(2); + tree_->radiusSearch (aux_point, 5*scale_, nn_indices, nn_distances); + + float theta_normalization_factor = 0.0; + std::vector theta (nn_indices.size ()); + for (std::size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) + { + float dist = nn_distances[nn_index_i]; + float theta_i = std::exp ( (-1) * dist / scale_squared_); + theta_normalization_factor += theta_i; + + average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap (); + theta[nn_index_i] = theta_i; + } + average_normal /= theta_normalization_factor; + average_normal(3) = 0.0f; + average_normal.normalize (); + + // find minimum along the normal + float e_residual_along_normal = 2, last_e_residual_along_normal = 3; + int small_iterations = 0; + int max_small_iterations = 10; + while ( std::fabs (e_residual_along_normal) < std::fabs (last_e_residual_along_normal) && + small_iterations < max_small_iterations) + { + small_iterations ++; + + e_residual_along_normal = 0.0f; + for (std::size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) + { + Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap (); + neighbor(3) = 0.0f; + float dot_product = average_normal.dot (neighbor - result_point); + e_residual_along_normal += theta[nn_index_i] * dot_product; + } + e_residual_along_normal /= theta_normalization_factor; + if (e_residual_along_normal < 1e-3) break; + + result_point += e_residual_along_normal * average_normal; + } + +// if (small_iterations == max_small_iterations) +// PCL_INFO ("passed the number of small iterations %d\n", small_iterations); + + last_error_residual = error_residual; + error_residual = e_residual_along_normal; + +// PCL_INFO ("last %f current %f\n", last_error_residual, error_residual); + } + + output_point.x = result_point(0); + output_point.y = result_point(1); + output_point.z = result_point(2); + output_normal = normals_->points[point_index]; + + if (big_iterations == max_big_iterations) + PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfelSmoothing::computeSmoothedCloud (PointCloudInPtr &output_positions, + NormalCloudPtr &output_normals) +{ + if (!initCompute ()) + { + PCL_ERROR ("[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n"); + return; + } + + tree_->setInputCloud (input_); + + output_positions->header = input_->header; + output_positions->height = input_->height; + output_positions->width = input_->width; + + output_normals->header = input_->header; + output_normals->height = input_->height; + output_normals->width = input_->width; + + output_positions->points.resize (input_->points.size ()); + output_normals->points.resize (input_->points.size ()); + for (std::size_t i = 0; i < input_->points.size (); ++i) + { + smoothPoint (i, output_positions->points[i], output_normals->points[i]); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales (PointCloudInPtr &cloud2, + NormalCloudPtr &cloud2_normals, + pcl::IndicesPtr &output_features) +{ + if (interm_cloud_->points.size () != cloud2->points.size () || + cloud2->points.size () != cloud2_normals->points.size ()) + { + PCL_ERROR ("[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n"); + return; + } + + std::vector diffs (cloud2->points.size ()); + for (std::size_t i = 0; i < cloud2->points.size (); ++i) + diffs[i] = cloud2_normals->points[i].getNormalVector4fMap ().dot (cloud2->points[i].getVector4fMap () - + interm_cloud_->points[i].getVector4fMap ()); + + std::vector nn_indices; + std::vector nn_distances; + + output_features->resize (cloud2->points.size ()); + for (int point_i = 0; point_i < static_cast (cloud2->points.size ()); ++point_i) + { + // Get neighbors + tree_->radiusSearch (point_i, scale_, nn_indices, nn_distances); + + bool largest = true; + bool smallest = true; + for (const int &nn_index : nn_indices) + { + if (diffs[point_i] < diffs[nn_index]) + largest = false; + else + smallest = false; + } + + if (largest || smallest) + (*output_features)[point_i] = point_i; + } +} + + + +#define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing; + +#endif /* PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ */ diff --git a/deps_install/include/pcl-1.10/pcl/surface/impl/texture_mapping.hpp b/deps_install/include/pcl-1.10/pcl/surface/impl/texture_mapping.hpp new file mode 100755 index 0000000..a6d6071 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/impl/texture_mapping.hpp @@ -0,0 +1,1102 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ +#define PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////////// +template std::vector > +pcl::TextureMapping::mapTexture2Face ( + const Eigen::Vector3f &p1, + const Eigen::Vector3f &p2, + const Eigen::Vector3f &p3) +{ + std::vector > tex_coordinates; + // process for each face + Eigen::Vector3f p1p2 (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]); + Eigen::Vector3f p1p3 (p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]); + Eigen::Vector3f p2p3 (p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]); + + // Normalize + p1p2 /= std::sqrt (p1p2.dot (p1p2)); + p1p3 /= std::sqrt (p1p3.dot (p1p3)); + p2p3 /= std::sqrt (p2p3.dot (p2p3)); + + // compute vector normal of a face + Eigen::Vector3f f_normal = p1p2.cross (p1p3); + f_normal /= std::sqrt (f_normal.dot (f_normal)); + + // project vector field onto the face: vector v1_projected = v1 - Dot(v1, n) * n; + Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot (f_normal) * f_normal; + + // Normalize + f_vector_field /= std::sqrt (f_vector_field.dot (f_vector_field)); + + // texture coordinates + Eigen::Vector2f tp1, tp2, tp3; + + double alpha = std::acos (f_vector_field.dot (p1p2)); + + // distance between 3 vertices of triangles + double e1 = (p2 - p3).norm () / f_; + double e2 = (p1 - p3).norm () / f_; + double e3 = (p1 - p2).norm () / f_; + + // initialize + tp1[0] = 0.0; + tp1[1] = 0.0; + + tp2[0] = static_cast (e3); + tp2[1] = 0.0; + + // determine texture coordinate tp3; + double cos_p1 = (e2 * e2 + e3 * e3 - e1 * e1) / (2 * e2 * e3); + double sin_p1 = sqrt (1 - (cos_p1 * cos_p1)); + + tp3[0] = static_cast (cos_p1 * e2); + tp3[1] = static_cast (sin_p1 * e2); + + // rotating by alpha (angle between V and pp1 & pp2) + Eigen::Vector2f r_tp2, r_tp3; + r_tp2[0] = static_cast (tp2[0] * std::cos (alpha) - tp2[1] * std::sin (alpha)); + r_tp2[1] = static_cast (tp2[0] * std::sin (alpha) + tp2[1] * std::cos (alpha)); + + r_tp3[0] = static_cast (tp3[0] * std::cos (alpha) - tp3[1] * std::sin (alpha)); + r_tp3[1] = static_cast (tp3[0] * std::sin (alpha) + tp3[1] * std::cos (alpha)); + + // shifting + tp1[0] = tp1[0]; + tp2[0] = r_tp2[0]; + tp3[0] = r_tp3[0]; + tp1[1] = tp1[1]; + tp2[1] = r_tp2[1]; + tp3[1] = r_tp3[1]; + + float min_x = tp1[0]; + float min_y = tp1[1]; + if (min_x > tp2[0]) + min_x = tp2[0]; + if (min_x > tp3[0]) + min_x = tp3[0]; + if (min_y > tp2[1]) + min_y = tp2[1]; + if (min_y > tp3[1]) + min_y = tp3[1]; + + if (min_x < 0) + { + tp1[0] -= min_x; + tp2[0] -= min_x; + tp3[0] -= min_x; + } + if (min_y < 0) + { + tp1[1] -= min_y; + tp2[1] -= min_y; + tp3[1] -= min_y; + } + + tex_coordinates.push_back (tp1); + tex_coordinates.push_back (tp2); + tex_coordinates.push_back (tp3); + return (tex_coordinates); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::mapTexture2Mesh (pcl::TextureMesh &tex_mesh) +{ + // mesh information + int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; + int point_size = static_cast (tex_mesh.cloud.data.size ()) / nr_points; + + // temporary PointXYZ + float x, y, z; + // temporary face + Eigen::Vector3f facet[3]; + + // texture coordinates for each mesh + std::vector > >texture_map; + + for (std::size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m) + { + // texture coordinates for each mesh + std::vector > texture_map_tmp; + + // processing for each face + for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i) + { + std::size_t idx; + + // get facet information + for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) + { + idx = tex_mesh.tex_polygons[m][i].vertices[j]; + memcpy (&x, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float)); + memcpy (&y, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float)); + memcpy (&z, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float)); + facet[j][0] = x; + facet[j][1] = y; + facet[j][2] = z; + } + + // get texture coordinates of each face + std::vector > tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]); + for (const auto &tex_coordinate : tex_coordinates) + texture_map_tmp.push_back (tex_coordinate); + }// end faces + + // texture materials + std::stringstream tex_name; + tex_name << "material_" << m; + tex_name >> tex_material_.tex_name; + tex_material_.tex_file = tex_files_[m]; + tex_mesh.tex_materials.push_back (tex_material_); + + // texture coordinates + tex_mesh.tex_coordinates.push_back (texture_map_tmp); + }// end meshes +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::mapTexture2MeshUV (pcl::TextureMesh &tex_mesh) +{ + // mesh information + int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; + int point_size = static_cast (tex_mesh.cloud.data.size ()) / nr_points; + + float x_lowest = 100000; + float x_highest = 0; + float y_lowest = 100000; + //float y_highest = 0 ; + float z_lowest = 100000; + float z_highest = 0; + float x_, y_, z_; + + for (int i = 0; i < nr_points; ++i) + { + memcpy (&x_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float)); + memcpy (&y_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float)); + memcpy (&z_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float)); + // x + if (x_ <= x_lowest) + x_lowest = x_; + if (x_ > x_lowest) + x_highest = x_; + + // y + if (y_ <= y_lowest) + y_lowest = y_; + //if (y_ > y_lowest) y_highest = y_; + + // z + if (z_ <= z_lowest) + z_lowest = z_; + if (z_ > z_lowest) + z_highest = z_; + } + // x + float x_range = (x_lowest - x_highest) * -1; + float x_offset = 0 - x_lowest; + // x + // float y_range = (y_lowest - y_highest)*-1; + // float y_offset = 0 - y_lowest; + // z + float z_range = (z_lowest - z_highest) * -1; + float z_offset = 0 - z_lowest; + + // texture coordinates for each mesh + std::vector > >texture_map; + + for (std::size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m) + { + // texture coordinates for each mesh + std::vector > texture_map_tmp; + + // processing for each face + for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i) + { + Eigen::Vector2f tmp_VT; + for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) + { + std::size_t idx = tex_mesh.tex_polygons[m][i].vertices[j]; + memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float)); + memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float)); + memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float)); + + // calculate uv coordinates + tmp_VT[0] = (x_ + x_offset) / x_range; + tmp_VT[1] = (z_ + z_offset) / z_range; + texture_map_tmp.push_back (tmp_VT); + } + }// end faces + + // texture materials + std::stringstream tex_name; + tex_name << "material_" << m; + tex_name >> tex_material_.tex_name; + tex_material_.tex_file = tex_files_[m]; + tex_mesh.tex_materials.push_back (tex_material_); + + // texture coordinates + tex_mesh.tex_coordinates.push_back (texture_map_tmp); + }// end meshes +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::mapMultipleTexturesToMeshUV (pcl::TextureMesh &tex_mesh, pcl::texture_mapping::CameraVector &cams) +{ + + if (tex_mesh.tex_polygons.size () != cams.size () + 1) + { + PCL_ERROR ("The mesh should be divided into nbCamera+1 sub-meshes.\n"); + PCL_ERROR ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ()); + return; + } + + PCL_INFO ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ()); + + typename pcl::PointCloud::Ptr originalCloud (new pcl::PointCloud); + typename pcl::PointCloud::Ptr camera_transformed_cloud (new pcl::PointCloud); + + // convert mesh's cloud to pcl format for ease + pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud); + + for (std::size_t m = 0; m < cams.size (); ++m) + { + // get current camera parameters + Camera current_cam = cams[m]; + + // get camera transform + Eigen::Affine3f cam_trans = current_cam.pose; + + // transform cloud into current camera frame + pcl::transformPointCloud (*originalCloud, *camera_transformed_cloud, cam_trans.inverse ()); + + // vector of texture coordinates for each face + std::vector > texture_map_tmp; + + // processing each face visible by this camera + for (const auto &tex_polygon : tex_mesh.tex_polygons[m]) + { + Eigen::Vector2f tmp_VT; + // for each point of this face + for (const unsigned int &vertex : tex_polygon.vertices) + { + // get point + PointInT pt = camera_transformed_cloud->points[vertex]; + + // compute UV coordinates for this point + getPointUVCoordinates (pt, current_cam, tmp_VT); + texture_map_tmp.push_back (tmp_VT); + }// end points + }// end faces + + // texture materials + std::stringstream tex_name; + tex_name << "material_" << m; + tex_name >> tex_material_.tex_name; + tex_material_.tex_file = current_cam.texture_file; + tex_mesh.tex_materials.push_back (tex_material_); + + // texture coordinates + tex_mesh.tex_coordinates.push_back (texture_map_tmp); + }// end cameras + + // push on extra empty UV map (for unseen faces) so that obj writer does not crash! + std::vector > texture_map_tmp; + for (const auto &tex_polygon : tex_mesh.tex_polygons[cams.size ()]) + for (std::size_t j = 0; j < tex_polygon.vertices.size (); ++j) + { + Eigen::Vector2f tmp_VT; + tmp_VT[0] = -1; + tmp_VT[1] = -1; + texture_map_tmp.push_back (tmp_VT); + } + + tex_mesh.tex_coordinates.push_back (texture_map_tmp); + + // push on an extra dummy material for the same reason + tex_material_.tex_name = "material_" + std::to_string(cams.size()); + tex_material_.tex_file = "occluded.jpg"; + tex_mesh.tex_materials.push_back (tex_material_); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::TextureMapping::isPointOccluded (const PointInT &pt, OctreePtr octree) +{ + Eigen::Vector3f direction; + direction (0) = pt.x; + direction (1) = pt.y; + direction (2) = pt.z; + + std::vector indices; + + PointCloudConstPtr cloud (new PointCloud()); + cloud = octree->getInputCloud(); + + double distance_threshold = octree->getResolution(); + + // raytrace + octree->getIntersectedVoxelIndices(direction, -direction, indices); + + int nbocc = static_cast (indices.size ()); + for (const int &index : indices) + { + // if intersected point is on the over side of the camera + if (pt.z * cloud->points[index].z < 0) + { + nbocc--; + continue; + } + + if (std::fabs (cloud->points[index].z - pt.z) <= distance_threshold) + { + // points are very close to each-other, we do not consider the occlusion + nbocc--; + } + } + + return (nbocc != 0); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::removeOccludedPoints (const PointCloudPtr &input_cloud, + PointCloudPtr &filtered_cloud, + const double octree_voxel_size, std::vector &visible_indices, + std::vector &occluded_indices) +{ + // variable used to filter occluded points by depth + double maxDeltaZ = octree_voxel_size; + + // create an octree to perform rayTracing + Octree octree (octree_voxel_size); + // create octree structure + octree.setInputCloud (input_cloud); + // update bounding box automatically + octree.defineBoundingBox (); + // add points in the tree + octree.addPointsFromInputCloud (); + + visible_indices.clear (); + + // for each point of the cloud, raycast toward camera and check intersected voxels. + Eigen::Vector3f direction; + std::vector indices; + for (std::size_t i = 0; i < input_cloud->points.size (); ++i) + { + direction (0) = input_cloud->points[i].x; + direction (1) = input_cloud->points[i].y; + direction (2) = input_cloud->points[i].z; + + // if point is not occluded + octree.getIntersectedVoxelIndices (direction, -direction, indices); + + int nbocc = static_cast (indices.size ()); + for (const int &index : indices) + { + // if intersected point is on the over side of the camera + if (input_cloud->points[i].z * input_cloud->points[index].z < 0) + { + nbocc--; + continue; + } + + if (std::fabs (input_cloud->points[index].z - input_cloud->points[i].z) <= maxDeltaZ) + { + // points are very close to each-other, we do not consider the occlusion + nbocc--; + } + } + + if (nbocc == 0) + { + // point is added in the filtered mesh + filtered_cloud->points.push_back (input_cloud->points[i]); + visible_indices.push_back (static_cast (i)); + } + else + { + occluded_indices.push_back (static_cast (i)); + } + } + +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size) +{ + // copy mesh + cleaned_mesh = tex_mesh; + + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + typename pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud); + + // load points into a PCL format + pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud); + + std::vector visible, occluded; + removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded); + + // Now that we know which points are visible, let's iterate over each face. + // if the face has one invisible point => out! + for (std::size_t polygons = 0; polygons < cleaned_mesh.tex_polygons.size (); ++polygons) + { + // remove all faces from cleaned mesh + cleaned_mesh.tex_polygons[polygons].clear (); + // iterate over faces + for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[polygons].size (); ++faces) + { + // check if all the face's points are visible + bool faceIsVisible = true; + std::vector::iterator it; + + // iterate over face's vertex + for (const unsigned int &vertex : tex_mesh.tex_polygons[polygons][faces].vertices) + { + it = find (occluded.begin (), occluded.end (), vertex); + + if (it == occluded.end ()) + { + // point is not in the occluded vector + // PCL_INFO (" VISIBLE!\n"); + } + else + { + // point was occluded + // PCL_INFO(" OCCLUDED!\n"); + faceIsVisible = false; + } + } + + if (faceIsVisible) + { + cleaned_mesh.tex_polygons[polygons].push_back (tex_mesh.tex_polygons[polygons][faces]); + } + + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, PointCloudPtr &filtered_cloud, + const double octree_voxel_size) +{ + PointCloudPtr cloud (new PointCloud); + + // load points into a PCL format + pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud); + + std::vector visible, occluded; + removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded); + +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template int +pcl::TextureMapping::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh, + const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size, + PointCloud &visible_pts) +{ + if (tex_mesh.tex_polygons.size () != 1) + { + PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n"); + return (-1); + } + + if (cameras.empty ()) + { + PCL_ERROR ("Must provide at least one camera info!\n"); + return (-1); + } + + // copy mesh + sorted_mesh = tex_mesh; + // clear polygons from cleaned_mesh + sorted_mesh.tex_polygons.clear (); + + typename pcl::PointCloud::Ptr original_cloud (new pcl::PointCloud); + typename pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud); + typename pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud); + + // load points into a PCL format + pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud); + + // for each camera + for (const auto &camera : cameras) + { + // get camera pose as transform + Eigen::Affine3f cam_trans = camera.pose; + + // transform original cloud in camera coordinates + pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ()); + + // find occlusions on transformed cloud + std::vector visible, occluded; + removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded); + visible_pts = *filtered_cloud; + + // find visible faces => add them to polygon N for camera N + // add polygon group for current camera in clean + std::vector visibleFaces_currentCam; + // iterate over the faces of the current mesh + for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces) + { + // check if all the face's points are visible + bool faceIsVisible = true; + std::vector::iterator it; + + // iterate over face's vertex + for (std::size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice) + { + // TODO this is far too long! Better create an helper function that raycasts here. + it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]); + + if (it == occluded.end ()) + { + // point is not occluded + // does it land on the camera's image plane? + PointInT pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]]; + Eigen::Vector2f dummy_UV; + if (!getPointUVCoordinates (pt, camera, dummy_UV)) + { + // point is not visible by the camera + faceIsVisible = false; + } + } + else + { + faceIsVisible = false; + } + } + + if (faceIsVisible) + { + // push current visible face into the sorted mesh + visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]); + // remove it from the unsorted mesh + tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces); + faces--; + } + + } + sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam); + } + + // we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0] + // we need to add them as an extra polygon in the sorted mesh + sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]); + return (0); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::showOcclusions (const PointCloudPtr &input_cloud, + pcl::PointCloud::Ptr &colored_cloud, + const double octree_voxel_size, const bool show_nb_occlusions, + const int max_occlusions) + { + // variable used to filter occluded points by depth + double maxDeltaZ = octree_voxel_size * 2.0; + + // create an octree to perform rayTracing + Octree octree (octree_voxel_size); + // create octree structure + octree.setInputCloud (input_cloud); + // update bounding box automatically + octree.defineBoundingBox (); + // add points in the tree + octree.addPointsFromInputCloud (); + + // ray direction + Eigen::Vector3f direction; + + std::vector indices; + // point from where we ray-trace + pcl::PointXYZI pt; + + std::vector zDist; + std::vector ptDist; + // for each point of the cloud, ray-trace toward the camera and check intersected voxels. + for (std::size_t i = 0; i < input_cloud->points.size (); ++i) + { + direction (0) = input_cloud->points[i].x; + pt.x = input_cloud->points[i].x; + direction (1) = input_cloud->points[i].y; + pt.y = input_cloud->points[i].y; + direction (2) = input_cloud->points[i].z; + pt.z = input_cloud->points[i].z; + + // get number of occlusions for that point + indices.clear (); + int nbocc = octree.getIntersectedVoxelIndices (direction, -direction, indices); + + nbocc = static_cast (indices.size ()); + + // TODO need to clean this up and find tricks to get remove aliasaing effect on planes + for (const int &index : indices) + { + // if intersected point is on the over side of the camera + if (pt.z * input_cloud->points[index].z < 0) + { + nbocc--; + } + else if (std::fabs (input_cloud->points[index].z - pt.z) <= maxDeltaZ) + { + // points are very close to each-other, we do not consider the occlusion + nbocc--; + } + else + { + zDist.push_back (std::fabs (input_cloud->points[index].z - pt.z)); + ptDist.push_back (pcl::euclideanDistance (input_cloud->points[index], pt)); + } + } + + if (show_nb_occlusions) + (nbocc <= max_occlusions) ? (pt.intensity = static_cast (nbocc)) : (pt.intensity = static_cast (max_occlusions)); + else + (nbocc == 0) ? (pt.intensity = 0) : (pt.intensity = 1); + + colored_cloud->points.push_back (pt); + } + + if (zDist.size () >= 2) + { + std::sort (zDist.begin (), zDist.end ()); + std::sort (ptDist.begin (), ptDist.end ()); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::showOcclusions (pcl::TextureMesh &tex_mesh, pcl::PointCloud::Ptr &colored_cloud, + double octree_voxel_size, bool show_nb_occlusions, int max_occlusions) +{ + // load points into a PCL format + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud); + + showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TextureMapping::textureMeshwithMultipleCameras (pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras) +{ + + if (mesh.tex_polygons.size () != 1) + return; + + typename pcl::PointCloud::Ptr mesh_cloud (new pcl::PointCloud); + + pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud); + + std::vector faces; + + for (int current_cam = 0; current_cam < static_cast (cameras.size ()); ++current_cam) + { + PCL_INFO ("Processing camera %d of %d.\n", current_cam+1, cameras.size ()); + + // transform mesh into camera's frame + typename pcl::PointCloud::Ptr camera_cloud (new pcl::PointCloud); + pcl::transformPointCloud (*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse ()); + + // CREATE UV MAP FOR CURRENT FACES + pcl::PointCloud::Ptr projections (new pcl::PointCloud); + std::vector visibility; + visibility.resize (mesh.tex_polygons[current_cam].size ()); + std::vector indexes_uv_to_points; + // for each current face + + //TODO change this + pcl::PointXY nan_point; + nan_point.x = std::numeric_limits::quiet_NaN (); + nan_point.y = std::numeric_limits::quiet_NaN (); + UvIndex u_null; + u_null.idx_cloud = -1; + u_null.idx_face = -1; + + int cpt_invisible=0; + for (int idx_face = 0; idx_face < static_cast (mesh.tex_polygons[current_cam].size ()); ++idx_face) + { + //project each vertice, if one is out of view, stop + pcl::PointXY uv_coord1; + pcl::PointXY uv_coord2; + pcl::PointXY uv_coord3; + + if (isFaceProjected (cameras[current_cam], + camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[0]], + camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[1]], + camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[2]], + uv_coord1, + uv_coord2, + uv_coord3)) + { + // face is in the camera's FOV + + // add UV coordinates + projections->points.push_back (uv_coord1); + projections->points.push_back (uv_coord2); + projections->points.push_back (uv_coord3); + + // remember corresponding face + UvIndex u1, u2, u3; + u1.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[0]; + u2.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[1]; + u3.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[2]; + u1.idx_face = idx_face; u2.idx_face = idx_face; u3.idx_face = idx_face; + indexes_uv_to_points.push_back (u1); + indexes_uv_to_points.push_back (u2); + indexes_uv_to_points.push_back (u3); + + //keep track of visibility + visibility[idx_face] = true; + } + else + { + projections->points.push_back (nan_point); + projections->points.push_back (nan_point); + projections->points.push_back (nan_point); + indexes_uv_to_points.push_back (u_null); + indexes_uv_to_points.push_back (u_null); + indexes_uv_to_points.push_back (u_null); + //keep track of visibility + visibility[idx_face] = false; + cpt_invisible++; + } + } + + // projections contains all UV points of the current faces + // indexes_uv_to_points links a uv point to its point in the camera cloud + // visibility contains tells if a face was in the camera FOV (false = skip) + + // TODO handle case were no face could be projected + if (visibility.size () - cpt_invisible !=0) + { + //create kdtree + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud (projections); + + std::vector idxNeighbors; + std::vector neighborsSquaredDistance; + // af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces + // then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded + cpt_invisible = 0; + for (int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam) + { + // project all faces + for (int idx_face = 0; idx_face < static_cast (mesh.tex_polygons[idx_pcam].size ()); ++idx_face) + { + + if (idx_pcam == current_cam && !visibility[idx_face]) + { + // we are now checking for self occlusions within the current faces + // the current face was already declared as occluded. + // therefore, it cannot occlude another face anymore => we skip it + continue; + } + + // project each vertice, if one is out of view, stop + pcl::PointXY uv_coord1; + pcl::PointXY uv_coord2; + pcl::PointXY uv_coord3; + + if (isFaceProjected (cameras[current_cam], + camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]], + camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]], + camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]], + uv_coord1, + uv_coord2, + uv_coord3)) + { + // face is in the camera's FOV + //get its circumsribed circle + double radius; + pcl::PointXY center; + // getTriangleCircumcenterAndSize (uv_coord1, uv_coord2, uv_coord3, center, radius); + getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius); // this function yields faster results than getTriangleCircumcenterAndSize + + // get points inside circ.circle + if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 ) + { + // for each neighbor + for (const int &idxNeighbor : idxNeighbors) + { + if (std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z, + std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z, + camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]].z)) + < camera_cloud->points[indexes_uv_to_points[idxNeighbor].idx_cloud].z) + { + // neighbor is farther than all the face's points. Check if it falls into the triangle + if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, projections->points[idxNeighbor])) + { + // current neighbor is inside triangle and is closer => the corresponding face + visibility[indexes_uv_to_points[idxNeighbor].idx_face] = false; + cpt_invisible++; + //TODO we could remove the projections of this face from the kd-tree cloud, but I fond it slower, and I need the point to keep ordered to querry UV coordinates later + } + } + } + } + } + } + } + } + + // now, visibility is true for each face that belongs to the current camera + // if a face is not visible, we push it into the next one. + + if (static_cast (mesh.tex_coordinates.size ()) <= current_cam) + { + std::vector > dummy_container; + mesh.tex_coordinates.push_back (dummy_container); + } + mesh.tex_coordinates[current_cam].resize (3 * visibility.size ()); + + std::vector occluded_faces; + occluded_faces.resize (visibility.size ()); + std::vector visible_faces; + visible_faces.resize (visibility.size ()); + + int cpt_occluded_faces = 0; + int cpt_visible_faces = 0; + + for (std::size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face) + { + if (visibility[idx_face]) + { + // face is visible by the current camera copy UV coordinates + mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](0) = projections->points[idx_face*3].x; + mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](1) = projections->points[idx_face*3].y; + + mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = projections->points[idx_face*3 + 1].x; + mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = projections->points[idx_face*3 + 1].y; + + mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = projections->points[idx_face*3 + 2].x; + mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = projections->points[idx_face*3 + 2].y; + + visible_faces[cpt_visible_faces] = mesh.tex_polygons[current_cam][idx_face]; + + cpt_visible_faces++; + } + else + { + // face is occluded copy face into temp vector + occluded_faces[cpt_occluded_faces] = mesh.tex_polygons[current_cam][idx_face]; + cpt_occluded_faces++; + } + } + mesh.tex_coordinates[current_cam].resize (cpt_visible_faces*3); + + occluded_faces.resize (cpt_occluded_faces); + mesh.tex_polygons.push_back (occluded_faces); + + visible_faces.resize (cpt_visible_faces); + mesh.tex_polygons[current_cam].clear (); + mesh.tex_polygons[current_cam] = visible_faces; + } + + // we have been through all the cameras. + // if any faces are left, they were not visible by any camera + // we still need to produce uv coordinates for them + + if (mesh.tex_coordinates.size() <= cameras.size ()) + { + std::vector > dummy_container; + mesh.tex_coordinates.push_back(dummy_container); + } + + + for(std::size_t idx_face = 0 ; idx_face < mesh.tex_polygons[cameras.size()].size() ; ++idx_face) + { + Eigen::Vector2f UV1, UV2, UV3; + UV1(0) = -1.0; UV1(1) = -1.0; + UV2(0) = -1.0; UV2(1) = -1.0; + UV3(0) = -1.0; UV3(1) = -1.0; + mesh.tex_coordinates[cameras.size()].push_back(UV1); + mesh.tex_coordinates[cameras.size()].push_back(UV2); + mesh.tex_coordinates[cameras.size()].push_back(UV3); + } + +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::TextureMapping::getTriangleCircumcenterAndSize(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circomcenter, double &radius) +{ + // we simplify the problem by translating the triangle's origin to its first point + pcl::PointXY ptB, ptC; + ptB.x = p2.x - p1.x; ptB.y = p2.y - p1.y; // B'=B-A + ptC.x = p3.x - p1.x; ptC.y = p3.y - p1.y; // C'=C-A + + double D = 2.0*(ptB.x*ptC.y - ptB.y*ptC.x); // D'=2(B'x*C'y - B'y*C'x) + + // Safety check to avoid division by zero + if(D == 0) + { + circomcenter.x = p1.x; + circomcenter.y = p1.y; + } + else + { + // compute squares once + double bx2 = ptB.x * ptB.x; // B'x^2 + double by2 = ptB.y * ptB.y; // B'y^2 + double cx2 = ptC.x * ptC.x; // C'x^2 + double cy2 = ptC.y * ptC.y; // C'y^2 + + // compute circomcenter's coordinates (translate back to original coordinates) + circomcenter.x = static_cast (p1.x + (ptC.y*(bx2 + by2) - ptB.y*(cx2 + cy2)) / D); + circomcenter.y = static_cast (p1.y + (ptB.x*(cx2 + cy2) - ptC.x*(bx2 + by2)) / D); + } + + radius = std::sqrt( (circomcenter.x - p1.x)*(circomcenter.x - p1.x) + (circomcenter.y - p1.y)*(circomcenter.y - p1.y));//2.0* (p1.x*(p2.y - p3.y) + p2.x*(p3.y - p1.y) + p3.x*(p1.y - p2.y)); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::TextureMapping::getTriangleCircumcscribedCircleCentroid ( const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius) +{ + // compute centroid's coordinates (translate back to original coordinates) + circumcenter.x = static_cast (p1.x + p2.x + p3.x ) / 3; + circumcenter.y = static_cast (p1.y + p2.y + p3.y ) / 3; + double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y) ; + double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y) ; + double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y) ; + + // radius + radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ; +} + + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline bool +pcl::TextureMapping::getPointUVCoordinates(const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates) +{ + if (pt.z > 0) + { + // compute image center and dimension + double sizeX = cam.width; + double sizeY = cam.height; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = sizeX / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = sizeY / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h > 0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; + + // project point on camera's image plane + UV_coordinates.x = static_cast ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal + UV_coordinates.y = 1.0f - static_cast ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical + + // point is visible! + if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0) + return (true); // point was visible by the camera + } + + // point is NOT visible by the camera + UV_coordinates.x = -1.0f; + UV_coordinates.y = -1.0f; + return (false); // point was not visible by the camera +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline bool +pcl::TextureMapping::checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt) +{ + // Compute vectors + Eigen::Vector2d v0, v1, v2; + v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y; // v0= C - A + v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y; // v1= B - A + v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y; // v2= P - A + + // Compute dot products + double dot00 = v0.dot(v0); // dot00 = dot(v0, v0) + double dot01 = v0.dot(v1); // dot01 = dot(v0, v1) + double dot02 = v0.dot(v2); // dot02 = dot(v0, v2) + double dot11 = v1.dot(v1); // dot11 = dot(v1, v1) + double dot12 = v1.dot(v2); // dot12 = dot(v1, v2) + + // Compute barycentric coordinates + double invDenom = 1.0 / (dot00*dot11 - dot01*dot01); + double u = (dot11*dot02 - dot01*dot12) * invDenom; + double v = (dot00*dot12 - dot01*dot02) * invDenom; + + // Check if point is in triangle + return ((u >= 0) && (v >= 0) && (u + v < 1)); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline bool +pcl::TextureMapping::isFaceProjected (const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3) +{ + return (getPointUVCoordinates(p1, camera, proj1) + && + getPointUVCoordinates(p2, camera, proj2) + && + getPointUVCoordinates(p3, camera, proj3) + ); +} + +#define PCL_INSTANTIATE_TextureMapping(T) \ + template class PCL_EXPORTS pcl::TextureMapping; + +#endif /* TEXTURE_MAPPING_HPP_ */ + diff --git a/deps_install/include/pcl-1.10/pcl/surface/marching_cubes.h b/deps_install/include/pcl-1.10/pcl/surface/marching_cubes.h new file mode 100755 index 0000000..5a499be --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/marching_cubes.h @@ -0,0 +1,527 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /* + * Tables, and functions, derived from Paul Bourke's Marching Cubes implementation: + * http://paulbourke.net/geometry/polygonise/ + * Cube vertex indices: + * y_dir 4 ________ 5 + * /| /| + * / | / | + * 7 /_______ / | + * | | |6 | + * | 0|__|_____|1 x_dir + * | / | / + * | / | / + z_dir|/_______|/ + * 3 2 + */ + const unsigned int edgeTable[256] = { + 0x0 , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c, + 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00, + 0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c, + 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90, + 0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c, + 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30, + 0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac, + 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0, + 0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c, + 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60, + 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc, + 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0, + 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c, + 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950, + 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc , + 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0, + 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc, + 0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0, + 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c, + 0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650, + 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc, + 0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0, + 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c, + 0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460, + 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac, + 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0, + 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c, + 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230, + 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c, + 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190, + 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c, + 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0 + }; + const int triTable[256][16] = { + {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1}, + {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1}, + {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1}, + {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1}, + {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1}, + {9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, + {2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1}, + {8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1}, + {9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, + {4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1}, + {3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1}, + {1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1}, + {4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1}, + {4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, + {5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1}, + {2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1}, + {9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, + {0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, + {2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1}, + {10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1}, + {5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1}, + {5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1}, + {9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1}, + {0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1}, + {1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1}, + {10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1}, + {8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1}, + {2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1}, + {7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1}, + {2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1}, + {11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1}, + {5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1}, + {11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1}, + {11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, + {1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1}, + {9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1}, + {5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1}, + {2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, + {5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1}, + {6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1}, + {3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1}, + {6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1}, + {5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1}, + {1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, + {10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1}, + {6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1}, + {8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1}, + {7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1}, + {3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, + {5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1}, + {0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1}, + {9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1}, + {8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1}, + {5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1}, + {0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1}, + {6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1}, + {10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1}, + {10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1}, + {8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1}, + {1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1}, + {0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1}, + {10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1}, + {3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1}, + {6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1}, + {9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1}, + {8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1}, + {3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1}, + {6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1}, + {0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1}, + {10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1}, + {10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1}, + {2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1}, + {7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1}, + {7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1}, + {2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1}, + {1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1}, + {11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1}, + {8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1}, + {0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1}, + {7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, + {10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, + {2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, + {6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1}, + {7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1}, + {2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1}, + {1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1}, + {10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1}, + {10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1}, + {0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1}, + {7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1}, + {6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1}, + {8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1}, + {9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1}, + {6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1}, + {4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1}, + {10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1}, + {8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1}, + {0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1}, + {1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1}, + {8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1}, + {10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1}, + {4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1}, + {10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, + {5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, + {11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1}, + {9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, + {6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1}, + {7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1}, + {3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1}, + {7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1}, + {9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1}, + {3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1}, + {6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1}, + {9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1}, + {1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1}, + {4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1}, + {7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1}, + {6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1}, + {3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1}, + {0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1}, + {6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1}, + {0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1}, + {11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1}, + {6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1}, + {5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1}, + {9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1}, + {1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1}, + {1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1}, + {10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1}, + {0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1}, + {5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1}, + {10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1}, + {11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1}, + {9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1}, + {7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1}, + {2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1}, + {8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1}, + {9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1}, + {9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1}, + {1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1}, + {9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1}, + {9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1}, + {5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1}, + {0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1}, + {10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1}, + {2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1}, + {0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1}, + {0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1}, + {9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1}, + {5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1}, + {3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1}, + {5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1}, + {8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1}, + {0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1}, + {9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1}, + {0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1}, + {1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1}, + {3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1}, + {4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1}, + {9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1}, + {11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1}, + {11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1}, + {2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1}, + {9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1}, + {3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1}, + {1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1}, + {4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1}, + {4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1}, + {0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1}, + {3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1}, + {3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1}, + {0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1}, + {9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1}, + {1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, + {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1} + }; + + + /** \brief The marching cubes surface reconstruction algorithm. This is an abstract class that takes a grid and + * extracts the isosurface as a mesh, based on the original marching cubes paper: + * + * Lorensen W.E., Cline H.E., "Marching cubes: A high resolution 3d surface construction algorithm", + * SIGGRAPH '87 + * + * \author Alexandru E. Ichim + * \ingroup surface + */ + template + class MarchingCubes : public SurfaceReconstruction + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SurfaceReconstruction::input_; + using SurfaceReconstruction::tree_; + + using PointCloudPtr = typename pcl::PointCloud::Ptr; + + using KdTree = pcl::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + /** \brief Constructor. */ + MarchingCubes (const float percentage_extend_grid = 0.0f, + const float iso_level = 0.0f) : + percentage_extend_grid_ (percentage_extend_grid), + iso_level_ (iso_level) + { + } + + /** \brief Destructor. */ + ~MarchingCubes (); + + + /** \brief Method that sets the iso level of the surface to be extracted. + * \param[in] iso_level the iso level. + */ + inline void + setIsoLevel (float iso_level) + { iso_level_ = iso_level; } + + /** \brief Method that returns the iso level of the surface to be extracted. */ + inline float + getIsoLevel () + { return iso_level_; } + + /** \brief Method that sets the marching cubes grid resolution. + * \param[in] res_x the resolution of the grid along the x-axis + * \param[in] res_y the resolution of the grid along the y-axis + * \param[in] res_z the resolution of the grid along the z-axis + */ + inline void + setGridResolution (int res_x, int res_y, int res_z) + { res_x_ = res_x; res_y_ = res_y; res_z_ = res_z; } + + /** \brief Method to get the marching cubes grid resolution. + * \param[in] res_x the resolution of the grid along the x-axis + * \param[in] res_y the resolution of the grid along the y-axis + * \param[in] res_z the resolution of the grid along the z-axis + */ + inline void + getGridResolution (int &res_x, int &res_y, int &res_z) + { res_x = res_x_; res_y = res_y_; res_z = res_z_; } + + /** \brief Method that sets the parameter that defines how much free space should be left inside the grid between + * the bounding box of the point cloud and the grid limits. Does not affect the resolution of the grid, it just + * changes the voxel size accordingly. + * \param[in] percentage the percentage of the bounding box that should be left empty between the bounding box and + * the grid limits. + */ + inline void + setPercentageExtendGrid (float percentage) + { percentage_extend_grid_ = percentage; } + + /** \brief Method that gets the parameter that defines how much free space should be left inside the grid between + * the bounding box of the point cloud and the grid limits, as a percentage of the bounding box. + */ + inline float + getPercentageExtendGrid () + { return percentage_extend_grid_; } + + protected: + /** \brief The data structure storing the 3D grid */ + std::vector grid_; + + /** \brief The grid resolution */ + int res_x_ = 32, res_y_ = 32, res_z_ = 32; + + /** \brief bounding box */ + Eigen::Array3f upper_boundary_; + Eigen::Array3f lower_boundary_; + + /** \brief size of voxels */ + Eigen::Array3f size_voxel_; + + /** \brief Parameter that defines how much free space should be left inside the grid between + * the bounding box of the point cloud and the grid limits, as a percentage of the bounding box.*/ + float percentage_extend_grid_; + + /** \brief The iso level to be extracted. */ + float iso_level_; + + /** \brief Convert the point cloud into voxel data. + */ + virtual void + voxelizeData () = 0; + + /** \brief Interpolate along the voxel edge. + * \param[in] p1 The first point on the edge + * \param[in] p2 The second point on the edge + * \param[in] val_p1 The scalar value at p1 + * \param[in] val_p2 The scalar value at p2 + * \param[out] output The interpolated point along the edge + */ + void + interpolateEdge (Eigen::Vector3f &p1, Eigen::Vector3f &p2, float val_p1, float val_p2, Eigen::Vector3f &output); + + + /** \brief Calculate out the corresponding polygons in the leaf node + * \param leaf_node the leaf node to be checked + * \param index_3d the 3d index of the leaf node to be checked + * \param cloud point cloud to store the vertices of the polygon + */ + void + createSurface (const std::vector &leaf_node, + const Eigen::Vector3i &index_3d, + pcl::PointCloud &cloud); + + /** \brief Get the bounding box for the input data points. + */ + void + getBoundingBox (); + + + /** \brief Method that returns the scalar value at the given grid position. + * \param[in] pos The 3D position in the grid + */ + virtual float + getGridValue (Eigen::Vector3i pos); + + /** \brief Method that returns the scalar values of the neighbors of a given 3D position in the grid. + * \param[in] index3d the point in the grid + * \param[out] leaf the set of values + */ + void + getNeighborList1D (std::vector &leaf, + Eigen::Vector3i &index3d); + + /** \brief Class get name method. */ + std::string getClassName () const override { return ("MarchingCubes"); } + + /** \brief Extract the surface. + * \param[out] output the resultant polygonal mesh + */ + void + performReconstruction (pcl::PolygonMesh &output) override; + + /** \brief Extract the surface. + * \param[out] points the points of the extracted mesh + * \param[out] polygons the connectivity between the point of the extracted mesh. + */ + void + performReconstruction (pcl::PointCloud &points, + std::vector &polygons) override; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/marching_cubes_hoppe.h b/deps_install/include/pcl-1.10/pcl/surface/marching_cubes_hoppe.h new file mode 100755 index 0000000..ebdfb71 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/marching_cubes_hoppe.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief The marching cubes surface reconstruction algorithm, using a signed distance function based on the distance + * from tangent planes, proposed by Hoppe et. al. in: + * Hoppe H., DeRose T., Duchamp T., MC-Donald J., Stuetzle W., "Surface reconstruction from unorganized points", + * SIGGRAPH '92 + * \author Alexandru E. Ichim + * \ingroup surface + */ + template + class MarchingCubesHoppe : public MarchingCubes + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SurfaceReconstruction::input_; + using SurfaceReconstruction::tree_; + using MarchingCubes::grid_; + using MarchingCubes::res_x_; + using MarchingCubes::res_y_; + using MarchingCubes::res_z_; + using MarchingCubes::size_voxel_; + using MarchingCubes::upper_boundary_; + using MarchingCubes::lower_boundary_; + + using PointCloudPtr = typename pcl::PointCloud::Ptr; + + using KdTree = pcl::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + + /** \brief Constructor. */ + MarchingCubesHoppe (const float dist_ignore = -1.0f, + const float percentage_extend_grid = 0.0f, + const float iso_level = 0.0f) : + MarchingCubes (percentage_extend_grid, iso_level), + dist_ignore_ (dist_ignore) + { + } + + /** \brief Destructor. */ + ~MarchingCubesHoppe (); + + /** \brief Convert the point cloud into voxel data. + */ + void + voxelizeData () override; + + /** \brief Method that sets the distance for ignoring voxels which are far from point cloud. + * If the distance is negative, then the distance functions would be calculated in all voxels; + * otherwise, only voxels with distance lower than dist_ignore would be involved in marching cube. + * \param[in] threshold of distance. Default value is -1.0. Set to negative if all voxels are + * to be involved. + */ + inline void + setDistanceIgnore (const float dist_ignore) + { dist_ignore_ = dist_ignore; } + + /** \brief get the distance for ignoring voxels which are far from point cloud. + * */ + inline float + getDistanceIgnore () const + { return dist_ignore_; } + + protected: + /** \brief ignore the distance function + * if it is negative + * or distance between voxel centroid and point are larger that it. */ + float dist_ignore_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/marching_cubes_rbf.h b/deps_install/include/pcl-1.10/pcl/surface/marching_cubes_rbf.h new file mode 100755 index 0000000..22fa923 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/marching_cubes_rbf.h @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief The marching cubes surface reconstruction algorithm, using a signed distance function based on radial + * basis functions. Partially based on: + * Carr J.C., Beatson R.K., Cherrie J.B., Mitchell T.J., Fright W.R., McCallum B.C. and Evans T.R., + * "Reconstruction and representation of 3D objects with radial basis functions" + * SIGGRAPH '01 + * + * \author Alexandru E. Ichim + * \ingroup surface + */ + template + class MarchingCubesRBF : public MarchingCubes + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SurfaceReconstruction::input_; + using SurfaceReconstruction::tree_; + using MarchingCubes::grid_; + using MarchingCubes::res_x_; + using MarchingCubes::res_y_; + using MarchingCubes::res_z_; + using MarchingCubes::size_voxel_; + using MarchingCubes::upper_boundary_; + using MarchingCubes::lower_boundary_; + + using PointCloudPtr = typename pcl::PointCloud::Ptr; + + using KdTree = pcl::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + + /** \brief Constructor. */ + MarchingCubesRBF (const float off_surface_epsilon = 0.1f, + const float percentage_extend_grid = 0.0f, + const float iso_level = 0.0f) : + MarchingCubes (percentage_extend_grid, iso_level), + off_surface_epsilon_ (off_surface_epsilon) + { + } + + /** \brief Destructor. */ + ~MarchingCubesRBF (); + + /** \brief Convert the point cloud into voxel data. + */ + void + voxelizeData () override; + + + /** \brief Set the off-surface points displacement value. + * \param[in] epsilon the value + */ + inline void + setOffSurfaceDisplacement (float epsilon) + { off_surface_epsilon_ = epsilon; } + + /** \brief Get the off-surface points displacement value. */ + inline float + getOffSurfaceDisplacement () + { return off_surface_epsilon_; } + + + protected: + /** \brief the Radial Basis Function kernel. */ + double + kernel (Eigen::Vector3d c, Eigen::Vector3d x); + + /** \brief The off-surface displacement value. */ + float off_surface_epsilon_; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/mls.h b/deps_install/include/pcl-1.10/pcl/surface/mls.h new file mode 100755 index 0000000..232a397 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/mls.h @@ -0,0 +1,754 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +// PCL includes +#include +#include +#include +#include + +#include +#include +#include + +namespace pcl +{ + + /** \brief Data structure used to store the results of the MLS fitting */ + struct MLSResult + { + enum ProjectionMethod + { + NONE, /**< \brief Project to the mls plane. */ + SIMPLE, /**< \brief Project along the mls plane normal to the polynomial surface. */ + ORTHOGONAL /**< \brief Project to the closest point on the polynonomial surface. */ + }; + + /** \brief Data structure used to store the MLS polynomial partial derivatives */ + struct PolynomialPartialDerivative + { + double z; /**< \brief The z component of the polynomial evaluated at z(u, v). */ + double z_u; /**< \brief The partial derivative dz/du. */ + double z_v; /**< \brief The partial derivative dz/dv. */ + double z_uu; /**< \brief The partial derivative d^2z/du^2. */ + double z_vv; /**< \brief The partial derivative d^2z/dv^2. */ + double z_uv; /**< \brief The partial derivative d^2z/dudv. */ + }; + + /** \brief Data structure used to store the MLS projection results */ + struct MLSProjectionResults + { + MLSProjectionResults () : u (0), v (0) {} + + double u; /**< \brief The u-coordinate of the projected point in local MLS frame. */ + double v; /**< \brief The u-coordinate of the projected point in local MLS frame. */ + Eigen::Vector3d point; /**< \brief The projected point. */ + Eigen::Vector3d normal; /**< \brief The projected point's normal. */ + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + inline + MLSResult () : num_neighbors (0), curvature (0.0f), order (0), valid (false) {} + + inline + MLSResult (const Eigen::Vector3d &a_query_point, + const Eigen::Vector3d &a_mean, + const Eigen::Vector3d &a_plane_normal, + const Eigen::Vector3d &a_u, + const Eigen::Vector3d &a_v, + const Eigen::VectorXd &a_c_vec, + const int a_num_neighbors, + const float a_curvature, + const int a_order); + + /** \brief Given a point calculate it's 3D location in the MLS frame. + * \param[in] pt The point + * \param[out] u The u-coordinate of the point in local MLS frame. + * \param[out] v The v-coordinate of the point in local MLS frame. + * \param[out] w The w-coordinate of the point in local MLS frame. + */ + inline void + getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const; + + /** \brief Given a point calculate it's 2D location in the MLS frame. + * \param[in] pt The point + * \param[out] u The u-coordinate of the point in local MLS frame. + * \param[out] v The v-coordinate of the point in local MLS frame. + */ + inline void + getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const; + + /** \brief Calculate the polynomial + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \return The polynomial value at the provide uv coordinates. + */ + inline double + getPolynomialValue (const double u, const double v) const; + + /** \brief Calculate the polynomial's first and second partial derivatives. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \return The polynomial partial derivatives at the provide uv coordinates. + */ + inline PolynomialPartialDerivative + getPolynomialPartialDerivative (const double u, const double v) const; + + /** \brief Calculate the principle curvatures using the polynomial surface. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \return The principle curvature [k1, k2] at the provided ub coordinates. + * \note If an error occurs the MLS_MINIMUM_PRINCIPLE_CURVATURE is returned. + */ + inline Eigen::Vector2f + calculatePrincipleCurvatures (const double u, const double v) const; + + /** \brief Project a point orthogonal to the polynomial surface. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \param[in] w The w-coordinate of the point in local MLS frame. + * \return The MLSProjectionResults for the input data. + * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane. + * \note If the optimization diverges it performs a simple projection on to the polynomial surface. + * \note This was implemented based on this https://math.stackexchange.com/questions/1497093/shortest-distance-between-point-and-surface + */ + inline MLSProjectionResults + projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const; + + /** \brief Project a point onto the MLS plane. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \return The MLSProjectionResults for the input data. + */ + inline MLSProjectionResults + projectPointToMLSPlane (const double u, const double v) const; + + /** \brief Project a point along the MLS plane normal to the polynomial surface. + * \param[in] u The u-coordinate of the point in local MLS frame. + * \param[in] v The v-coordinate of the point in local MLS frame. + * \return The MLSProjectionResults for the input data. + * \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane. + */ + inline MLSProjectionResults + projectPointSimpleToPolynomialSurface (const double u, const double v) const; + + /** + * \brief Project a point using the specified method. + * \param[in] pt The point to be project. + * \param[in] method The projection method to be used. + * \param[in] required_neighbors The minimum number of neighbors required. + * \note If required_neighbors then any number of neighbors is allowed. + * \note If required_neighbors is not satisfied it projects to the mls plane. + * \return The MLSProjectionResults for the input data. + */ + inline MLSProjectionResults + projectPoint (const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors = 0) const; + + /** + * \brief Project the query point used to generate the mls surface about using the specified method. + * \param[in] method The projection method to be used. + * \param[in] required_neighbors The minimum number of neighbors required. + * \note If required_neighbors then any number of neighbors is allowed. + * \note If required_neighbors is not satisfied it projects to the mls plane. + * \return The MLSProjectionResults for the input data. + */ + inline MLSProjectionResults + projectQueryPoint (ProjectionMethod method, int required_neighbors = 0) const; + + /** \brief Smooth a given point and its neighborghood using Moving Least Squares. + * \param[in] index the index of the query point in the input cloud + * \param[in] nn_indices the set of nearest neighbors indices for pt + * \param[in] search_radius the search radius used to find nearest neighbors for pt + * \param[in] polynomial_order the order of the polynomial to fit to the nearest neighbors + * \param[in] weight_func defines the weight function for the polynomial fit + */ + template void + computeMLSSurface (const pcl::PointCloud &cloud, + int index, + const std::vector &nn_indices, + double search_radius, + int polynomial_order = 2, + std::function weight_func = {}); + + Eigen::Vector3d query_point; /**< \brief The query point about which the mls surface was generated */ + Eigen::Vector3d mean; /**< \brief The mean point of all the neighbors. */ + Eigen::Vector3d plane_normal; /**< \brief The normal of the local plane of the query point. */ + Eigen::Vector3d u_axis; /**< \brief The axis corresponding to the u-coordinates of the local plane of the query point. */ + Eigen::Vector3d v_axis; /**< \brief The axis corresponding to the v-coordinates of the local plane of the query point. */ + Eigen::VectorXd c_vec; /**< \brief The polynomial coefficients Example: z = c_vec[0] + c_vec[1]*v + c_vec[2]*v^2 + c_vec[3]*u + c_vec[4]*u*v + c_vec[5]*u^2 */ + int num_neighbors; /**< \brief The number of neighbors used to create the mls surface. */ + float curvature; /**< \brief The curvature at the query point. */ + int order; /**< \brief The order of the polynomial. If order > 1 then use polynomial fit */ + bool valid; /**< \brief If True, the mls results data is valid, otherwise False. */ + PCL_MAKE_ALIGNED_OPERATOR_NEW + private: + /** + * \brief The default weight function used when fitting a polynomial surface + * \param sq_dist the squared distance from a point to origin of the mls frame + * \param sq_mls_radius the squraed mls search radius used + * \return The weight for a point at squared distance from the origin of the mls frame + */ + inline + double computeMLSWeight (const double sq_dist, const double sq_mls_radius) { return (std::exp (-sq_dist / sq_mls_radius)); } + + }; + + /** \brief MovingLeastSquares represent an implementation of the MLS (Moving Least Squares) algorithm + * for data smoothing and improved normal estimation. It also contains methods for upsampling the + * resulting cloud based on the parametric fit. + * Reference paper: "Computing and Rendering Point Set Surfaces" by Marc Alexa, Johannes Behr, + * Daniel Cohen-Or, Shachar Fleishman, David Levin and Claudio T. Silva + * www.sci.utah.edu/~shachar/Publications/crpss.pdf + * \note There is a parallelized version of the processing step, using the OpenMP standard. + * Compared to the standard version, an overhead is incurred in terms of runtime and memory usage. + * The upsampling methods DISTINCT_CLOUD and VOXEL_GRID_DILATION are not parallelized completely, + * i.e. parts of the algorithm run on a single thread only. + * \author Zoltan Csaba Marton, Radu B. Rusu, Alexandru E. Ichim, Suat Gedikli, Robert Huitl + * \ingroup surface + */ + template + class MovingLeastSquares : public CloudSurfaceProcessing + { + public: + typedef shared_ptr > Ptr; + typedef shared_ptr > ConstPtr; + + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::fake_indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + using NormalCloud = pcl::PointCloud; + using NormalCloudPtr = NormalCloud::Ptr; + + using PointCloudOut = pcl::PointCloud; + using PointCloudOutPtr = typename PointCloudOut::Ptr; + using PointCloudOutConstPtr = typename PointCloudOut::ConstPtr; + + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using SearchMethod = std::function &, std::vector &)>; + + enum UpsamplingMethod + { + NONE, /**< \brief No upsampling will be done, only the input points will be projected + to their own MLS surfaces. */ + DISTINCT_CLOUD, /**< \brief Project the points of the distinct cloud to the MLS surface. */ + SAMPLE_LOCAL_PLANE, /**< \brief The local plane of each input point will be sampled in a circular fashion + using the \ref upsampling_radius_ and the \ref upsampling_step_ parameters. */ + RANDOM_UNIFORM_DENSITY, /**< \brief The local plane of each input point will be sampled using an uniform random + distribution such that the density of points is constant throughout the + cloud - given by the \ref desired_num_points_in_radius_ parameter. */ + VOXEL_GRID_DILATION /**< \brief The input cloud will be inserted into a voxel grid with voxels of + size \ref voxel_size_; this voxel grid will be dilated \ref dilation_iteration_num_ + times and the resulting points will be projected to the MLS surface + of the closest point in the input cloud; the result is a point cloud + with filled holes and a constant point density. */ + }; + + /** \brief Empty constructor. */ + MovingLeastSquares () : CloudSurfaceProcessing (), + distinct_cloud_ (), + tree_ (), + order_ (2), + search_radius_ (0.0), + sqr_gauss_param_ (0.0), + compute_normals_ (false), + upsample_method_ (NONE), + upsampling_radius_ (0.0), + upsampling_step_ (0.0), + desired_num_points_in_radius_ (0), + cache_mls_results_ (true), + projection_method_ (MLSResult::SIMPLE), + threads_ (1), + voxel_size_ (1.0), + dilation_iteration_num_ (0), + nr_coeff_ (), + rng_uniform_distribution_ () + {}; + + /** \brief Empty destructor */ + ~MovingLeastSquares () {} + + + /** \brief Set whether the algorithm should also store the normals computed + * \note This is optional, but need a proper output cloud type + */ + inline void + setComputeNormals (bool compute_normals) { compute_normals_ = compute_normals; } + + /** \brief Provide a pointer to the search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) + { + tree_ = tree; + // Declare the search locator definition + search_method_ = [this] (int index, double radius, std::vector& k_indices, std::vector& k_sqr_distances) + { + return tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, 0); + }; + } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () const { return (tree_); } + + /** \brief Set the order of the polynomial to be fit. + * \param[in] order the order of the polynomial + * \note Setting order > 1 indicates using a polynomial fit. + */ + inline void + setPolynomialOrder (int order) { order_ = order; } + + /** \brief Get the order of the polynomial to be fit. */ + inline int + getPolynomialOrder () const { return (order_); } + + /** \brief Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation. + * \param[in] polynomial_fit set to true for polynomial fit + */ + PCL_DEPRECATED("use setPolynomialOrder() instead") + inline void + setPolynomialFit (bool polynomial_fit) + { + if (polynomial_fit) + { + if (order_ < 2) + { + order_ = 2; + } + } + else + { + order_ = 0; + } + } + + /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */ + PCL_DEPRECATED("use getPolynomialOrder() instead") + inline bool + getPolynomialFit () const { return (order_ > 1); } + + /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting. + * \param[in] radius the sphere radius that is to contain all k-nearest neighbors + * \note Calling this method resets the squared Gaussian parameter to radius * radius ! + */ + inline void + setSearchRadius (double radius) { search_radius_ = radius; sqr_gauss_param_ = search_radius_ * search_radius_; } + + /** \brief Get the sphere radius used for determining the k-nearest neighbors. */ + inline double + getSearchRadius () const { return (search_radius_); } + + /** \brief Set the parameter used for distance based weighting of neighbors (the square of the search radius works + * best in general). + * \param[in] sqr_gauss_param the squared Gaussian parameter + */ + inline void + setSqrGaussParam (double sqr_gauss_param) { sqr_gauss_param_ = sqr_gauss_param; } + + /** \brief Get the parameter for distance based weighting of neighbors. */ + inline double + getSqrGaussParam () const { return (sqr_gauss_param_); } + + /** \brief Set the upsampling method to be used + * \param method + */ + inline void + setUpsamplingMethod (UpsamplingMethod method) { upsample_method_ = method; } + + /** \brief Set the distinct cloud used for the DISTINCT_CLOUD upsampling method. */ + inline void + setDistinctCloud (PointCloudInConstPtr distinct_cloud) { distinct_cloud_ = distinct_cloud; } + + /** \brief Get the distinct cloud used for the DISTINCT_CLOUD upsampling method. */ + inline PointCloudInConstPtr + getDistinctCloud () const { return (distinct_cloud_); } + + + /** \brief Set the radius of the circle in the local point plane that will be sampled + * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling + * \param[in] radius the radius of the circle + */ + inline void + setUpsamplingRadius (double radius) { upsampling_radius_ = radius; } + + /** \brief Get the radius of the circle in the local point plane that will be sampled + * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling + */ + inline double + getUpsamplingRadius () const { return (upsampling_radius_); } + + /** \brief Set the step size for the local plane sampling + * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling + * \param[in] step_size the step size + */ + inline void + setUpsamplingStepSize (double step_size) { upsampling_step_ = step_size; } + + + /** \brief Get the step size for the local plane sampling + * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling + */ + inline double + getUpsamplingStepSize () const { return (upsampling_step_); } + + /** \brief Set the parameter that specifies the desired number of points within the search radius + * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling + * \param[in] desired_num_points_in_radius the desired number of points in the output cloud in a sphere of + * radius \ref search_radius_ around each point + */ + inline void + setPointDensity (int desired_num_points_in_radius) { desired_num_points_in_radius_ = desired_num_points_in_radius; } + + + /** \brief Get the parameter that specifies the desired number of points within the search radius + * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling + */ + inline int + getPointDensity () const { return (desired_num_points_in_radius_); } + + /** \brief Set the voxel size for the voxel grid + * \note Used only in the VOXEL_GRID_DILATION upsampling method + * \param[in] voxel_size the edge length of a cubic voxel in the voxel grid + */ + inline void + setDilationVoxelSize (float voxel_size) { voxel_size_ = voxel_size; } + + + /** \brief Get the voxel size for the voxel grid + * \note Used only in the VOXEL_GRID_DILATION upsampling method + */ + inline float + getDilationVoxelSize () const { return (voxel_size_); } + + /** \brief Set the number of dilation steps of the voxel grid + * \note Used only in the VOXEL_GRID_DILATION upsampling method + * \param[in] iterations the number of dilation iterations + */ + inline void + setDilationIterations (int iterations) { dilation_iteration_num_ = iterations; } + + /** \brief Get the number of dilation steps of the voxel grid + * \note Used only in the VOXEL_GRID_DILATION upsampling method + */ + inline int + getDilationIterations () const { return (dilation_iteration_num_); } + + /** \brief Set whether the mls results should be stored for each point in the input cloud + * \param[in] True if the mls results should be stored, otherwise false. + * \note The cache_mls_results_ is forced to true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD. + * \note If memory consumption is a concern set to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD. + */ + inline void + setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; } + + /** \brief Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false). */ + inline bool + getCacheMLSResults () const { return (cache_mls_results_); } + + /** \brief Set the method to be used when projection the point on to the MLS surface. + * \param method + * \note This is only used when polynomial fit is enabled. + */ + inline void + setProjectionMethod (MLSResult::ProjectionMethod method) { projection_method_ = method; } + + + /** \brief Get the current projection method being used. */ + inline MLSResult::ProjectionMethod + getProjectionMethod () const { return (projection_method_); } + + /** \brief Get the MLSResults for input cloud + * \note The results are only stored if setCacheMLSResults(true) was called or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION. + * \note This vector is align with the input cloud indices, so use getCorrespondingIndices to get the correct results when using output cloud indices. + */ + inline const std::vector& + getMLSResults () const { return (mls_results_); } + + /** \brief Set the maximum number of threads to use + * \param threads the maximum number of hardware threads to use (0 sets the value to 1) + */ + inline void + setNumberOfThreads (unsigned int threads = 1) + { + threads_ = threads; + } + + /** \brief Base method for surface reconstruction for all points given in + * \param[out] output the resultant reconstructed surface model + */ + void + process (PointCloudOut &output) override; + + + /** \brief Get the set of indices with each point in output having the + * corresponding point in input */ + inline PointIndicesPtr + getCorrespondingIndices () const { return (corresponding_input_indices_); } + + protected: + /** \brief The point cloud that will hold the estimated normals, if set. */ + NormalCloudPtr normals_; + + /** \brief The distinct point cloud that will be projected to the MLS surface. */ + PointCloudInConstPtr distinct_cloud_; + + /** \brief The search method template for indices. */ + SearchMethod search_method_; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The order of the polynomial to be fit. */ + int order_; + + /** \brief The nearest neighbors search radius for each point. */ + double search_radius_; + + /** \brief Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine) */ + double sqr_gauss_param_; + + /** \brief Parameter that specifies whether the normals should be computed for the input cloud or not */ + bool compute_normals_; + + /** \brief Parameter that specifies the upsampling method to be used */ + UpsamplingMethod upsample_method_; + + /** \brief Radius of the circle in the local point plane that will be sampled + * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling + */ + double upsampling_radius_; + + /** \brief Step size for the local plane sampling + * \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling + */ + double upsampling_step_; + + /** \brief Parameter that specifies the desired number of points within the search radius + * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling + */ + int desired_num_points_in_radius_; + + /** \brief True if the mls results for the input cloud should be stored + * \note This is forced to true when using upsampling methods VOXEL_GRID_DILATION or DISTINCT_CLOUD. + */ + bool cache_mls_results_; + + /** \brief Stores the MLS result for each point in the input cloud + * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling + */ + std::vector mls_results_; + + /** \brief Parameter that specifies the projection method to be used. */ + MLSResult::ProjectionMethod projection_method_; + + /** \brief The maximum number of threads the scheduler should use. */ + unsigned int threads_; + + + /** \brief A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling + * \note Used only in the case of VOXEL_GRID_DILATION upsampling + */ + class MLSVoxelGrid + { + public: + struct Leaf { Leaf () : valid (true) {} bool valid; }; + + MLSVoxelGrid (PointCloudInConstPtr& cloud, + IndicesPtr &indices, + float voxel_size); + + void + dilate (); + + inline void + getIndexIn1D (const Eigen::Vector3i &index, std::uint64_t &index_1d) const + { + index_1d = index[0] * data_size_ * data_size_ + + index[1] * data_size_ + index[2]; + } + + inline void + getIndexIn3D (std::uint64_t index_1d, Eigen::Vector3i& index_3d) const + { + index_3d[0] = static_cast (index_1d / (data_size_ * data_size_)); + index_1d -= index_3d[0] * data_size_ * data_size_; + index_3d[1] = static_cast (index_1d / data_size_); + index_1d -= index_3d[1] * data_size_; + index_3d[2] = static_cast (index_1d); + } + + inline void + getCellIndex (const Eigen::Vector3f &p, Eigen::Vector3i& index) const + { + for (int i = 0; i < 3; ++i) + index[i] = static_cast ((p[i] - bounding_min_ (i)) / voxel_size_); + } + + inline void + getPosition (const std::uint64_t &index_1d, Eigen::Vector3f &point) const + { + Eigen::Vector3i index_3d; + getIndexIn3D (index_1d, index_3d); + for (int i = 0; i < 3; ++i) + point[i] = static_cast (index_3d[i]) * voxel_size_ + bounding_min_[i]; + } + + typedef std::map HashMap; + HashMap voxel_grid_; + Eigen::Vector4f bounding_min_, bounding_max_; + std::uint64_t data_size_; + float voxel_size_; + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + + /** \brief Voxel size for the VOXEL_GRID_DILATION upsampling method */ + float voxel_size_; + + /** \brief Number of dilation steps for the VOXEL_GRID_DILATION upsampling method */ + int dilation_iteration_num_; + + /** \brief Number of coefficients, to be computed from the requested order.*/ + int nr_coeff_; + + /** \brief Collects for each point in output the corrseponding point in the input. */ + PointIndicesPtr corresponding_input_indices_; + + /** \brief Search for the closest nearest neighbors of a given point using a radius search + * \param[in] index the index of the query point + * \param[out] indices the resultant vector of indices representing the k-nearest neighbors + * \param[out] sqr_distances the resultant squared distances from the query point to the k-nearest neighbors + */ + inline int + searchForNeighbors (int index, std::vector &indices, std::vector &sqr_distances) const + { + return (search_method_ (index, search_radius_, indices, sqr_distances)); + } + + /** \brief Smooth a given point and its neighborghood using Moving Least Squares. + * \param[in] index the index of the query point in the input cloud + * \param[in] nn_indices the set of nearest neighbors indices for pt + * \param[out] projected_points the set of points projected points around the query point + * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned, + * in the case of the other upsampling methods, multiple points will be returned) + * \param[out] projected_points_normals the normals corresponding to the projected points + * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input + * \param[out] mls_result stores the MLS result for each point in the input cloud + * (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling) + */ + void + computeMLSPointNormal (int index, + const std::vector &nn_indices, + PointCloudOut &projected_points, + NormalCloud &projected_points_normals, + PointIndices &corresponding_input_indices, + MLSResult &mls_result) const; + + + /** \brief This is a helper function for add projected points + * \param[in] index the index of the query point in the input cloud + * \param[in] point the projected point to be added + * \param[in] normal the projected point's normal to be added + * \param[in] curvature the projected point's curvature + * \param[out] projected_points the set of projected points around the query point + * \param[out] projected_points_normals the normals corresponding to the projected points + * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input + */ + void + addProjectedPointNormal (int index, + const Eigen::Vector3d &point, + const Eigen::Vector3d &normal, + double curvature, + PointCloudOut &projected_points, + NormalCloud &projected_points_normals, + PointIndices &corresponding_input_indices) const; + + + void + copyMissingFields (const PointInT &point_in, + PointOutT &point_out) const; + + /** \brief Abstract surface reconstruction method. + * \param[out] output the result of the reconstruction + */ + void + performProcessing (PointCloudOut &output) override; + + /** \brief Perform upsampling for the distinct-cloud and voxel-grid methods + * \param[out] output the result of the reconstruction + */ + void + performUpsampling (PointCloudOut &output); + + private: + /** \brief Random number generator algorithm. */ + mutable std::mt19937 rng_; + + /** \brief Random number generator using an uniform distribution of floats + * \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling + */ + std::unique_ptr> rng_uniform_distribution_; + + /** \brief Abstract class get name method. */ + std::string + getClassName () const { return ("MovingLeastSquares"); } + }; + + template + using MovingLeastSquaresOMP PCL_DEPRECATED("use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/organized_fast_mesh.h b/deps_install/include/pcl-1.10/pcl/surface/organized_fast_mesh.h new file mode 100755 index 0000000..b9fe9f0 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/organized_fast_mesh.h @@ -0,0 +1,489 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011, Dirk Holz, University of Bonn. + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + + /** \brief Simple triangulation/surface reconstruction for organized point + * clouds. Neighboring points (pixels in image space) are connected to + * construct a triangular (or quad) mesh. + * + * \note If you use this code in any academic work, please cite: + * D. Holz and S. Behnke. + * Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing. + * In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS), + * Jeju Island, Korea, June 26-29 2012. + * http://purl.org/holz/papers/holz_2012_ias.pdf + * + * \author Dirk Holz, Radu B. Rusu + * \ingroup surface + */ + template + class OrganizedFastMesh : public MeshConstruction + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using MeshConstruction::input_; + using MeshConstruction::check_tree_; + + using PointCloudPtr = typename pcl::PointCloud::Ptr; + + using Polygons = std::vector; + + enum TriangulationType + { + TRIANGLE_RIGHT_CUT, // _always_ "cuts" a quad from top left to bottom right + TRIANGLE_LEFT_CUT, // _always_ "cuts" a quad from top right to bottom left + TRIANGLE_ADAPTIVE_CUT, // "cuts" where possible and prefers larger differences in 'z' direction + QUAD_MESH // create a simple quad mesh + }; + + /** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */ + OrganizedFastMesh () + : max_edge_length_a_ (0.0f) + , max_edge_length_b_ (0.0f) + , max_edge_length_c_ (0.0f) + , max_edge_length_set_ (false) + , max_edge_length_dist_dependent_ (false) + , triangle_pixel_size_rows_ (1) + , triangle_pixel_size_columns_ (1) + , triangulation_type_ (QUAD_MESH) + , viewpoint_ (Eigen::Vector3f::Zero ()) + , store_shadowed_faces_ (false) + , cos_angle_tolerance_ (std::abs (std::cos (pcl::deg2rad (12.5f)))) + , distance_tolerance_ (-1.0f) + , distance_dependent_ (false) + , use_depth_as_distance_(false) + { + check_tree_ = false; + }; + + /** \brief Destructor. */ + ~OrganizedFastMesh () {}; + + /** \brief Set a maximum edge length. + * Using not only the scalar \a a, but also \a b and \a c, allows for using a distance threshold in the form of: + * threshold(x) = c*x*x + b*x + a + * \param[in] a scalar coefficient of the (distance-dependent polynom) threshold + * \param[in] b linear coefficient of the (distance-dependent polynom) threshold + * \param[in] c quadratic coefficient of the (distance-dependent polynom) threshold + */ + inline void + setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f) + { + max_edge_length_a_ = a; + max_edge_length_b_ = b; + max_edge_length_c_ = c; + if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits::min()) + max_edge_length_set_ = true; + else + max_edge_length_set_ = false; + }; + + inline void + unsetMaxEdgeLength () + { + max_edge_length_set_ = false; + } + + /** \brief Set the edge length (in pixels) used for constructing the fixed mesh. + * \param[in] triangle_size edge length in pixels + * (Default: 1 = neighboring pixels are connected) + */ + inline void + setTrianglePixelSize (int triangle_size) + { + setTrianglePixelSizeRows (triangle_size); + setTrianglePixelSizeColumns (triangle_size); + } + + /** \brief Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh. + * \param[in] triangle_size edge length in pixels + * (Default: 1 = neighboring pixels are connected) + */ + inline void + setTrianglePixelSizeRows (int triangle_size) + { + triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1)); + } + + /** \brief Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh. + * \param[in] triangle_size edge length in pixels + * (Default: 1 = neighboring pixels are connected) + */ + inline void + setTrianglePixelSizeColumns (int triangle_size) + { + triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1)); + } + + /** \brief Set the triangulation type (see \a TriangulationType) + * \param[in] type quad mesh, triangle mesh with fixed left, right cut, + * or adaptive cut (splits a quad w.r.t. the depth (z) of the points) + */ + inline void + setTriangulationType (TriangulationType type) + { + triangulation_type_ = type; + } + + /** \brief Set the viewpoint from where the input point cloud has been acquired. + * \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data) + */ + inline void setViewpoint (const Eigen::Vector3f& viewpoint) + { + viewpoint_ = viewpoint; + } + + /** \brief Get the viewpoint from where the input point cloud has been acquired. */ + const inline Eigen::Vector3f& getViewpoint () const + { + return viewpoint_; + } + + /** \brief Store shadowed faces or not. + * \param[in] enable set to true to store shadowed faces + */ + inline void + storeShadowedFaces (bool enable) + { + store_shadowed_faces_ = enable; + } + + /** \brief Set the angle tolerance used for checking whether or not an edge is occluded. + * Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to + * disable the check for shadowed edges. + * \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable. + */ + inline void + setAngleTolerance(float angle_tolerance) + { + if (angle_tolerance > 0) + cos_angle_tolerance_ = std::abs (std::cos (angle_tolerance)); + else + cos_angle_tolerance_ = -1.0f; + } + + + inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false) + { + distance_tolerance_ = distance_tolerance; + if (distance_tolerance_ < 0) + return; + + distance_dependent_ = depth_dependent; + if (!distance_dependent_) + distance_tolerance_ *= distance_tolerance_; + } + + /** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint). + * \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */ + inline void useDepthAsDistance(bool enable) + { + use_depth_as_distance_ = enable; + } + + protected: + /** \brief max length of edge, scalar component */ + float max_edge_length_a_; + /** \brief max length of edge, scalar component */ + float max_edge_length_b_; + /** \brief max length of edge, scalar component */ + float max_edge_length_c_; + /** \brief flag whether or not edges are limited in length */ + bool max_edge_length_set_; + + /** \brief flag whether or not max edge length is distance dependent. */ + bool max_edge_length_dist_dependent_; + + /** \brief size of triangle edges (in pixels) for iterating over rows. */ + int triangle_pixel_size_rows_; + + /** \brief size of triangle edges (in pixels) for iterating over columns*/ + int triangle_pixel_size_columns_; + + /** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */ + TriangulationType triangulation_type_; + + /** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */ + Eigen::Vector3f viewpoint_; + + /** \brief Whether or not shadowed faces are stored, e.g., for exploration */ + bool store_shadowed_faces_; + + /** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */ + float cos_angle_tolerance_; + + /** \brief distance tolerance for filtering out shadowed/occluded edges */ + float distance_tolerance_; + + /** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */ + bool distance_dependent_; + + /** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint). + This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */ + bool use_depth_as_distance_; + + + /** \brief Perform the actual polygonal reconstruction. + * \param[out] polygons the resultant polygons + */ + void + reconstructPolygons (std::vector& polygons); + + /** \brief Create the surface. + * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. + */ + void + performReconstruction (std::vector &polygons) override; + + /** \brief Create the surface. + * + * Simply uses image indices to create an initial polygonal mesh for organized point clouds. + * \a indices_ are ignored! + * + * \param[out] output the resultant polygonal mesh + */ + void + performReconstruction (pcl::PolygonMesh &output) override; + + /** \brief Add a new triangle to the current polygon mesh + * \param[in] a index of the first vertex + * \param[in] b index of the second vertex + * \param[in] c index of the third vertex + * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons) + * \param[out] polygons the polygon mesh to be updated + */ + inline void + addTriangle (int a, int b, int c, int idx, std::vector& polygons) + { + assert (idx < static_cast (polygons.size ())); + polygons[idx].vertices.resize (3); + polygons[idx].vertices[0] = a; + polygons[idx].vertices[1] = b; + polygons[idx].vertices[2] = c; + } + + /** \brief Add a new quad to the current polygon mesh + * \param[in] a index of the first vertex + * \param[in] b index of the second vertex + * \param[in] c index of the third vertex + * \param[in] d index of the fourth vertex + * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons) + * \param[out] polygons the polygon mesh to be updated + */ + inline void + addQuad (int a, int b, int c, int d, int idx, std::vector& polygons) + { + assert (idx < static_cast (polygons.size ())); + polygons[idx].vertices.resize (4); + polygons[idx].vertices[0] = a; + polygons[idx].vertices[1] = b; + polygons[idx].vertices[2] = c; + polygons[idx].vertices[3] = d; + } + + /** \brief Set (all) coordinates of a particular point to the specified value + * \param[in] point_index index of point + * \param[out] mesh to modify + * \param[in] value value to use when re-setting + * \param[in] field_x_idx the X coordinate of the point + * \param[in] field_y_idx the Y coordinate of the point + * \param[in] field_z_idx the Z coordinate of the point + */ + inline void + resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f, + int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2) + { + float new_value = value; + memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float)); + memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float)); + memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float)); + } + + /** \brief Check if a point is shadowed by another point + * \param[in] point_a the first point + * \param[in] point_b the second point + */ + inline bool + isShadowed (const PointInT& point_a, const PointInT& point_b) + { + bool valid = true; + + Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap (); + Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap (); + float distance_to_points = dir_a.norm (); + float distance_between_points = dir_b.norm (); + + if (cos_angle_tolerance_ > 0) + { + float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points); + if (std::isnan(cos_angle)) + cos_angle = 1.0f; + bool check_angle = std::fabs (cos_angle) >= cos_angle_tolerance_; + + bool check_distance = true; + if (check_angle && (distance_tolerance_ > 0)) + { + float dist_thresh = distance_tolerance_; + if (distance_dependent_) + { + float d = distance_to_points; + if (use_depth_as_distance_) + d = std::max(point_a.z, point_b.z); + dist_thresh *= d*d; + dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false. + } + check_distance = (distance_between_points > dist_thresh); + } + valid = !(check_angle && check_distance); + } + + // check if max. edge length is not exceeded + if (max_edge_length_set_) + { + float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points); + float dist_thresh = max_edge_length_a_; + if (std::fabs(max_edge_length_b_) > std::numeric_limits::min()) + dist_thresh += max_edge_length_b_ * dist; + if (std::fabs(max_edge_length_c_) > std::numeric_limits::min()) + dist_thresh += max_edge_length_c_ * dist * dist; + valid = (distance_between_points <= dist_thresh); + } + + return !valid; + } + + /** \brief Check if a triangle is valid. + * \param[in] a index of the first vertex + * \param[in] b index of the second vertex + * \param[in] c index of the third vertex + */ + inline bool + isValidTriangle (const int& a, const int& b, const int& c) + { + if (!pcl::isFinite (input_->points[a])) return (false); + if (!pcl::isFinite (input_->points[b])) return (false); + if (!pcl::isFinite (input_->points[c])) return (false); + return (true); + } + + /** \brief Check if a triangle is shadowed. + * \param[in] a index of the first vertex + * \param[in] b index of the second vertex + * \param[in] c index of the third vertex + */ + inline bool + isShadowedTriangle (const int& a, const int& b, const int& c) + { + if (isShadowed (input_->points[a], input_->points[b])) return (true); + if (isShadowed (input_->points[b], input_->points[c])) return (true); + if (isShadowed (input_->points[c], input_->points[a])) return (true); + return (false); + } + + /** \brief Check if a quad is valid. + * \param[in] a index of the first vertex + * \param[in] b index of the second vertex + * \param[in] c index of the third vertex + * \param[in] d index of the fourth vertex + */ + inline bool + isValidQuad (const int& a, const int& b, const int& c, const int& d) + { + if (!pcl::isFinite (input_->points[a])) return (false); + if (!pcl::isFinite (input_->points[b])) return (false); + if (!pcl::isFinite (input_->points[c])) return (false); + if (!pcl::isFinite (input_->points[d])) return (false); + return (true); + } + + /** \brief Check if a triangle is shadowed. + * \param[in] a index of the first vertex + * \param[in] b index of the second vertex + * \param[in] c index of the third vertex + * \param[in] d index of the fourth vertex + */ + inline bool + isShadowedQuad (const int& a, const int& b, const int& c, const int& d) + { + if (isShadowed (input_->points[a], input_->points[b])) return (true); + if (isShadowed (input_->points[b], input_->points[c])) return (true); + if (isShadowed (input_->points[c], input_->points[d])) return (true); + if (isShadowed (input_->points[d], input_->points[a])) return (true); + return (false); + } + + /** \brief Create a quad mesh. + * \param[out] polygons the resultant mesh + */ + void + makeQuadMesh (std::vector& polygons); + + /** \brief Create a right cut mesh. + * \param[out] polygons the resultant mesh + */ + void + makeRightCutMesh (std::vector& polygons); + + /** \brief Create a left cut mesh. + * \param[out] polygons the resultant mesh + */ + void + makeLeftCutMesh (std::vector& polygons); + + /** \brief Create an adaptive cut mesh. + * \param[out] polygons the resultant mesh + */ + void + makeAdaptiveCutMesh (std::vector& polygons); + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/poisson.h b/deps_install/include/pcl-1.10/pcl/surface/poisson.h new file mode 100755 index 0000000..2aefc1b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/poisson.h @@ -0,0 +1,258 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include + +namespace pcl +{ + namespace poisson + { + class CoredVectorMeshData; + template struct Point3D; + } + + /** \brief The Poisson surface reconstruction algorithm. + * \note Code adapted from Misha Kazhdan: http://www.cs.jhu.edu/~misha/Code/PoissonRecon/ + * \note Based on the paper: + * * Michael Kazhdan, Matthew Bolitho, Hugues Hoppe, "Poisson surface reconstruction", + * SGP '06 Proceedings of the fourth Eurographics symposium on Geometry processing + * \author Alexandru-Eugen Ichim + * \ingroup surface + */ + template + class Poisson : public SurfaceReconstruction + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using SurfaceReconstruction::input_; + using SurfaceReconstruction::tree_; + + using PointCloudPtr = typename pcl::PointCloud::Ptr; + + using KdTree = pcl::KdTree; + using KdTreePtr = typename KdTree::Ptr; + + /** \brief Constructor that sets all the parameters to working default values. */ + Poisson (); + + /** \brief Destructor. */ + ~Poisson (); + + /** \brief Create the surface. + * \param[out] output the resultant polygonal mesh + */ + void + performReconstruction (pcl::PolygonMesh &output) override; + + /** \brief Create the surface. + * \param[out] points the vertex positions of the resulting mesh + * \param[out] polygons the connectivity of the resulting mesh + */ + void + performReconstruction (pcl::PointCloud &points, + std::vector &polygons) override; + + /** \brief Set the maximum depth of the tree that will be used for surface reconstruction. + * \note Running at depth d corresponds to solving on a voxel grid whose resolution is no larger than + * 2^d x 2^d x 2^d. Note that since the reconstructor adapts the octree to the sampling density, the specified + * reconstruction depth is only an upper bound. + * \param[in] depth the depth parameter + */ + inline void + setDepth (int depth) { depth_ = depth; } + + /** \brief Get the depth parameter */ + inline int + getDepth () { return depth_; } + + inline void + setMinDepth (int min_depth) { min_depth_ = min_depth; } + + inline int + getMinDepth () { return min_depth_; } + + inline void + setPointWeight (float point_weight) { point_weight_ = point_weight; } + + inline float + getPointWeight () { return point_weight_; } + + /** \brief Set the ratio between the diameter of the cube used for reconstruction and the diameter of the + * samples' bounding cube. + * \param[in] scale the given parameter value + */ + inline void + setScale (float scale) { scale_ = scale; } + + /** Get the ratio between the diameter of the cube used for reconstruction and the diameter of the + * samples' bounding cube. + */ + inline float + getScale () { return scale_; } + + /** \brief Set the the depth at which a block Gauss-Seidel solver is used to solve the Laplacian equation + * \note Using this parameter helps reduce the memory overhead at the cost of a small increase in + * reconstruction time. (In practice, we have found that for reconstructions of depth 9 or higher a subdivide + * depth of 7 or 8 can greatly reduce the memory usage.) + * \param[in] solver_divide the given parameter value + */ + inline void + setSolverDivide (int solver_divide) { solver_divide_ = solver_divide; } + + /** \brief Get the depth at which a block Gauss-Seidel solver is used to solve the Laplacian equation */ + inline int + getSolverDivide () { return solver_divide_; } + + /** \brief Set the depth at which a block iso-surface extractor should be used to extract the iso-surface + * \note Using this parameter helps reduce the memory overhead at the cost of a small increase in extraction + * time. (In practice, we have found that for reconstructions of depth 9 or higher a subdivide depth of 7 or 8 + * can greatly reduce the memory usage.) + * \param[in] iso_divide the given parameter value + */ + inline void + setIsoDivide (int iso_divide) { iso_divide_ = iso_divide; } + + /** \brief Get the depth at which a block iso-surface extractor should be used to extract the iso-surface */ + inline int + getIsoDivide () { return iso_divide_; } + + /** \brief Set the minimum number of sample points that should fall within an octree node as the octree + * construction is adapted to sampling density + * \note For noise-free samples, small values in the range [1.0 - 5.0] can be used. For more noisy samples, + * larger values in the range [15.0 - 20.0] may be needed to provide a smoother, noise-reduced, reconstruction. + * \param[in] samples_per_node the given parameter value + */ + inline void + setSamplesPerNode (float samples_per_node) { samples_per_node_ = samples_per_node; } + + /** \brief Get the minimum number of sample points that should fall within an octree node as the octree + * construction is adapted to sampling density + */ + inline float + getSamplesPerNode () { return samples_per_node_; } + + /** \brief Set the confidence flag + * \note Enabling this flag tells the reconstructor to use the size of the normals as confidence information. + * When the flag is not enabled, all normals are normalized to have unit-length prior to reconstruction. + * \param[in] confidence the given flag + */ + inline void + setConfidence (bool confidence) { confidence_ = confidence; } + + /** \brief Get the confidence flag */ + inline bool + getConfidence () { return confidence_; } + + /** \brief Enabling this flag tells the reconstructor to output a polygon mesh (rather than triangulating the + * results of Marching Cubes). + * \param[in] output_polygons the given flag + */ + inline void + setOutputPolygons (bool output_polygons) { output_polygons_ = output_polygons; } + + /** \brief Get whether the algorithm outputs a polygon mesh or a triangle mesh */ + inline bool + getOutputPolygons () { return output_polygons_; } + + /** \brief Set the degree parameter + * \param[in] degree the given degree + */ + inline void + setDegree (int degree) { degree_ = degree; } + + /** \brief Get the degree parameter */ + inline int + getDegree () { return degree_; } + + /** \brief Set the manifold flag. + * \note Enabling this flag tells the reconstructor to add the polygon barycenter when triangulating polygons + * with more than three vertices. + * \param[in] manifold the given flag + */ + inline void + setManifold (bool manifold) { manifold_ = manifold; } + + /** \brief Get the manifold flag */ + inline bool + getManifold () { return manifold_; } + + protected: + /** \brief Class get name method. */ + std::string + getClassName () const override { return ("Poisson"); } + + private: + int depth_; + int min_depth_; + float point_weight_; + float scale_; + int solver_divide_; + int iso_divide_; + float samples_per_node_; + bool confidence_; + bool output_polygons_; + + bool no_reset_samples_; + bool no_clip_tree_; + bool manifold_; + + int refine_; + int kernel_depth_; + int degree_; + bool non_adaptive_weights_; + bool show_residual_; + int min_iterations_; + float solver_accuracy_; + + template void + execute (poisson::CoredVectorMeshData &mesh, + poisson::Point3D &translate, + float &scale); + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/processing.h b/deps_install/include/pcl-1.10/pcl/surface/processing.h new file mode 100755 index 0000000..a8d9a0c --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/processing.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + /** \brief @b CloudSurfaceProcessing represents the base class for algorithms that takes a point cloud as input and + * produces a new output cloud that has been modified towards a better surface representation. These types of + * algorithms include surface smoothing, hole filling, cloud upsampling etc. + * + * \author Alexandru E. Ichim + * \ingroup surface + */ + template + class CloudSurfaceProcessing : public PCLBase + { + public: + typedef shared_ptr > Ptr; + typedef shared_ptr > ConstPtr; + + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + public: + /** \brief Constructor. */ + CloudSurfaceProcessing () : PCLBase () + {}; + + /** \brief Empty destructor */ + ~CloudSurfaceProcessing () {} + + /** \brief Process the input cloud and store the results + * \param[out] output the cloud where the results will be stored + */ + virtual void + process (pcl::PointCloud &output); + + protected: + /** \brief Abstract cloud processing method */ + virtual void + performProcessing (pcl::PointCloud &output) = 0; + + }; + + + /** \brief @b MeshProcessing represents the base class for mesh processing algorithms. + * \author Alexandru E. Ichim + * \ingroup surface + */ + class PCL_EXPORTS MeshProcessing + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using PolygonMeshConstPtr = PolygonMesh::ConstPtr; + + /** \brief Constructor. */ + MeshProcessing () {} + + /** \brief Destructor. */ + virtual ~MeshProcessing () {} + + /** \brief Set the input mesh that we want to process + * \param[in] input the input polygonal mesh + */ + inline void + setInputMesh (const pcl::PolygonMeshConstPtr &input) + { input_mesh_ = input; } + + /** \brief Get the input mesh to be processed + * \returns the mesh + */ + inline pcl::PolygonMeshConstPtr + getInputMesh () const + { return input_mesh_; } + + /** \brief Process the input surface mesh and store the results + * \param[out] output the resultant processed surface model + */ + void + process (pcl::PolygonMesh &output); + + protected: + /** \brief Initialize computation. Must be called before processing starts. */ + virtual bool + initCompute (); + + /** \brief UnInitialize computation. Must be called after processing ends. */ + virtual void + deinitCompute (); + + /** \brief Abstract surface processing method. */ + virtual void + performProcessing (pcl::PolygonMesh &output) = 0; + + /** \brief Abstract class get name method. */ + virtual std::string + getClassName () const + { return (""); } + + /** \brief Input polygonal mesh. */ + pcl::PolygonMeshConstPtr input_mesh_; + }; +} + +#include "pcl/surface/impl/processing.hpp" diff --git a/deps_install/include/pcl-1.10/pcl/surface/qhull.h b/deps_install/include/pcl-1.10/pcl/surface/qhull.h new file mode 100755 index 0000000..39eee73 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/qhull.h @@ -0,0 +1,72 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: concave_hull.h 5027 2012-03-12 03:10:45Z rusu $ + * + */ + +#pragma once + +#include +#ifdef HAVE_QHULL + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +extern "C" +{ +#ifdef HAVE_QHULL_2011 +# include "libqhull/libqhull.h" +# include "libqhull/mem.h" +# include "libqhull/qset.h" +# include "libqhull/geom.h" +# include "libqhull/merge.h" +# include "libqhull/poly.h" +# include "libqhull/io.h" +# include "libqhull/stat.h" +#else +# include "qhull/qhull.h" +# include "qhull/mem.h" +# include "qhull/qset.h" +# include "qhull/geom.h" +# include "qhull/merge.h" +# include "qhull/poly.h" +# include "qhull/io.h" +# include "qhull/stat.h" +#endif +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/reconstruction.h b/deps_install/include/pcl-1.10/pcl/surface/reconstruction.h new file mode 100755 index 0000000..fb7436d --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/reconstruction.h @@ -0,0 +1,245 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + /** \brief Pure abstract class. All types of meshing/reconstruction + * algorithms in \b libpcl_surface must inherit from this, in order to make + * sure we have a consistent API. The methods that we care about here are: + * + * - \b setSearchMethod(&SearchPtr): passes a search locator + * - \b reconstruct(&PolygonMesh): creates a PolygonMesh object from the input data + * + * \author Radu B. Rusu, Michael Dixon, Alexandru E. Ichim + */ + template + class PCLSurfaceBase: public PCLBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using KdTree = pcl::search::Search; + using KdTreePtr = typename KdTree::Ptr; + + /** \brief Empty constructor. */ + PCLSurfaceBase () : tree_ () {} + + /** \brief Empty destructor */ + ~PCLSurfaceBase () {} + + /** \brief Provide an optional pointer to a search object. + * \param[in] tree a pointer to the spatial search object. + */ + inline void + setSearchMethod (const KdTreePtr &tree) + { + tree_ = tree; + } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr + getSearchMethod () { return (tree_); } + + /** \brief Base method for surface reconstruction for all points given in + * + * \param[out] output the resultant reconstructed surface model + */ + virtual void + reconstruct (pcl::PolygonMesh &output) = 0; + + protected: + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief Abstract class get name method. */ + virtual std::string + getClassName () const { return (""); } + }; + + /** \brief SurfaceReconstruction represents a base surface reconstruction + * class. All \b surface reconstruction methods take in a point cloud and + * generate a new surface from it, by either re-sampling the data or + * generating new data altogether. These methods are thus \b not preserving + * the topology of the original data. + * + * \note Reconstruction methods that always preserve the original input + * point cloud data as the surface vertices and simply construct the mesh on + * top should inherit from \ref MeshConstruction. + * + * \author Radu B. Rusu, Michael Dixon, Alexandru E. Ichim + * \ingroup surface + */ + template + class SurfaceReconstruction: public PCLSurfaceBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PCLSurfaceBase::input_; + using PCLSurfaceBase::indices_; + using PCLSurfaceBase::initCompute; + using PCLSurfaceBase::deinitCompute; + using PCLSurfaceBase::tree_; + using PCLSurfaceBase::getClassName; + + /** \brief Constructor. */ + SurfaceReconstruction () : check_tree_ (true) {} + + /** \brief Destructor. */ + ~SurfaceReconstruction () {} + + /** \brief Base method for surface reconstruction for all points given in + * + * \param[out] output the resultant reconstructed surface model + */ + void + reconstruct (pcl::PolygonMesh &output) override; + + /** \brief Base method for surface reconstruction for all points given in + * + * \param[out] points the resultant points lying on the new surface + * \param[out] polygons the resultant polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + */ + virtual void + reconstruct (pcl::PointCloud &points, + std::vector &polygons); + + protected: + /** \brief A flag specifying whether or not the derived reconstruction + * algorithm needs the search object \a tree.*/ + bool check_tree_; + + /** \brief Abstract surface reconstruction method. + * \param[out] output the output polygonal mesh + */ + virtual void + performReconstruction (pcl::PolygonMesh &output) = 0; + + /** \brief Abstract surface reconstruction method. + * \param[out] points the resultant points lying on the surface + * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. + */ + virtual void + performReconstruction (pcl::PointCloud &points, + std::vector &polygons) = 0; + }; + + /** \brief MeshConstruction represents a base surface reconstruction + * class. All \b mesh constructing methods that take in a point cloud and + * generate a surface that uses the original data as vertices should inherit + * from this class. + * + * \note Reconstruction methods that generate a new surface or create new + * vertices in locations different than the input data should inherit from + * \ref SurfaceReconstruction. + * + * \author Radu B. Rusu, Michael Dixon, Alexandru E. Ichim + * \ingroup surface + */ + template + class MeshConstruction: public PCLSurfaceBase + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PCLSurfaceBase::input_; + using PCLSurfaceBase::indices_; + using PCLSurfaceBase::initCompute; + using PCLSurfaceBase::deinitCompute; + using PCLSurfaceBase::tree_; + using PCLSurfaceBase::getClassName; + + /** \brief Constructor. */ + MeshConstruction () : check_tree_ (true) {} + + /** \brief Destructor. */ + ~MeshConstruction () {} + + /** \brief Base method for surface reconstruction for all points given in + * + * \param[out] output the resultant reconstructed surface model + * + * \note This method copies the input point cloud data from + * PointCloud to PCLPointCloud2, and is implemented here for backwards + * compatibility only! + * + */ + void + reconstruct (pcl::PolygonMesh &output) override; + + /** \brief Base method for mesh construction for all points given in + * + * \param[out] polygons the resultant polygons, as a set of + * vertices. The Vertices structure contains an array of point indices. + */ + virtual void + reconstruct (std::vector &polygons); + + protected: + /** \brief A flag specifying whether or not the derived reconstruction + * algorithm needs the search object \a tree.*/ + bool check_tree_; + + /** \brief Abstract surface reconstruction method. + * \param[out] output the output polygonal mesh + */ + virtual void + performReconstruction (pcl::PolygonMesh &output) = 0; + + /** \brief Abstract surface reconstruction method. + * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices. + */ + virtual void + performReconstruction (std::vector &polygons) = 0; + }; +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/surface/simplification_remove_unused_vertices.h b/deps_install/include/pcl-1.10/pcl/surface/simplification_remove_unused_vertices.h new file mode 100755 index 0000000..0616104 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/simplification_remove_unused_vertices.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Dirk Holz, University of Bonn. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace surface + { + class PCL_EXPORTS SimplificationRemoveUnusedVertices + { + public: + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Constructor. */ + SimplificationRemoveUnusedVertices () {}; + /** \brief Destructor. */ + ~SimplificationRemoveUnusedVertices () {}; + + /** \brief Simply a polygonal mesh. + * \param[in] input the input mesh + * \param[out] output the output mesh + */ + inline void + simplify (const pcl::PolygonMesh& input, pcl::PolygonMesh& output) + { + std::vector indices; + simplify (input, output, indices); + } + + /** \brief Perform simplification (remove unused vertices). + * \param[in] input the input mesh + * \param[out] output the output mesh + * \param[out] indices the resultant vector of indices + */ + void + simplify (const pcl::PolygonMesh& input, pcl::PolygonMesh& output, std::vector& indices); + + }; + } +} diff --git a/deps_install/include/pcl-1.10/pcl/surface/surfel_smoothing.h b/deps_install/include/pcl-1.10/pcl/surface/surfel_smoothing.h new file mode 100755 index 0000000..3f93510 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/surfel_smoothing.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Alexandru-Eugen Ichim + * Willow Garage, Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#pragma once + +#include +#include + +namespace pcl +{ + template + class SurfelSmoothing : public PCLBase + { + using PCLBase::input_; + using PCLBase::initCompute; + + public: + typedef shared_ptr > Ptr; + typedef shared_ptr > ConstPtr; + + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename pcl::PointCloud::Ptr; + using NormalCloud = pcl::PointCloud; + using NormalCloudPtr = typename pcl::PointCloud::Ptr; + using CloudKdTree = pcl::search::Search; + using CloudKdTreePtr = typename pcl::search::Search::Ptr; + + SurfelSmoothing (float a_scale = 0.01) + : PCLBase () + , scale_ (a_scale) + , scale_squared_ (a_scale * a_scale) + , normals_ () + , interm_cloud_ () + , interm_normals_ () + , tree_ () + { + } + + void + setInputNormals (NormalCloudPtr &a_normals) { normals_ = a_normals; }; + + void + setSearchMethod (const CloudKdTreePtr &a_tree) { tree_ = a_tree; }; + + bool + initCompute (); + + float + smoothCloudIteration (PointCloudInPtr &output_positions, + NormalCloudPtr &output_normals); + + void + computeSmoothedCloud (PointCloudInPtr &output_positions, + NormalCloudPtr &output_normals); + + + void + smoothPoint (std::size_t &point_index, + PointT &output_point, + PointNT &output_normal); + + void + extractSalientFeaturesBetweenScales (PointCloudInPtr &cloud2, + NormalCloudPtr &cloud2_normals, + pcl::IndicesPtr &output_features); + + private: + float scale_, scale_squared_; + NormalCloudPtr normals_; + + PointCloudInPtr interm_cloud_; + NormalCloudPtr interm_normals_; + + CloudKdTreePtr tree_; + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/surface/texture_mapping.h b/deps_install/include/pcl-1.10/pcl/surface/texture_mapping.h new file mode 100755 index 0000000..59ea2cf --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/surface/texture_mapping.h @@ -0,0 +1,424 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include +#include + + +namespace pcl +{ + namespace texture_mapping + { + + /** \brief Structure to store camera pose and focal length. + * + * One can assign a value to focal_length, to be used along + * both camera axes or, optionally, axis-specific values + * (focal_length_w and focal_length_h). Optionally, one can + * also specify center-of-focus using parameters + * center_w and center_h. If the center-of-focus is not + * specified, it will be set to the geometric center of + * the camera, as defined by the width and height parameters. + */ + struct Camera + { + Camera () : focal_length (), focal_length_w (-1), focal_length_h (-1), + center_w (-1), center_h (-1), height (), width () {} + Eigen::Affine3f pose; + double focal_length; + double focal_length_w; // optional + double focal_length_h; // optinoal + double center_w; // optional + double center_h; // optional + double height; + double width; + std::string texture_file; + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief Structure that links a uv coordinate to its 3D point and face. + */ + struct UvIndex + { + UvIndex () : idx_cloud (), idx_face () {} + int idx_cloud; // Index of the PointXYZ in the camera's cloud + int idx_face; // Face corresponding to that projection + }; + + using CameraVector = std::vector >; + + } + + /** \brief The texture mapping algorithm + * \author Khai Tran, Raphael Favier + * \ingroup surface + */ + template + class TextureMapping + { + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using Octree = pcl::octree::OctreePointCloudSearch; + using OctreePtr = typename Octree::Ptr; + using OctreeConstPtr = typename Octree::ConstPtr; + + using Camera = pcl::texture_mapping::Camera; + using UvIndex = pcl::texture_mapping::UvIndex; + + /** \brief Constructor. */ + TextureMapping () : + f_ () + { + } + + /** \brief Destructor. */ + ~TextureMapping () + { + } + + /** \brief Set mesh scale control + * \param[in] f + */ + inline void + setF (float f) + { + f_ = f; + } + + /** \brief Set vector field + * \param[in] x data point x + * \param[in] y data point y + * \param[in] z data point z + */ + inline void + setVectorField (float x, float y, float z) + { + vector_field_ = Eigen::Vector3f (x, y, z); + // normalize vector field + vector_field_ /= std::sqrt (vector_field_.dot (vector_field_)); + } + + /** \brief Set texture files + * \param[in] tex_files list of texture files + */ + inline void + setTextureFiles (std::vector tex_files) + { + tex_files_ = tex_files; + } + + /** \brief Set texture materials + * \param[in] tex_material texture material + */ + inline void + setTextureMaterials (TexMaterial tex_material) + { + tex_material_ = tex_material; + } + + /** \brief Map texture to a mesh synthesis algorithm + * \param[in] tex_mesh texture mesh + */ + void + mapTexture2Mesh (pcl::TextureMesh &tex_mesh); + + /** \brief Map texture to a mesh UV mapping + * \param[in] tex_mesh texture mesh + */ + void + mapTexture2MeshUV (pcl::TextureMesh &tex_mesh); + + /** \brief Map textures acquired from a set of cameras onto a mesh. + * \details With UV mapping, the mesh must be divided into NbCamera + 1 sub-meshes. + * Each sub-mesh corresponding to the faces visible by one camera. The last submesh containing all non-visible faces + * \param[in] tex_mesh texture mesh + * \param[in] cams cameras used for UV mapping + */ + void + mapMultipleTexturesToMeshUV (pcl::TextureMesh &tex_mesh, + pcl::texture_mapping::CameraVector &cams); + + /** \brief computes UV coordinates of point, observed by one particular camera + * \param[in] pt XYZ point to project on camera plane + * \param[in] cam the camera used for projection + * \param[out] UV_coordinates the resulting uv coordinates. Set to (-1.0,-1.0) if the point is not visible by the camera + * \returns false if the point is not visible by the camera + */ + inline bool + getPointUVCoordinates (const PointInT &pt, const Camera &cam, Eigen::Vector2f &UV_coordinates) + { + // if the point is in front of the camera + if (pt.z > 0) + { + // compute image center and dimension + double sizeX = cam.width; + double sizeY = cam.height; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = (sizeX) / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = (sizeY) / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h>0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; + + // project point on image frame + UV_coordinates[0] = static_cast ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal + UV_coordinates[1] = 1.0f - static_cast (((focal_y * (pt.y / pt.z) + cy) / sizeY)); //vertical + + // point is visible! + if (UV_coordinates[0] >= 0.0 && UV_coordinates[0] <= 1.0 && UV_coordinates[1] >= 0.0 && UV_coordinates[1] + <= 1.0) + return (true); + } + + // point is NOT visible by the camera + UV_coordinates[0] = -1.0; + UV_coordinates[1] = -1.0; + return (false); + } + + /** \brief Check if a point is occluded using raycasting on octree. + * \param[in] pt XYZ from which the ray will start (toward the camera) + * \param[in] octree the octree used for raycasting. It must be initialized with a cloud transformed into the camera's frame + * \returns true if the point is occluded. + */ + inline bool + isPointOccluded (const PointInT &pt, const OctreePtr octree); + + /** \brief Remove occluded points from a point cloud + * \param[in] input_cloud the cloud on which to perform occlusion detection + * \param[out] filtered_cloud resulting cloud, containing only visible points + * \param[in] octree_voxel_size octree resolution (in meters) + * \param[out] visible_indices will contain indices of visible points + * \param[out] occluded_indices will contain indices of occluded points + */ + void + removeOccludedPoints (const PointCloudPtr &input_cloud, + PointCloudPtr &filtered_cloud, const double octree_voxel_size, + std::vector &visible_indices, std::vector &occluded_indices); + + /** \brief Remove occluded points from a textureMesh + * \param[in] tex_mesh input mesh, on witch to perform occlusion detection + * \param[out] cleaned_mesh resulting mesh, containing only visible points + * \param[in] octree_voxel_size octree resolution (in meters) + */ + void + removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size); + + + /** \brief Remove occluded points from a textureMesh + * \param[in] tex_mesh input mesh, on witch to perform occlusion detection + * \param[out] filtered_cloud resulting cloud, containing only visible points + * \param[in] octree_voxel_size octree resolution (in meters) + */ + void + removeOccludedPoints (const pcl::TextureMesh &tex_mesh, PointCloudPtr &filtered_cloud, const double octree_voxel_size); + + + /** \brief Segment faces by camera visibility. Point-based segmentation. + * \details With N camera, faces will be arranged into N+1 groups: 1 for each camera, plus 1 for faces not visible from any camera. + * \param[in] tex_mesh input mesh that needs sorting. Must contain only 1 sub-mesh. + * \param[in] sorted_mesh resulting mesh, will contain nbCamera + 1 sub-mesh. + * \param[in] cameras vector containing the cameras used for texture mapping. + * \param[in] octree_voxel_size octree resolution (in meters) + * \param[out] visible_pts cloud containing only visible points + */ + int + sortFacesByCamera (pcl::TextureMesh &tex_mesh, + pcl::TextureMesh &sorted_mesh, + const pcl::texture_mapping::CameraVector &cameras, + const double octree_voxel_size, PointCloud &visible_pts); + + /** \brief Colors a point cloud, depending on its occlusions. + * \details If showNbOcclusions is set to True, each point is colored depending on the number of points occluding it. + * Else, each point is given a different a 0 value is not occluded, 1 if occluded. + * By default, the number of occlusions is bounded to 4. + * \param[in] input_cloud input cloud on which occlusions will be computed. + * \param[out] colored_cloud resulting colored cloud showing the number of occlusions per point. + * \param[in] octree_voxel_size octree resolution (in meters). + * \param[in] show_nb_occlusions If false, color information will only represent. + * \param[in] max_occlusions Limit the number of occlusions per point. + */ + void + showOcclusions (const PointCloudPtr &input_cloud, + pcl::PointCloud::Ptr &colored_cloud, + const double octree_voxel_size, + const bool show_nb_occlusions = true, + const int max_occlusions = 4); + + /** \brief Colors the point cloud of a Mesh, depending on its occlusions. + * \details If showNbOcclusions is set to True, each point is colored depending on the number of points occluding it. + * Else, each point is given a different a 0 value is not occluded, 1 if occluded. + * By default, the number of occlusions is bounded to 4. + * \param[in] tex_mesh input mesh on which occlusions will be computed. + * \param[out] colored_cloud resulting colored cloud showing the number of occlusions per point. + * \param[in] octree_voxel_size octree resolution (in meters). + * \param[in] show_nb_occlusions If false, color information will only represent. + * \param[in] max_occlusions Limit the number of occlusions per point. + */ + void + showOcclusions (pcl::TextureMesh &tex_mesh, + pcl::PointCloud::Ptr &colored_cloud, + double octree_voxel_size, + bool show_nb_occlusions = true, + int max_occlusions = 4); + + /** \brief Segment and texture faces by camera visibility. Face-based segmentation. + * \details With N camera, faces will be arranged into N+1 groups: 1 for each camera, plus 1 for faces not visible from any camera. + * The mesh will also contain uv coordinates for each face + * \param mesh input mesh that needs sorting. Should contain only 1 sub-mesh. + * \param[in] cameras vector containing the cameras used for texture mapping. + */ + void + textureMeshwithMultipleCameras (pcl::TextureMesh &mesh, + const pcl::texture_mapping::CameraVector &cameras); + + protected: + /** \brief mesh scale control. */ + float f_; + + /** \brief vector field */ + Eigen::Vector3f vector_field_; + + /** \brief list of texture files */ + std::vector tex_files_; + + /** \brief list of texture materials */ + TexMaterial tex_material_; + + /** \brief Map texture to a face + * \param[in] p1 the first point + * \param[in] p2 the second point + * \param[in] p3 the third point + */ + std::vector > + mapTexture2Face (const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3); + + /** \brief Returns the circumcenter of a triangle and the circle's radius. + * \details see http://en.wikipedia.org/wiki/Circumcenter for formulas. + * \param[in] p1 first point of the triangle. + * \param[in] p2 second point of the triangle. + * \param[in] p3 third point of the triangle. + * \param[out] circumcenter resulting circumcenter + * \param[out] radius the radius of the circumscribed circle. + */ + inline void + getTriangleCircumcenterAndSize (const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius); + + + /** \brief Returns the centroid of a triangle and the corresponding circumscribed circle's radius. + * \details yield a tighter circle than getTriangleCircumcenterAndSize. + * \param[in] p1 first point of the triangle. + * \param[in] p2 second point of the triangle. + * \param[in] p3 third point of the triangle. + * \param[out] circumcenter resulting circumcenter + * \param[out] radius the radius of the circumscribed circle. + */ + inline void + getTriangleCircumcscribedCircleCentroid ( const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius); + + + /** \brief computes UV coordinates of point, observed by one particular camera + * \param[in] pt XYZ point to project on camera plane + * \param[in] cam the camera used for projection + * \param[out] UV_coordinates the resulting UV coordinates. Set to (-1.0,-1.0) if the point is not visible by the camera + * \returns false if the point is not visible by the camera + */ + inline bool + getPointUVCoordinates (const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates); + + /** \brief Returns true if all the vertices of one face are projected on the camera's image plane. + * \param[in] camera camera on which to project the face. + * \param[in] p1 first point of the face. + * \param[in] p2 second point of the face. + * \param[in] p3 third point of the face. + * \param[out] proj1 UV coordinates corresponding to p1. + * \param[out] proj2 UV coordinates corresponding to p2. + * \param[out] proj3 UV coordinates corresponding to p3. + */ + inline bool + isFaceProjected (const Camera &camera, + const PointInT &p1, const PointInT &p2, const PointInT &p3, + pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3); + + /** \brief Returns True if a point lays within a triangle + * \details see http://www.blackpawn.com/texts/pointinpoly/default.html + * \param[in] p1 first point of the triangle. + * \param[in] p2 second point of the triangle. + * \param[in] p3 third point of the triangle. + * \param[in] pt the querry point. + */ + inline bool + checkPointInsideTriangle (const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt); + + /** \brief Class get name method. */ + std::string + getClassName () const + { + return ("TextureMapping"); + } + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; +} diff --git a/deps_install/include/pcl-1.10/pcl/tracking/approx_nearest_pair_point_cloud_coherence.h b/deps_install/include/pcl-1.10/pcl/tracking/approx_nearest_pair_point_cloud_coherence.h new file mode 100755 index 0000000..3fa7290 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/approx_nearest_pair_point_cloud_coherence.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include +namespace pcl +{ + namespace tracking + { + /** \brief @b ApproxNearestPairPointCloudCoherence computes coherence between two pointclouds using the + approximate nearest point pairs. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class ApproxNearestPairPointCloudCoherence: public NearestPairPointCloudCoherence + { + public: + using PointCoherencePtr = typename NearestPairPointCloudCoherence::PointCoherencePtr; + using PointCloudInConstPtr = typename NearestPairPointCloudCoherence::PointCloudInConstPtr; + //using NearestPairPointCloudCoherence::search_; + using NearestPairPointCloudCoherence::maximum_distance_; + using NearestPairPointCloudCoherence::target_input_; + using NearestPairPointCloudCoherence::point_coherences_; + using NearestPairPointCloudCoherence::coherence_name_; + using NearestPairPointCloudCoherence::new_target_; + using NearestPairPointCloudCoherence::getClassName; + + /** \brief empty constructor */ + ApproxNearestPairPointCloudCoherence () : + NearestPairPointCloudCoherence (), search_ () + { + coherence_name_ = "ApproxNearestPairPointCloudCoherence"; + } + + protected: + /** \brief This method should get called before starting the actual computation. */ + bool initCompute () override; + + /** \brief compute the nearest pairs and compute coherence using point_coherences_ */ + void + computeCoherence (const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w_j) override; + + typename pcl::search::Octree::Ptr search_; + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/coherence.h b/deps_install/include/pcl-1.10/pcl/tracking/coherence.h new file mode 100755 index 0000000..49dfe58 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/coherence.h @@ -0,0 +1,131 @@ +#pragma once + +#include + +namespace pcl +{ + + namespace tracking + { + + /** \brief @b PointCoherence is a base class to compute coherence between the two points. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class PointCoherence + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + public: + /** \brief empty constructor */ + PointCoherence () {} + + /** \brief empty distructor */ + virtual ~PointCoherence () {} + + /** \brief compute coherence from the source point to the target point. + * \param source instance of source point. + * \param target instance of target point. + */ + inline double + compute (PointInT &source, PointInT &target); + + protected: + + /** \brief The coherence name. */ + std::string coherence_name_; + + /** \brief abstract method to calculate coherence. + * \param[in] source instance of source point. + * \param[in] target instance of target point. + */ + virtual double + computeCoherence (PointInT &source, PointInT &target) = 0; + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const { return (coherence_name_); } + + }; + + /** \brief @b PointCloudCoherence is a base class to compute coherence between the two PointClouds. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class PointCloudCoherence + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCoherencePtr = typename PointCoherence::Ptr; + /** \brief Constructor. */ + PointCloudCoherence () : target_input_ (), point_coherences_ () {} + + /** \brief Destructor. */ + virtual ~PointCloudCoherence () {} + + /** \brief compute coherence between two pointclouds. */ + inline void + compute (const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, + float &w_i); + + /** \brief get a list of pcl::tracking::PointCoherence.*/ + inline std::vector + getPointCoherences () { return point_coherences_; } + + /** \brief set a list of pcl::tracking::PointCoherence. + * \param coherences a list of pcl::tracking::PointCoherence. + */ + inline void + setPointCoherences (std::vector coherences) { point_coherences_ = coherences; } + + /** \brief This method should get called before starting the actual computation. */ + virtual bool initCompute (); + + /** \brief add a PointCoherence to the PointCloudCoherence. + * \param coherence a pointer to PointCoherence. + */ + inline void + addPointCoherence (PointCoherencePtr coherence) { point_coherences_.push_back (coherence); } + + /** \brief add a PointCoherence to the PointCloudCoherence. + * \param cloud a pointer to PointCoherence. + */ + virtual inline void + setTargetCloud (const PointCloudInConstPtr &cloud) { target_input_ = cloud; } + + protected: + /** \brief Abstract method to compute coherence. */ + virtual void + computeCoherence (const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w_j) = 0; + + inline double calcPointCoherence (PointInT &source, PointInT &target); + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const { return (coherence_name_); } + + + /** \brief The coherence name. */ + std::string coherence_name_; + + /** \brief a pointer to target point cloud*/ + PointCloudInConstPtr target_input_; + + /** \brief a list of pointers to PointCoherence.*/ + std::vector point_coherences_; + }; + + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/tracking/distance_coherence.h b/deps_install/include/pcl-1.10/pcl/tracking/distance_coherence.h new file mode 100755 index 0000000..de5a1aa --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/distance_coherence.h @@ -0,0 +1,54 @@ +#pragma once + +#include + +#include + +namespace pcl +{ + namespace tracking + { + /** \brief @b DistanceCoherence computes coherence between two points from the distance + between them. the coherence is calculated by 1 / (1 + weight * d^2 ). + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class DistanceCoherence: public PointCoherence + { + public: + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr>; + + /** \brief initialize the weight to 1.0. */ + DistanceCoherence () + : PointCoherence () + , weight_ (1.0) + {} + + /** \brief set the weight of coherence. + * \param weight the value of the wehgit. + */ + inline void setWeight (double weight) { weight_ = weight; } + + /** \brief get the weight of coherence.*/ + inline double getWeight () { return weight_; } + + protected: + + /** \brief return the distance coherence between the two points. + * \param source instance of source point. + * \param target instance of target point. + */ + double computeCoherence (PointInT &source, PointInT &target) override; + + /** \brief the weight of coherence.*/ + double weight_; + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/hsv_color_coherence.h b/deps_install/include/pcl-1.10/pcl/tracking/hsv_color_coherence.h new file mode 100755 index 0000000..69b73a7 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/hsv_color_coherence.h @@ -0,0 +1,101 @@ +#pragma once + +#include + +namespace pcl +{ + namespace tracking + { + /** \brief @b HSVColorCoherence computes coherence between the two points from + the color difference between them. the color difference is calculated in HSV color space. + the coherence is calculated by 1 / ( 1 + w * (w_h^2 * h_diff^2 + w_s^2 * s_diff^2 + w_v^2 * v_diff^2)) + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class HSVColorCoherence: public PointCoherence + { + public: + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief initialize the weights of the computation. + weight_, h_weight_, s_weight_ default to 1.0 and + v_weight_ defaults to 0.0. + */ + HSVColorCoherence () + : PointCoherence () + , weight_ (1.0) + , h_weight_ (1.0) + , s_weight_ (1.0) + , v_weight_ (0.0) + {} + + /** \brief set the weight of coherence + * \param[in] weight the weight of coherence. + */ + inline void + setWeight (double weight) { weight_ = weight; } + + /** \brief get the weight (w) of coherence */ + inline double + getWeight () { return weight_; } + + /** \brief set the hue weight (w_h) of coherence + * \param[in] weight the hue weight (w_h) of coherence. + */ + inline void + setHWeight (double weight) { h_weight_ = weight; } + + /** \brief get the hue weight (w_h) of coherence */ + inline double + getHWeight () { return h_weight_; } + + /** \brief set the saturation weight (w_s) of coherence + * \param[in] weight the saturation weight (w_s) of coherence. + */ + inline void + setSWeight (double weight) { s_weight_ = weight; } + + /** \brief get the saturation weight (w_s) of coherence */ + inline double + getSWeight () { return s_weight_; } + + /** \brief set the value weight (w_v) of coherence + * \param[in] weight the value weight (w_v) of coherence. + */ + inline void + setVWeight (double weight) { v_weight_ = weight; } + + /** \brief get the value weight (w_v) of coherence */ + inline double + getVWeight () { return v_weight_; } + + protected: + + /** \brief return the color coherence between the two points. + * \param[in] source instance of source point. + * \param[in] target instance of target point. + */ + double + computeCoherence (PointInT &source, PointInT &target) override; + + /** \brief the weight of coherence (w) */ + double weight_; + + /** \brief the hue weight (w_h) */ + double h_weight_; + + /** \brief the saturation weight (w_s) */ + double s_weight_; + + /** \brief the value weight (w_v) */ + double v_weight_; + + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp new file mode 100755 index 0000000..896ab4a --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp @@ -0,0 +1,68 @@ +#ifndef PCL_TRACKING_IMPL_APPROX_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_ +#define PCL_TRACKING_IMPL_APPROX_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_ + +#include +#include + +namespace pcl +{ + namespace tracking + { + template void + ApproxNearestPairPointCloudCoherence::computeCoherence ( + const PointCloudInConstPtr &cloud, const IndicesConstPtr &, float &w) + { + double val = 0.0; + //for (std::size_t i = 0; i < indices->size (); i++) + for (std::size_t i = 0; i < cloud->points.size (); i++) + { + int k_index = 0; + float k_distance = 0.0; + //PointInT input_point = cloud->points[(*indices)[i]]; + PointInT input_point = cloud->points[i]; + search_->approxNearestSearch(input_point, k_index, k_distance); + if (k_distance < maximum_distance_ * maximum_distance_) + { + PointInT target_point = target_input_->points[k_index]; + double coherence_val = 1.0; + for (std::size_t i = 0; i < point_coherences_.size (); i++) + { + PointCoherencePtr coherence = point_coherences_[i]; + double w = coherence->compute (input_point, target_point); + coherence_val *= w; + } + val += coherence_val; + } + } + w = - static_cast (val); + } + + template bool + ApproxNearestPairPointCloudCoherence::initCompute () + { + if (!PointCloudCoherence::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n", getClassName ().c_str ()); + //deinitCompute (); + return (false); + } + + // initialize tree + if (!search_) + search_.reset (new pcl::search::Octree (0.01)); + + if (new_target_ && target_input_) + { + search_->setInputCloud (target_input_); + new_target_ = false; + } + + return true; + } + + } +} + +#define PCL_INSTANTIATE_ApproxNearestPairPointCloudCoherence(T) template class PCL_EXPORTS pcl::tracking::ApproxNearestPairPointCloudCoherence; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/coherence.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/coherence.hpp new file mode 100755 index 0000000..e7dc5ae --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/coherence.hpp @@ -0,0 +1,61 @@ +#ifndef PCL_TRACKING_IMPL_COHERENCE_H_ +#define PCL_TRACKING_IMPL_COHERENCE_H_ + +#include +#include + +namespace pcl +{ + namespace tracking + { + + template double + PointCoherence::compute (PointInT &source, PointInT &target) + { + return computeCoherence (source, target); + } + + template double + PointCloudCoherence::calcPointCoherence (PointInT &source, PointInT &target) + { + double val = 0.0; + for (std::size_t i = 0; i < point_coherences_.size (); i++) + { + PointCoherencePtr coherence = point_coherences_[i]; + double d = std::log(coherence->compute (source, target)); + //double d = coherence->compute (source, target); + if (! std::isnan(d)) + val += d; + else + PCL_WARN ("nan!\n"); + } + return val; + } + + template bool + PointCloudCoherence::initCompute () + { + if (!target_input_ || target_input_->points.empty ()) + { + PCL_ERROR ("[pcl::%s::compute] target_input_ is empty!\n", getClassName ().c_str ()); + return false; + } + + return true; + + } + + template void + PointCloudCoherence::compute (const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w) + { + if (!initCompute ()) + { + PCL_ERROR ("[pcl::%s::compute] Init failed.\n", getClassName ().c_str ()); + return; + } + computeCoherence (cloud, indices, w); + } + } +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/distance_coherence.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/distance_coherence.hpp new file mode 100755 index 0000000..7b68e9e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/distance_coherence.hpp @@ -0,0 +1,24 @@ +#ifndef PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_ +#define PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_ + +#include +#include + +namespace pcl +{ + namespace tracking + { + template double + DistanceCoherence::computeCoherence (PointInT &source, PointInT &target) + { + Eigen::Vector4f p = source.getVector4fMap (); + Eigen::Vector4f p_dash = target.getVector4fMap (); + double d = (p - p_dash).norm (); + return 1.0 / (1.0 + d * d * weight_); + } + } +} + +#define PCL_INSTANTIATE_DistanceCoherence(T) template class PCL_EXPORTS pcl::tracking::DistanceCoherence; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/hsv_color_coherence.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/hsv_color_coherence.hpp new file mode 100755 index 0000000..5301cc1 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/hsv_color_coherence.hpp @@ -0,0 +1,184 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_ +#define PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_ + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +namespace pcl +{ + namespace tracking + { + // utility + union RGBValue + { + struct /*anonymous*/ + { + unsigned char Blue; // Blue channel + unsigned char Green; // Green channel + unsigned char Red; // Red channel + }; + float float_value; + int int_value; + }; + + /** \brief Convert a RGB tuple to an HSV one. + * \param[in] r the input Red component + * \param[in] g the input Green component + * \param[in] b the input Blue component + * \param[out] fh the output Hue component + * \param[out] fs the output Saturation component + * \param[out] fv the output Value component + */ + void + RGB2HSV (int r, int g, int b, float& fh, float& fs, float& fv) + { + // mostly copied from opencv-svn/modules/imgproc/src/color.cpp + // revision is 4351 + const int hsv_shift = 12; + + static const int div_table[] = + { + 0, 1044480, 522240, 348160, 261120, 208896, 174080, 149211, + 130560, 116053, 104448, 94953, 87040, 80345, 74606, 69632, + 65280, 61440, 58027, 54973, 52224, 49737, 47476, 45412, + 43520, 41779, 40172, 38684, 37303, 36017, 34816, 33693, + 32640, 31651, 30720, 29842, 29013, 28229, 27486, 26782, + 26112, 25475, 24869, 24290, 23738, 23211, 22706, 22223, + 21760, 21316, 20890, 20480, 20086, 19707, 19342, 18991, + 18651, 18324, 18008, 17703, 17408, 17123, 16846, 16579, + 16320, 16069, 15825, 15589, 15360, 15137, 14921, 14711, + 14507, 14308, 14115, 13926, 13743, 13565, 13391, 13221, + 13056, 12895, 12738, 12584, 12434, 12288, 12145, 12006, + 11869, 11736, 11605, 11478, 11353, 11231, 11111, 10995, + 10880, 10768, 10658, 10550, 10445, 10341, 10240, 10141, + 10043, 9947, 9854, 9761, 9671, 9582, 9495, 9410, + 9326, 9243, 9162, 9082, 9004, 8927, 8852, 8777, + 8704, 8632, 8561, 8492, 8423, 8356, 8290, 8224, + 8160, 8097, 8034, 7973, 7913, 7853, 7795, 7737, + 7680, 7624, 7569, 7514, 7461, 7408, 7355, 7304, + 7253, 7203, 7154, 7105, 7057, 7010, 6963, 6917, + 6872, 6827, 6782, 6739, 6695, 6653, 6611, 6569, + 6528, 6487, 6447, 6408, 6369, 6330, 6292, 6254, + 6217, 6180, 6144, 6108, 6073, 6037, 6003, 5968, + 5935, 5901, 5868, 5835, 5803, 5771, 5739, 5708, + 5677, 5646, 5615, 5585, 5556, 5526, 5497, 5468, + 5440, 5412, 5384, 5356, 5329, 5302, 5275, 5249, + 5222, 5196, 5171, 5145, 5120, 5095, 5070, 5046, + 5022, 4998, 4974, 4950, 4927, 4904, 4881, 4858, + 4836, 4813, 4791, 4769, 4748, 4726, 4705, 4684, + 4663, 4642, 4622, 4601, 4581, 4561, 4541, 4522, + 4502, 4483, 4464, 4445, 4426, 4407, 4389, 4370, + 4352, 4334, 4316, 4298, 4281, 4263, 4246, 4229, + 4212, 4195, 4178, 4161, 4145, 4128, 4112, 4096 + }; + int hr = 180, hscale = 15; + int h, s, v = b; + int vmin = b, diff; + int vr, vg; + + v = std::max (v, g); + v = std::max (v, r); + vmin = std::min (vmin, g); + vmin = std::min (vmin, r); + + diff = v - vmin; + vr = v == r ? -1 : 0; + vg = v == g ? -1 : 0; + + s = diff * div_table[v] >> hsv_shift; + h = (vr & (g - b)) + + (~vr & ((vg & (b - r + 2 * diff)) + + ((~vg) & (r - g + 4 * diff)))); + h = (h * div_table[diff] * hscale + + (1 << (hsv_shift + 6))) >> (7 + hsv_shift); + + h += h < 0 ? hr : 0; + fh = static_cast (h) / 180.0f; + fs = static_cast (s) / 255.0f; + fv = static_cast (v) / 255.0f; + } + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template double + HSVColorCoherence::computeCoherence (PointInT &source, PointInT &target) + { + // convert color space from RGB to HSV + RGBValue source_rgb, target_rgb; + source_rgb.int_value = source.rgba; + target_rgb.int_value = target.rgba; + + float source_h, source_s, source_v, target_h, target_s, target_v; + RGB2HSV (source_rgb.Red, source_rgb.Blue, source_rgb.Green, + source_h, source_s, source_v); + RGB2HSV (target_rgb.Red, target_rgb.Blue, target_rgb.Green, + target_h, target_s, target_v); + // hue value is in 0 ~ 2pi, but circulated. + const float _h_diff = std::abs (source_h - target_h); + // Also need to compute distance other way around circle - but need to check which is closer to 0 + float _h_diff2; + if (source_h < target_h) + _h_diff2 = std::abs (1.0f + source_h - target_h); //Add 2pi to source, subtract target + else + _h_diff2 = std::abs (1.0f + target_h - source_h); //Add 2pi to target, subtract source + + float h_diff; + //Now we need to choose the smaller distance + if (_h_diff < _h_diff2) + h_diff = static_cast (h_weight_) * _h_diff * _h_diff; + else + h_diff = static_cast (h_weight_) * _h_diff2 * _h_diff2; + + const float s_diff = static_cast (s_weight_) * (source_s - target_s) * (source_s - target_s); + const float v_diff = static_cast (v_weight_) * (source_v - target_v) * (source_v - target_v); + const float diff2 = h_diff + s_diff + v_diff; + + return (1.0 / (1.0 + weight_ * diff2)); + } + } +} + +#define PCL_INSTANTIATE_HSVColorCoherence(T) template class PCL_EXPORTS pcl::tracking::HSVColorCoherence; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/kld_adaptive_particle_filter.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/kld_adaptive_particle_filter.hpp new file mode 100755 index 0000000..e4f6836 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/kld_adaptive_particle_filter.hpp @@ -0,0 +1,94 @@ +#ifndef PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_ +#define PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_FILTER_H_ + +#include + +template bool +pcl::tracking::KLDAdaptiveParticleFilterTracker::initCompute () +{ + if (!Tracker::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + if (transed_reference_vector_.empty ()) + { + // only one time allocation + transed_reference_vector_.resize (maximum_particle_number_); + for (unsigned int i = 0; i < maximum_particle_number_; i++) + { + transed_reference_vector_[i] = PointCloudInPtr (new PointCloudIn ()); + } + } + + coherence_->setTargetCloud (input_); + + if (!change_detector_) + change_detector_.reset (new pcl::octree::OctreePointCloudChangeDetector (change_detector_resolution_)); + + if (!particles_ || particles_->points.empty ()) + initParticles (true); + return (true); +} + +template bool +pcl::tracking::KLDAdaptiveParticleFilterTracker::insertIntoBins +(std::vector &&new_bin, std::vector > &bins) +{ + for (auto &existing_bin : bins) + { + if (equalBin (new_bin, existing_bin)) + return false; + } + bins.push_back (std::move(new_bin)); + return true; +} + +template void +pcl::tracking::KLDAdaptiveParticleFilterTracker::resample () +{ + unsigned int k = 0; + unsigned int n = 0; + PointCloudStatePtr S (new PointCloudState); + std::vector > bins; + + // initializing for sampling without replacement + std::vector a (particles_->points.size ()); + std::vector q (particles_->points.size ()); + this->genAliasTable (a, q, particles_); + + const std::vector zero_mean (StateT::stateDimension (), 0.0); + + // select the particles with KLD sampling + do + { + int j_n = sampleWithReplacement (a, q); + StateT x_t = particles_->points[j_n]; + x_t.sample (zero_mean, step_noise_covariance_); + + // motion + if (rand () / double (RAND_MAX) < motion_ratio_) + x_t = x_t + motion_; + + S->points.push_back (x_t); + // calc bin + std::vector new_bin (StateT::stateDimension ()); + for (int i = 0; i < StateT::stateDimension (); i++) + new_bin[i] = static_cast (x_t[i] / bin_size_[i]); + + // calc bin index... how? + if (insertIntoBins (std::move(new_bin), bins)) + ++k; + ++n; + } + while (n < maximum_particle_number_ && (k < 2 || n < calcKLBound (k))); + + particles_ = S; // swap + particle_num_ = static_cast (particles_->points.size ()); +} + + +#define PCL_INSTANTIATE_KLDAdaptiveParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterTracker; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp new file mode 100755 index 0000000..e1fe863 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp @@ -0,0 +1,104 @@ +#ifndef PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_OMP_FILTER_H_ +#define PCL_TRACKING_IMPL_KLD_ADAPTIVE_PARTICLE_OMP_FILTER_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::KLDAdaptiveParticleFilterOMPTracker::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::KLDAdaptiveParticleFilterOMPTracker::weight () +{ + if (!use_normal_) + { +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]); + + PointCloudInPtr coherence_input (new PointCloudIn); + this->cropInputPointCloud (input_, *coherence_input); + if (change_counter_ == 0) + { + // test change detector + if (!use_change_detector_ || this->testChangeDetection (coherence_input)) + { + changed_ = true; + change_counter_ = change_detector_interval_; + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + IndicesPtr indices; + coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); + } + } + else + changed_ = false; + } + else + { + --change_counter_; + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + IndicesPtr indices; + coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); + } + } + } + else + { + std::vector indices_list (particle_num_); + for (int i = 0; i < particle_num_; i++) + { + indices_list[i] = IndicesPtr (new std::vector); + } +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]); + } + + PointCloudInPtr coherence_input (new PointCloudIn); + this->cropInputPointCloud (input_, *coherence_input); + + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight); + } + } + + normalizeWeight (); +} + +#define PCL_INSTANTIATE_KLDAdaptiveParticleFilterOMPTracker(T,ST) template class PCL_EXPORTS pcl::tracking::KLDAdaptiveParticleFilterOMPTracker; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/nearest_pair_point_cloud_coherence.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/nearest_pair_point_cloud_coherence.hpp new file mode 100755 index 0000000..1614d56 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/nearest_pair_point_cloud_coherence.hpp @@ -0,0 +1,71 @@ +#ifndef PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_ +#define PCL_TRACKING_IMPL_NEAREST_PAIR_POINT_CLOUD_COHERENCE_H_ + +#include +#include +#include + +namespace pcl +{ + namespace tracking + { + template void + NearestPairPointCloudCoherence::computeCoherence ( + const PointCloudInConstPtr &cloud, const IndicesConstPtr &, float &w) + { + double val = 0.0; + //for (std::size_t i = 0; i < indices->size (); i++) + for (std::size_t i = 0; i < cloud->points.size (); i++) + { + PointInT input_point = cloud->points[i]; + std::vector k_indices(1); + std::vector k_distances(1); + search_->nearestKSearch (input_point, 1, k_indices, k_distances); + int k_index = k_indices[0]; + float k_distance = k_distances[0]; + if (k_distance < maximum_distance_ * maximum_distance_) + { + // nearest_targets.push_back (k_index); + // nearest_inputs.push_back (i); + PointInT target_point = target_input_->points[k_index]; + double coherence_val = 1.0; + for (std::size_t i = 0; i < point_coherences_.size (); i++) + { + PointCoherencePtr coherence = point_coherences_[i]; + double w = coherence->compute (input_point, target_point); + coherence_val *= w; + } + val += coherence_val; + } + } + w = - static_cast (val); + } + + template bool + NearestPairPointCloudCoherence::initCompute () + { + if (!PointCloudCoherence::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] PointCloudCoherence::Init failed.\n", getClassName ().c_str ()); + //deinitCompute (); + return (false); + } + + // initialize tree + if (!search_) + search_.reset (new pcl::search::KdTree (false)); + + if (new_target_ && target_input_) + { + search_->setInputCloud (target_input_); + new_target_ = false; + } + + return true; + } + } +} + +#define PCL_INSTANTIATE_NearestPairPointCloudCoherence(T) template class PCL_EXPORTS pcl::tracking::NearestPairPointCloudCoherence; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/normal_coherence.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/normal_coherence.hpp new file mode 100755 index 0000000..9f32d4b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/normal_coherence.hpp @@ -0,0 +1,32 @@ +#ifndef PCL_TRACKING_IMPL_NORMAL_COHERENCE_H_ +#define PCL_TRACKING_IMPL_NORMAL_COHERENCE_H_ + +#include +#include +#include + +template double +pcl::tracking::NormalCoherence::computeCoherence (PointInT &source, PointInT &target) +{ + Eigen::Vector4f n = source.getNormalVector4fMap (); + Eigen::Vector4f n_dash = target.getNormalVector4fMap (); + if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 ) + { + PCL_ERROR("norm might be ZERO!\n"); + std::cout << "source: " << source << std::endl; + std::cout << "target: " << target << std::endl; + exit (1); + return 0.0; + } + n.normalize (); + n_dash.normalize (); + double theta = pcl::getAngle3D (n, n_dash); + if (!std::isnan (theta)) + return 1.0 / (1.0 + weight_ * theta * theta); + return 0.0; +} + + +#define PCL_INSTANTIATE_NormalCoherence(T) template class PCL_EXPORTS pcl::tracking::NormalCoherence; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/particle_filter.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/particle_filter.hpp new file mode 100755 index 0000000..caea903 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/particle_filter.hpp @@ -0,0 +1,411 @@ +#ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_ +#define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_ + +#include + +#include +#include +#include +#include + +template bool +pcl::tracking::ParticleFilterTracker::initCompute () +{ + if (!Tracker::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + if (transed_reference_vector_.empty ()) + { + // only one time allocation + transed_reference_vector_.resize (particle_num_); + for (int i = 0; i < particle_num_; i++) + { + transed_reference_vector_[i] = PointCloudInPtr (new PointCloudIn ()); + } + } + + coherence_->setTargetCloud (input_); + + if (!change_detector_) + change_detector_.reset (new pcl::octree::OctreePointCloudChangeDetector (change_detector_resolution_)); + + if (!particles_ || particles_->points.empty ()) + initParticles (true); + return (true); +} + +template int +pcl::tracking::ParticleFilterTracker::sampleWithReplacement +(const std::vector& a, const std::vector& q) +{ + static std::mt19937 rng([] { std::random_device rd; return rd(); } ()); + std::uniform_real_distribution<> rd (0.0, 1.0); + + double rU = rd (rng) * static_cast (particles_->points.size ()); + int k = static_cast (rU); + rU -= k; /* rU - [rU] */ + if ( rU < q[k] ) + return k; + return a[k]; +} + +template void +pcl::tracking::ParticleFilterTracker::genAliasTable (std::vector &a, std::vector &q, + const PointCloudStateConstPtr &particles) +{ + /* generate an alias table, a and q */ + std::vector HL (particles->points.size ()); + std::vector::iterator H = HL.begin (); + std::vector::iterator L = HL.end () - 1; + std::size_t num = particles->points.size (); + for ( std::size_t i = 0; i < num; i++ ) + q[i] = particles->points[i].weight * static_cast (num); + for ( std::size_t i = 0; i < num; i++ ) + a[i] = static_cast (i); + // setup H and L + for ( std::size_t i = 0; i < num; i++ ) + if ( q[i] >= 1.0 ) + *H++ = static_cast (i); + else + *L-- = static_cast (i); + + while ( H != HL.begin() && L != HL.end() - 1 ) + { + int j = *(L + 1); + int k = *(H - 1); + a[j] = k; + q[k] += q[j] - 1; + ++L; + if ( q[k] < 1.0 ) + { + *L-- = k; + --H; + } + } +} + +template void +pcl::tracking::ParticleFilterTracker::initParticles (bool reset) +{ + particles_.reset (new PointCloudState ()); + if (reset) + { + representative_state_.zero (); + StateT offset = StateT::toState (trans_); + representative_state_ = offset; + representative_state_.weight = 1.0f / static_cast (particle_num_); + } + + // sampling... + for ( int i = 0; i < particle_num_; i++ ) + { + StateT p; + p.zero (); + p.sample (initial_noise_mean_, initial_noise_covariance_); + p = p + representative_state_; + p.weight = 1.0f / static_cast (particle_num_); + particles_->points.push_back (p); // update + } +} + +template void +pcl::tracking::ParticleFilterTracker::normalizeWeight () +{ + // apply exponential function + double w_min = std::numeric_limits::max (); + double w_max = - std::numeric_limits::max (); + for ( std::size_t i = 0; i < particles_->points.size (); i++ ) + { + double weight = particles_->points[i].weight; + if (w_min > weight) + w_min = weight; + if (weight != 0.0 && w_max < weight) + w_max = weight; + } + + fit_ratio_ = w_min; + if (w_max != w_min) + { + for ( std::size_t i = 0; i < particles_->points.size (); i++ ) + { + if (particles_->points[i].weight != 0.0) + { + particles_->points[i].weight = static_cast (normalizeParticleWeight (particles_->points[i].weight, w_min, w_max)); + } + } + } + else + { + for ( std::size_t i = 0; i < particles_->points.size (); i++ ) + particles_->points[i].weight = 1.0f / static_cast (particles_->points.size ()); + } + + double sum = 0.0; + for ( std::size_t i = 0; i < particles_->points.size (); i++ ) + { + sum += particles_->points[i].weight; + } + + if (sum != 0.0) + { + for ( std::size_t i = 0; i < particles_->points.size (); i++ ) + particles_->points[i].weight /= static_cast (sum); + } + else + { + for ( std::size_t i = 0; i < particles_->points.size (); i++ ) + particles_->points[i].weight = 1.0f / static_cast (particles_->points.size ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::ParticleFilterTracker::cropInputPointCloud ( + const PointCloudInConstPtr &, PointCloudIn &output) +{ + double x_min, y_min, z_min, x_max, y_max, z_max; + calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max); + pass_x_.setFilterLimits (float (x_min), float (x_max)); + pass_y_.setFilterLimits (float (y_min), float (y_max)); + pass_z_.setFilterLimits (float (z_min), float (z_max)); + + // x + PointCloudInPtr xcloud (new PointCloudIn); + pass_x_.setInputCloud (input_); + pass_x_.filter (*xcloud); + // y + PointCloudInPtr ycloud (new PointCloudIn); + pass_y_.setInputCloud (xcloud); + pass_y_.filter (*ycloud); + // z + pass_z_.setInputCloud (ycloud); + pass_z_.filter (output); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::ParticleFilterTracker::calcBoundingBox ( + double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max) +{ + x_min = y_min = z_min = std::numeric_limits::max (); + x_max = y_max = z_max = - std::numeric_limits::max (); + + for (std::size_t i = 0; i < transed_reference_vector_.size (); i++) + { + PointCloudInPtr target = transed_reference_vector_[i]; + PointInT Pmin, Pmax; + pcl::getMinMax3D (*target, Pmin, Pmax); + if (x_min > Pmin.x) + x_min = Pmin.x; + if (x_max < Pmax.x) + x_max = Pmax.x; + if (y_min > Pmin.y) + y_min = Pmin.y; + if (y_max < Pmax.y) + y_max = Pmax.y; + if (z_min > Pmin.z) + z_min = Pmin.z; + if (z_max < Pmax.z) + z_max = Pmax.z; + } +} + +template bool +pcl::tracking::ParticleFilterTracker::testChangeDetection +(const PointCloudInConstPtr &input) +{ + change_detector_->setInputCloud (input); + change_detector_->addPointsFromInputCloud (); + std::vector newPointIdxVector; + change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_); + change_detector_->switchBuffers (); + return !newPointIdxVector.empty (); +} + +template void +pcl::tracking::ParticleFilterTracker::weight () +{ + if (!use_normal_) + { + for (std::size_t i = 0; i < particles_->points.size (); i++) + { + computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]); + } + + PointCloudInPtr coherence_input (new PointCloudIn); + cropInputPointCloud (input_, *coherence_input); + + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); + for (std::size_t i = 0; i < particles_->points.size (); i++) + { + IndicesPtr indices; + coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); + } + } + else + { + for (std::size_t i = 0; i < particles_->points.size (); i++) + { + IndicesPtr indices (new std::vector); + computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]); + } + + PointCloudInPtr coherence_input (new PointCloudIn); + cropInputPointCloud (input_, *coherence_input); + + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); + for (std::size_t i = 0; i < particles_->points.size (); i++) + { + IndicesPtr indices (new std::vector); + coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); + } + } + + normalizeWeight (); +} + +template void +pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud +(const StateT& hypothesis, std::vector& indices, PointCloudIn &cloud) +{ + if (use_normal_) + computeTransformedPointCloudWithNormal (hypothesis, indices, cloud); + else + computeTransformedPointCloudWithoutNormal (hypothesis, cloud); +} + +template void +pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithoutNormal +(const StateT& hypothesis, PointCloudIn &cloud) +{ + const Eigen::Affine3f trans = toEigenMatrix (hypothesis); + // destructively assigns to cloud + pcl::transformPointCloud (*ref_, cloud, trans); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal ( +#ifdef PCL_TRACKING_NORMAL_SUPPORTED + const StateT& hypothesis, std::vector& indices, PointCloudIn &cloud) +#else + const StateT&, std::vector&, PointCloudIn &) +#endif +{ +#ifdef PCL_TRACKING_NORMAL_SUPPORTED + const Eigen::Affine3f trans = toEigenMatrix (hypothesis); + // destructively assigns to cloud + pcl::transformPointCloudWithNormals (*ref_, cloud, trans); + for ( std::size_t i = 0; i < cloud.points.size (); i++ ) + { + PointInT input_point = cloud.points[i]; + + if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) || !std::isfinite(input_point.z)) + continue; + // take occlusion into account + Eigen::Vector4f p = input_point.getVector4fMap (); + Eigen::Vector4f n = input_point.getNormalVector4fMap (); + double theta = pcl::getAngle3D (p, n); + if ( theta > occlusion_angle_thr_ ) + indices.push_back (i); + } +#else + PCL_WARN ("[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.", + getClassName ().c_str ()); +#endif +} + +template void +pcl::tracking::ParticleFilterTracker::resample () +{ + resampleWithReplacement (); +} + +template void +pcl::tracking::ParticleFilterTracker::resampleWithReplacement () +{ + std::vector a (particles_->points.size ()); + std::vector q (particles_->points.size ()); + genAliasTable (a, q, particles_); + + const std::vector zero_mean (StateT::stateDimension (), 0.0); + // memoize the original list of particles + PointCloudStatePtr origparticles = particles_; + particles_->points.clear (); + // the first particle, it is a just copy of the maximum result + StateT p = representative_state_; + particles_->points.push_back (p); + + // with motion + int motion_num = static_cast (particles_->points.size ()) * static_cast (motion_ratio_); + for ( int i = 1; i < motion_num; i++ ) + { + int target_particle_index = sampleWithReplacement (a, q); + StateT p = origparticles->points[target_particle_index]; + // add noise using gaussian + p.sample (zero_mean, step_noise_covariance_); + p = p + motion_; + particles_->points.push_back (p); + } + + // no motion + for ( int i = motion_num; i < particle_num_; i++ ) + { + int target_particle_index = sampleWithReplacement (a, q); + StateT p = origparticles->points[target_particle_index]; + // add noise using gaussian + p.sample (zero_mean, step_noise_covariance_); + particles_->points.push_back (p); + } +} + +template void +pcl::tracking::ParticleFilterTracker::update () +{ + + StateT orig_representative = representative_state_; + representative_state_.zero (); + representative_state_.weight = 0.0; + for ( std::size_t i = 0; i < particles_->points.size (); i++) + { + StateT p = particles_->points[i]; + representative_state_ = representative_state_ + p * p.weight; + } + representative_state_.weight = 1.0f / static_cast (particles_->points.size ()); + motion_ = representative_state_ - orig_representative; +} + +template void +pcl::tracking::ParticleFilterTracker::computeTracking () +{ + + for (int i = 0; i < iteration_num_; i++) + { + if (changed_) + { + resample (); + } + + weight (); // likelihood is called in it + + if (changed_) + { + update (); + } + } + + // if ( getResult ().weight < resample_likelihood_thr_ ) + // { + // PCL_WARN ("too small likelihood, re-initializing...\n"); + // initParticles (false); + // } +} + +#define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/particle_filter_omp.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/particle_filter_omp.hpp new file mode 100755 index 0000000..bfc100b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/particle_filter_omp.hpp @@ -0,0 +1,104 @@ +#ifndef PCL_TRACKING_IMPL_PARTICLE_OMP_FILTER_H_ +#define PCL_TRACKING_IMPL_PARTICLE_OMP_FILTER_H_ + +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::ParticleFilterOMPTracker::setNumberOfThreads (unsigned int nr_threads) +{ + if (nr_threads == 0) +#ifdef _OPENMP + threads_ = omp_get_num_procs(); +#else + threads_ = 1; +#endif + else + threads_ = nr_threads; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::ParticleFilterOMPTracker::weight () +{ + if (!use_normal_) + { +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]); + + PointCloudInPtr coherence_input (new PointCloudIn); + this->cropInputPointCloud (input_, *coherence_input); + if (change_counter_ == 0) + { + // test change detector + if (!use_change_detector_ || this->testChangeDetection (coherence_input)) + { + changed_ = true; + change_counter_ = change_detector_interval_; + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + IndicesPtr indices; // dummy + coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); + } + } + else + changed_ = false; + } + else + { + --change_counter_; + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + IndicesPtr indices; // dummy + coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight); + } + } + } + else + { + std::vector indices_list (particle_num_); + for (int i = 0; i < particle_num_; i++) + { + indices_list[i] = IndicesPtr (new std::vector); + } +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]); + } + + PointCloudInPtr coherence_input (new PointCloudIn); + this->cropInputPointCloud (input_, *coherence_input); + + coherence_->setTargetCloud (coherence_input); + coherence_->initCompute (); +#ifdef _OPENMP +#pragma omp parallel for num_threads(threads_) +#endif + for (int i = 0; i < particle_num_; i++) + { + coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight); + } + } + + normalizeWeight (); +} + +#define PCL_INSTANTIATE_ParticleFilterOMPTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterOMPTracker; + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/pyramidal_klt.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/pyramidal_klt.hpp new file mode 100755 index 0000000..16887e4 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/pyramidal_klt.hpp @@ -0,0 +1,640 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP +#define PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::tracking::PyramidalKLTTracker::setTrackingWindowSize (int width, int height) +{ + track_width_ = width; + track_height_ = height; +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::tracking::PyramidalKLTTracker::setPointsToTrack (const pcl::PointCloud::ConstPtr& keypoints) +{ + if (keypoints->size () <= keypoints_nbr_) + keypoints_ = keypoints; + else + { + pcl::PointCloud::Ptr p (new pcl::PointCloud); + p->reserve (keypoints_nbr_); + for (std::size_t i = 0; i < keypoints_nbr_; ++i) + p->push_back (keypoints->points[i]); + keypoints_ = p; + } + + keypoints_status_.reset (new pcl::PointIndices); + keypoints_status_->indices.resize (keypoints_->size (), 0); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template inline void +pcl::tracking::PyramidalKLTTracker::setPointsToTrack (const pcl::PointIndicesConstPtr& points) +{ + assert ((input_ || ref_) && "[pcl::tracking::PyramidalKLTTracker] CALL setInputCloud FIRST!"); + + pcl::PointCloud::Ptr keypoints (new pcl::PointCloud); + keypoints->reserve (keypoints_nbr_); + for (std::size_t i = 0; i < keypoints_nbr_; ++i) + { + pcl::PointUV uv; + uv.u = points->indices[i] % input_->width; + uv.v = points->indices[i] / input_->width; + keypoints->push_back (uv); + } + setPointsToTrack (keypoints); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::tracking::PyramidalKLTTracker::initCompute () +{ + // std::cout << ">>> [PyramidalKLTTracker::initCompute]" << std::endl; + if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::tracking::%s::initCompute] PCLBase::Init failed.\n", + tracker_name_.c_str ()); + return (false); + } + + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!", + tracker_name_.c_str ()); + return (false); + } + + if (!keypoints_ || keypoints_->empty ()) + { + PCL_ERROR ("[pcl::tracking::%s::initCompute] No keypoints aborting!", + tracker_name_.c_str ()); + return (false); + } + + // This is the first call + if (!ref_) + { + ref_ = input_; + // std::cout << "First run!!!" << std::endl; + + if ((track_height_ * track_width_)%2 == 0) + { + PCL_ERROR ("[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be odd!\n", + tracker_name_.c_str (), track_width_, track_height_); + return (false); + } + + if (track_height_ < 3 || track_width_ < 3) + { + PCL_ERROR ("[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be >= 3x3!\n", + tracker_name_.c_str (), track_width_, track_height_); + return (false); + } + + track_width_2_ = track_width_ / 2; + track_height_2_ = track_height_ / 2; + + if (nb_levels_ < 2) + { + PCL_ERROR ("[pcl::tracking::%s::initCompute] Number of pyramid levels should be at least 2!", + tracker_name_.c_str ()); + return (false); + } + + if (nb_levels_ > 5) + { + PCL_ERROR ("[pcl::tracking::%s::initCompute] Number of pyramid levels should not exceed 5!", + tracker_name_.c_str ()); + return (false); + } + + computePyramids (ref_, ref_pyramid_, pcl::BORDER_REFLECT_101); + return (true); + } + initialized_ = true; + + return (true); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const +{ + // std::cout << ">>> derivatives" << std::endl; + //////////////////////////////////////////////////////// + // Use Shcarr operator to compute derivatives. // + // Vertical kernel +3 +10 +3 = [1 0 -1]T * [3 10 3] // + // 0 0 0 // + // -3 -10 -3 // + // Horizontal kernel +3 0 -3 = [3 10 3]T * [1 0 -1] // + // +10 0 -10 // + // +3 0 -3 // + //////////////////////////////////////////////////////// + if (grad_x.size () != src.size () || grad_x.width != src.width || grad_x.height != src.height) + grad_x = FloatImage (src.width, src.height); + if (grad_y.size () != src.size () || grad_y.width != src.width || grad_y.height != src.height) + grad_y = FloatImage (src.width, src.height); + + int height = src.height, width = src.width; + float *row0 = new float [src.width + 2]; + float *row1 = new float [src.width + 2]; + float *trow0 = row0; ++trow0; + float *trow1 = row1; ++trow1; + const float* src_ptr = &(src.points[0]); + + for (int y = 0; y < height; y++) + { + const float* srow0 = src_ptr + (y > 0 ? y-1 : height > 1 ? 1 : 0) * width; + const float* srow1 = src_ptr + y * width; + const float* srow2 = src_ptr + (y < height-1 ? y+1 : height > 1 ? height-2 : 0) * width; + float* grad_x_row = &(grad_x.points[y * width]); + float* grad_y_row = &(grad_y.points[y * width]); + + // do vertical convolution + for (int x = 0; x < width; x++) + { + trow0[x] = (srow0[x] + srow2[x])*3 + srow1[x]*10; + trow1[x] = srow2[x] - srow0[x]; + } + + // make border + int x0 = width > 1 ? 1 : 0, x1 = width > 1 ? width-2 : 0; + trow0[-1] = trow0[x0]; trow0[width] = trow0[x1]; + trow1[-1] = trow1[x0]; trow1[width] = trow1[x1]; + + // do horizontal convolution and store results + for (int x = 0; x < width; x++) + { + grad_x_row[x] = trow0[x+1] - trow0[x-1]; + grad_y_row[x] = (trow1[x+1] + trow1[x-1])*3 + trow1[x]*10; + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::downsample (const FloatImageConstPtr& input, + FloatImageConstPtr& output) const +{ + FloatImage smoothed (input->width, input->height); + convolve (input, smoothed); + + int width = (smoothed.width +1) / 2; + int height = (smoothed.height +1) / 2; + std::vector ii (width); + for (int i = 0; i < width; ++i) + ii[i] = 2 * i; + + FloatImagePtr down (new FloatImage (width, height)); +#ifdef _OPENMP +#pragma omp parallel for shared (output) firstprivate (ii) num_threads (threads_) +#endif + for (int j = 0; j < height; ++j) + { + int jj = 2*j; + for (int i = 0; i < width; ++i) + (*down) (i,j) = smoothed (ii[i],jj); + } + + output = down; +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::downsample (const FloatImageConstPtr& input, + FloatImageConstPtr& output, + FloatImageConstPtr& output_grad_x, + FloatImageConstPtr& output_grad_y) const +{ + downsample (input, output); + FloatImagePtr grad_x (new FloatImage (input->width, input->height)); + FloatImagePtr grad_y (new FloatImage (input->width, input->height)); + derivatives (*output, *grad_x, *grad_y); + output_grad_x = grad_x; + output_grad_y = grad_y; +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::convolve (const FloatImageConstPtr& input, FloatImage& output) const +{ + FloatImagePtr tmp (new FloatImage (input->width, input->height)); + convolveRows (input, *tmp); + convolveCols (tmp, output); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::convolveRows (const FloatImageConstPtr& input, FloatImage& output) const +{ + int width = input->width; + int height = input->height; + int last = input->width - kernel_size_2_; + int w = last - 1; + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for (int j = 0; j < height; ++j) + { + for (int i = kernel_size_2_; i < last; ++i) + { + double result = 0; + for (int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l) + result+= kernel_[k] * (*input) (l,j); + + output (i,j) = static_cast (result); + } + + for (int i = last; i < width; ++i) + output (i,j) = output (w, j); + + for (int i = 0; i < kernel_size_2_; ++i) + output (i,j) = output (kernel_size_2_, j); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::convolveCols (const FloatImageConstPtr& input, FloatImage& output) const +{ + output = FloatImage (input->width, input->height); + + int width = input->width; + int height = input->height; + int last = input->height - kernel_size_2_; + int h = last -1; + +#ifdef _OPENMP +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for (int i = 0; i < width; ++i) + { + for (int j = kernel_size_2_; j < last; ++j) + { + double result = 0; + for (int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l) + result += kernel_[k] * (*input) (i,l); + output (i,j) = static_cast (result); + } + + for (int j = last; j < height; ++j) + output (i,j) = output (i,h); + + for (int j = 0; j < kernel_size_2_; ++j) + output (i,j) = output (i, kernel_size_2_); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::computePyramids (const PointCloudInConstPtr& input, + std::vector& pyramid, + pcl::InterpolationType border_type) const +{ + int step = 3; + pyramid.resize (step * nb_levels_); + + FloatImageConstPtr previous; + FloatImagePtr tmp (new FloatImage (input->width, input->height)); +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for (int i = 0; i < static_cast (input->size ()); ++i) + tmp->points[i] = intensity_ (input->points[i]); + previous = tmp; + + FloatImagePtr img (new FloatImage (previous->width + 2*track_width_, + previous->height + 2*track_height_)); + + pcl::copyPointCloud (*tmp, *img, track_height_, track_height_, track_width_, track_width_, + border_type, 0.f); + pyramid[0] = img; + + // compute first level gradients + FloatImagePtr g_x (new FloatImage (input->width, input->height)); + FloatImagePtr g_y (new FloatImage (input->width, input->height)); + derivatives (*img, *g_x, *g_y); + // copy to bigger clouds + FloatImagePtr grad_x (new FloatImage (previous->width + 2*track_width_, + previous->height + 2*track_height_)); + pcl::copyPointCloud (*g_x, *grad_x, track_height_, track_height_, track_width_, track_width_, + pcl::BORDER_CONSTANT, 0.f); + pyramid[1] = grad_x; + + FloatImagePtr grad_y (new FloatImage (previous->width + 2*track_width_, + previous->height + 2*track_height_)); + pcl::copyPointCloud (*g_y, *grad_y, track_height_, track_height_, track_width_, track_width_, + pcl::BORDER_CONSTANT, 0.f); + pyramid[2] = grad_y; + + for (int level = 1; level < nb_levels_; ++level) + { + // compute current level and current level gradients + FloatImageConstPtr current; + FloatImageConstPtr g_x; + FloatImageConstPtr g_y; + downsample (previous, current, g_x, g_y); + // copy to bigger clouds + FloatImagePtr image (new FloatImage (current->width + 2*track_width_, + current->height + 2*track_height_)); + pcl::copyPointCloud (*current, *image, track_height_, track_height_, track_width_, track_width_, + border_type, 0.f); + pyramid[level*step] = image; + FloatImagePtr gradx (new FloatImage (g_x->width + 2*track_width_, g_x->height + 2*track_height_)); + pcl::copyPointCloud (*g_x, *gradx, track_height_, track_height_, track_width_, track_width_, + pcl::BORDER_CONSTANT, 0.f); + pyramid[level*step + 1] = gradx; + FloatImagePtr grady (new FloatImage (g_y->width + 2*track_width_, g_y->height + 2*track_height_)); + pcl::copyPointCloud (*g_y, *grady, track_height_, track_height_, track_width_, track_width_, + pcl::BORDER_CONSTANT, 0.f); + pyramid[level*step + 2] = grady; + // set the new level + previous = current; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::spatialGradient (const FloatImage& img, + const FloatImage& grad_x, + const FloatImage& grad_y, + const Eigen::Array2i& location, + const Eigen::Array4f& weight, + Eigen::ArrayXXf& win, + Eigen::ArrayXXf& grad_x_win, + Eigen::ArrayXXf& grad_y_win, + Eigen::Array3f &covariance) const +{ + const int step = img.width; + covariance.setZero (); + + for (int y = 0; y < track_height_; y++) + { + const float* img_ptr = &(img.points[0]) + (y + location[1])*step + location[0]; + const float* grad_x_ptr = &(grad_x.points[0]) + (y + location[1])*step + location[0]; + const float* grad_y_ptr = &(grad_y.points[0]) + (y + location[1])*step + location[0]; + + float* win_ptr = win.data () + y*win.cols (); + float* grad_x_win_ptr = grad_x_win.data () + y*grad_x_win.cols (); + float* grad_y_win_ptr = grad_y_win.data () + y*grad_y_win.cols (); + + for (int x =0; x < track_width_; ++x, ++grad_x_ptr, ++grad_y_ptr) + { + *win_ptr++ = img_ptr[x]*weight[0] + img_ptr[x+1]*weight[1] + img_ptr[x+step]*weight[2] + img_ptr[x+step+1]*weight[3]; + float ixval = grad_x_ptr[0]*weight[0] + grad_x_ptr[1]*weight[1] + grad_x_ptr[step]*weight[2] + grad_x_ptr[step+1]*weight[3]; + float iyval = grad_y_ptr[0]*weight[0] + grad_y_ptr[1]*weight[1] + grad_y_ptr[step]*weight[2] + grad_y_ptr[step+1]*weight[3]; + //!!! store those + *grad_x_win_ptr++ = ixval; + *grad_y_win_ptr++ = iyval; + //covariance components + covariance[0] += ixval*ixval; + covariance[1] += ixval*iyval; + covariance[2] += iyval*iyval; + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::mismatchVector (const Eigen::ArrayXXf& prev, + const Eigen::ArrayXXf& prev_grad_x, + const Eigen::ArrayXXf& prev_grad_y, + const FloatImage& next, + const Eigen::Array2i& location, + const Eigen::Array4f& weight, + Eigen::Array2f &b) const +{ + const int step = next.width; + b.setZero (); + for (int y = 0; y < track_height_; y++) + { + const float* next_ptr = &(next.points[0]) + (y + location[1])*step + location[0]; + const float* prev_ptr = prev.data () + y*prev.cols (); + const float* prev_grad_x_ptr = prev_grad_x.data () + y*prev_grad_x.cols (); + const float* prev_grad_y_ptr = prev_grad_y.data () + y*prev_grad_y.cols (); + + for (int x = 0; x < track_width_; ++x, ++prev_grad_y_ptr, ++prev_grad_x_ptr) + { + float diff = next_ptr[x]*weight[0] + next_ptr[x+1]*weight[1] + + next_ptr[x+step]*weight[2] + next_ptr[x+step+1]*weight[3] - prev_ptr[x]; + b[0] += *prev_grad_x_ptr * diff; + b[1] += *prev_grad_y_ptr * diff; + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::track (const PointCloudInConstPtr& prev_input, + const PointCloudInConstPtr& input, + const std::vector& prev_pyramid, + const std::vector& pyramid, + const pcl::PointCloud::ConstPtr& prev_keypoints, + pcl::PointCloud::Ptr& keypoints, + std::vector& status, + Eigen::Affine3f& motion) const +{ + std::vector > next_pts (prev_keypoints->size ()); + Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f); + pcl::TransformationFromCorrespondences transformation_computer; + const int nb_points = prev_keypoints->size (); + for (int level = nb_levels_ - 1; level >= 0; --level) + { + const FloatImage& prev = *(prev_pyramid[level*3]); + const FloatImage& next = *(pyramid[level*3]); + const FloatImage& grad_x = *(prev_pyramid[level*3+1]); + const FloatImage& grad_y = *(prev_pyramid[level*3+2]); + + Eigen::ArrayXXf prev_win (track_height_, track_width_); + Eigen::ArrayXXf grad_x_win (track_height_, track_width_); + Eigen::ArrayXXf grad_y_win (track_height_, track_width_); + float ratio (1./(1 << level)); + for (int ptidx = 0; ptidx < nb_points; ptidx++) + { + Eigen::Array2f prev_pt (prev_keypoints->points[ptidx].u * ratio, + prev_keypoints->points[ptidx].v * ratio); + Eigen::Array2f next_pt; + if (level == nb_levels_ -1) + next_pt = prev_pt; + else + next_pt = next_pts[ptidx]*2.f; + + next_pts[ptidx] = next_pt; + + Eigen::Array2i iprev_point; + prev_pt -= half_win; + iprev_point[0] = std::floor (prev_pt[0]); + iprev_point[1] = std::floor (prev_pt[1]); + + if (iprev_point[0] < -track_width_ || (std::uint32_t) iprev_point[0] >= grad_x.width || + iprev_point[1] < -track_height_ || (std::uint32_t) iprev_point[1] >= grad_y.height) + { + if (level == 0) + status [ptidx] = -1; + continue; + } + + float a = prev_pt[0] - iprev_point[0]; + float b = prev_pt[1] - iprev_point[1]; + Eigen::Array4f weight; + weight[0] = (1.f - a)*(1.f - b); + weight[1] = a*(1.f - b); + weight[2] = (1.f - a)*b; + weight[3] = 1 - weight[0] - weight[1] - weight[2]; + + Eigen::Array3f covar = Eigen::Array3f::Zero (); + spatialGradient (prev, grad_x, grad_y, iprev_point, weight, prev_win, grad_x_win, grad_y_win, covar); + + float det = covar[0]*covar[2] - covar[1]*covar[1]; + float min_eigenvalue = (covar[2] + covar[0] - std::sqrt ((covar[0]-covar[2])*(covar[0]-covar[2]) + 4.f*covar[1]*covar[1]))/2.f; + + if (min_eigenvalue < min_eigenvalue_threshold_ || det < std::numeric_limits::epsilon ()) + { + status[ptidx] = -2; + continue; + } + + det = 1.f/det; + next_pt -= half_win; + + Eigen::Array2f prev_delta (0, 0); + for (unsigned int j = 0; j < max_iterations_; j++) + { + Eigen::Array2i inext_pt = next_pt.floor ().cast (); + + if (inext_pt[0] < -track_width_ || (std::uint32_t) inext_pt[0] >= next.width || + inext_pt[1] < -track_height_ || (std::uint32_t) inext_pt[1] >= next.height) + { + if (level == 0) + status[ptidx] = -1; + break; + } + + a = next_pt[0] - inext_pt[0]; + b = next_pt[1] - inext_pt[1]; + weight[0] = (1.f - a)*(1.f - b); + weight[1] = a*(1.f - b); + weight[2] = (1.f - a)*b; + weight[3] = 1 - weight[0] - weight[1] - weight[2]; + // compute mismatch vector + Eigen::Array2f beta = Eigen::Array2f::Zero (); + mismatchVector (prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta); + // optical flow resolution + Eigen::Vector2f delta ((covar[1]*beta[1] - covar[2]*beta[0])*det, (covar[1]*beta[0] - covar[0]*beta[1])*det); + // update position + next_pt[0] += delta[0]; next_pt[1] += delta[1]; + next_pts[ptidx] = next_pt + half_win; + + if (delta.squaredNorm () <= epsilon_) + break; + + if (j > 0 && std::abs (delta[0] + prev_delta[0]) < 0.01 && + std::abs (delta[1] + prev_delta[1]) < 0.01 ) + { + next_pts[ptidx][0] -= delta[0]*0.5f; + next_pts[ptidx][1] -= delta[1]*0.5f; + break; + } + // update delta + prev_delta = delta; + } + + // update tracked points + if (level == 0 && !status[ptidx]) + { + Eigen::Array2f next_point = next_pts[ptidx] - half_win; + Eigen::Array2i inext_point; + + inext_point[0] = std::floor (next_point[0]); + inext_point[1] = std::floor (next_point[1]); + + if (inext_point[0] < -track_width_ || (std::uint32_t) inext_point[0] >= next.width || + inext_point[1] < -track_height_ || (std::uint32_t) inext_point[1] >= next.height) + { + status[ptidx] = -1; + continue; + } + // insert valid keypoint + pcl::PointUV n; + n.u = next_pts[ptidx][0]; + n.v = next_pts[ptidx][1]; + keypoints->push_back (n); + // add points pair to compute transformation + inext_point[0] = std::floor (next_pts[ptidx][0]); + inext_point[1] = std::floor (next_pts[ptidx][1]); + iprev_point[0] = std::floor (prev_keypoints->points[ptidx].u); + iprev_point[1] = std::floor (prev_keypoints->points[ptidx].v); + const PointInT& prev_pt = prev_input->points[iprev_point[1]*prev_input->width + iprev_point[0]]; + const PointInT& next_pt = input->points[inext_point[1]*input->width + inext_point[0]]; + transformation_computer.add (prev_pt.getVector3fMap (), next_pt.getVector3fMap (), 1.0); + } + } + } + motion = transformation_computer.getTransformation (); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::tracking::PyramidalKLTTracker::computeTracking () +{ + if (!initialized_) + return; + + std::vector pyramid; + computePyramids (input_, pyramid, pcl::BORDER_REFLECT_101); + pcl::PointCloud::Ptr keypoints (new pcl::PointCloud); + keypoints->reserve (keypoints_->size ()); + std::vector status (keypoints_->size (), 0); + track (ref_, input_, ref_pyramid_, pyramid, keypoints_, keypoints, status, motion_); + //swap reference and input + ref_ = input_; + ref_pyramid_ = pyramid; + keypoints_ = keypoints; + keypoints_status_->indices = status; +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/tracker.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/tracker.hpp new file mode 100755 index 0000000..8d8e45b --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/tracker.hpp @@ -0,0 +1,39 @@ +#ifndef PCL_TRACKING_IMPL_TRACKER_H_ +#define PCL_TRACKING_IMPL_TRACKER_H_ + +#include +#include +#include + +template bool +pcl::tracking::Tracker::initCompute () +{ + if (!PCLBase::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] PCLBase::Init failed.\n", getClassName ().c_str ()); + return (false); + } + + // If the dataset is empty, just return + if (input_->points.empty ()) + { + PCL_ERROR ("[pcl::%s::compute] input_ is empty!\n", getClassName ().c_str ()); + // Cleanup + deinitCompute (); + return (false); + } + + return (true); +} + +template void +pcl::tracking::Tracker::compute () +{ + if (!initCompute ()) + return; + + computeTracking (); + deinitCompute (); +} + +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/impl/tracking.hpp b/deps_install/include/pcl-1.10/pcl/tracking/impl/tracking.hpp new file mode 100755 index 0000000..2078d04 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/impl/tracking.hpp @@ -0,0 +1,816 @@ +#ifndef PCL_TRACKING_IMPL_TRACKING_H_ +#define PCL_TRACKING_IMPL_TRACKING_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace tracking + { + struct _ParticleXYZRPY + { + PCL_ADD_POINT4D; + union + { + struct + { + float roll; + float pitch; + float yaw; + float weight; + }; + float data_c[4]; + }; + }; + + // particle definition + struct EIGEN_ALIGN16 ParticleXYZRPY : public _ParticleXYZRPY + { + inline ParticleXYZRPY () + { + x = y = z = roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYZRPY (float _x, float _y, float _z) + { + x = _x; y = _y; z = _z; + roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYZRPY (float _x, float _y, float _z, float _roll, float _pitch, float _yaw) + { + x = _x; y = _y; z = _z; + roll = _roll; pitch = _pitch; yaw = _yaw; + data[3] = 1.0f; + } + + inline static int + stateDimension () { return 6; } + + void + sample (const std::vector& mean, const std::vector& cov) + { + x += static_cast (sampleNormal (mean[0], cov[0])); + y += static_cast (sampleNormal (mean[1], cov[1])); + z += static_cast (sampleNormal (mean[2], cov[2])); + + // The roll, pitch, yaw space is not Euclidean, so if we sample roll, + // pitch, and yaw independently, we bias our sampling in a complicated + // way that depends on where in the space we are sampling. + // + // A solution is to always sample around mean = 0 and project our + // samples onto the space of rotations, SO(3). We rely on the fact + // that we are sampling with small variance, so we do not bias + // ourselves too much with this projection. We then rotate our + // samples by the user's requested mean. The benefit of this approach + // is that our distribution's properties are consistent over the space + // of rotations. + Eigen::Matrix3f current_rotation; + current_rotation = getTransformation (x, y, z, roll, pitch, yaw).rotation (); + Eigen::Quaternionf q_current_rotation (current_rotation); + + Eigen::Matrix3f mean_rotation; + mean_rotation = getTransformation (mean[0], mean[1], mean[2], + mean[3], mean[4], mean[5]).rotation (); + Eigen::Quaternionf q_mean_rotation (mean_rotation); + + // Scales 1.0 radians of variance in RPY sampling into equivalent units for quaternion sampling. + const float scale_factor = 0.2862; + + float a = sampleNormal (0, scale_factor*cov[3]); + float b = sampleNormal (0, scale_factor*cov[4]); + float c = sampleNormal (0, scale_factor*cov[5]); + + Eigen::Vector4f vec_sample_mean_0 (a, b, c, 1); + Eigen::Quaternionf q_sample_mean_0 (vec_sample_mean_0); + q_sample_mean_0.normalize (); + + Eigen::Quaternionf q_sample_user_mean = q_sample_mean_0 * q_mean_rotation * q_current_rotation; + + Eigen::Affine3f affine_R (q_sample_user_mean.toRotationMatrix ()); + pcl::getEulerAngles (affine_R, roll, pitch, yaw); + } + + void + zero () + { + x = 0.0; + y = 0.0; + z = 0.0; + roll = 0.0; + pitch = 0.0; + yaw = 0.0; + } + + inline Eigen::Affine3f + toEigenMatrix () const + { + return getTransformation(x, y, z, roll, pitch, yaw); + } + + static pcl::tracking::ParticleXYZRPY + toState (const Eigen::Affine3f &trans) + { + float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw; + getTranslationAndEulerAngles (trans, + trans_x, trans_y, trans_z, + trans_roll, trans_pitch, trans_yaw); + return {trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw}; + } + + // a[i] + inline float operator [] (unsigned int i) + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return roll; + case 4: return pitch; + case 5: return yaw; + default: return 0.0; + } + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + inline std::ostream& operator << (std::ostream& os, const ParticleXYZRPY& p) + { + os << "(" << p.x << "," << p.y << "," << p.z << "," + << p.roll << "," << p.pitch << "," << p.yaw << ")"; + return (os); + } + + // a * k + inline pcl::tracking::ParticleXYZRPY operator * (const ParticleXYZRPY& p, double val) + { + pcl::tracking::ParticleXYZRPY newp; + newp.x = static_cast (p.x * val); + newp.y = static_cast (p.y * val); + newp.z = static_cast (p.z * val); + newp.roll = static_cast (p.roll * val); + newp.pitch = static_cast (p.pitch * val); + newp.yaw = static_cast (p.yaw * val); + return (newp); + } + + // a + b + inline pcl::tracking::ParticleXYZRPY operator + (const ParticleXYZRPY& a, const ParticleXYZRPY& b) + { + pcl::tracking::ParticleXYZRPY newp; + newp.x = a.x + b.x; + newp.y = a.y + b.y; + newp.z = a.z + b.z; + newp.roll = a.roll + b.roll; + newp.pitch = a.pitch + b.pitch; + newp.yaw = a.yaw + b.yaw; + return (newp); + } + + // a - b + inline pcl::tracking::ParticleXYZRPY operator - (const ParticleXYZRPY& a, const ParticleXYZRPY& b) + { + pcl::tracking::ParticleXYZRPY newp; + newp.x = a.x - b.x; + newp.y = a.y - b.y; + newp.z = a.z - b.z; + newp.roll = a.roll - b.roll; + newp.pitch = a.pitch - b.pitch; + newp.yaw = a.yaw - b.yaw; + return (newp); + } + + } +} + + +//########################################################################33 + + +namespace pcl +{ + namespace tracking + { + struct _ParticleXYZR + { + PCL_ADD_POINT4D; + union + { + struct + { + float roll; + float pitch; + float yaw; + float weight; + }; + float data_c[4]; + }; + }; + + // particle definition + struct EIGEN_ALIGN16 ParticleXYZR : public _ParticleXYZR + { + inline ParticleXYZR () + { + x = y = z = roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYZR (float _x, float _y, float _z) + { + x = _x; y = _y; z = _z; + roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYZR (float _x, float _y, float _z, float, float _pitch, float) + { + x = _x; y = _y; z = _z; + roll = 0; pitch = _pitch; yaw = 0; + data[3] = 1.0f; + } + + inline static int + stateDimension () { return 6; } + + void + sample (const std::vector& mean, const std::vector& cov) + { + x += static_cast (sampleNormal (mean[0], cov[0])); + y += static_cast (sampleNormal (mean[1], cov[1])); + z += static_cast (sampleNormal (mean[2], cov[2])); + roll = 0; + pitch += static_cast (sampleNormal (mean[4], cov[4])); + yaw = 0; + } + + void + zero () + { + x = 0.0; + y = 0.0; + z = 0.0; + roll = 0.0; + pitch = 0.0; + yaw = 0.0; + } + + inline Eigen::Affine3f + toEigenMatrix () const + { + return getTransformation(x, y, z, roll, pitch, yaw); + } + + static pcl::tracking::ParticleXYZR + toState (const Eigen::Affine3f &trans) + { + float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw; + getTranslationAndEulerAngles (trans, + trans_x, trans_y, trans_z, + trans_roll, trans_pitch, trans_yaw); + return (pcl::tracking::ParticleXYZR (trans_x, trans_y, trans_z, 0, trans_pitch, 0)); + } + + // a[i] + inline float operator [] (unsigned int i) + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return roll; + case 4: return pitch; + case 5: return yaw; + default: return 0.0; + } + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + inline std::ostream& operator << (std::ostream& os, const ParticleXYZR& p) + { + os << "(" << p.x << "," << p.y << "," << p.z << "," + << p.roll << "," << p.pitch << "," << p.yaw << ")"; + return (os); + } + + // a * k + inline pcl::tracking::ParticleXYZR operator * (const ParticleXYZR& p, double val) + { + pcl::tracking::ParticleXYZR newp; + newp.x = static_cast (p.x * val); + newp.y = static_cast (p.y * val); + newp.z = static_cast (p.z * val); + newp.roll = static_cast (p.roll * val); + newp.pitch = static_cast (p.pitch * val); + newp.yaw = static_cast (p.yaw * val); + return (newp); + } + + // a + b + inline pcl::tracking::ParticleXYZR operator + (const ParticleXYZR& a, const ParticleXYZR& b) + { + pcl::tracking::ParticleXYZR newp; + newp.x = a.x + b.x; + newp.y = a.y + b.y; + newp.z = a.z + b.z; + newp.roll = 0; + newp.pitch = a.pitch + b.pitch; + newp.yaw = 0.0; + return (newp); + } + + // a - b + inline pcl::tracking::ParticleXYZR operator - (const ParticleXYZR& a, const ParticleXYZR& b) + { + pcl::tracking::ParticleXYZR newp; + newp.x = a.x - b.x; + newp.y = a.y - b.y; + newp.z = a.z - b.z; + newp.roll = 0.0; + newp.pitch = a.pitch - b.pitch; + newp.yaw = 0.0; + return (newp); + } + + } +} + +//########################################################################33 + + + +namespace pcl +{ + namespace tracking + { + struct _ParticleXYRPY + { + PCL_ADD_POINT4D; + union + { + struct + { + float roll; + float pitch; + float yaw; + float weight; + }; + float data_c[4]; + }; + }; + + // particle definition + struct EIGEN_ALIGN16 ParticleXYRPY : public _ParticleXYRPY + { + inline ParticleXYRPY () + { + x = y = z = roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYRPY (float _x, float, float _z) + { + x = _x; y = 0; z = _z; + roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYRPY (float _x, float, float _z, float _roll, float _pitch, float _yaw) + { + x = _x; y = 0; z = _z; + roll = _roll; pitch = _pitch; yaw = _yaw; + data[3] = 1.0f; + } + + inline static int + stateDimension () { return 6; } + + void + sample (const std::vector& mean, const std::vector& cov) + { + x += static_cast (sampleNormal (mean[0], cov[0])); + y = 0; + z += static_cast (sampleNormal (mean[2], cov[2])); + roll += static_cast (sampleNormal (mean[3], cov[3])); + pitch += static_cast (sampleNormal (mean[4], cov[4])); + yaw += static_cast (sampleNormal (mean[5], cov[5])); + } + + void + zero () + { + x = 0.0; + y = 0.0; + z = 0.0; + roll = 0.0; + pitch = 0.0; + yaw = 0.0; + } + + inline Eigen::Affine3f + toEigenMatrix () const + { + return getTransformation(x, y, z, roll, pitch, yaw); + } + + static pcl::tracking::ParticleXYRPY + toState (const Eigen::Affine3f &trans) + { + float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw; + getTranslationAndEulerAngles (trans, + trans_x, trans_y, trans_z, + trans_roll, trans_pitch, trans_yaw); + return (pcl::tracking::ParticleXYRPY (trans_x, 0, trans_z, trans_roll, trans_pitch, trans_yaw)); + } + + // a[i] + inline float operator [] (unsigned int i) + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return roll; + case 4: return pitch; + case 5: return yaw; + default: return 0.0; + } + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + inline std::ostream& operator << (std::ostream& os, const ParticleXYRPY& p) + { + os << "(" << p.x << "," << p.y << "," << p.z << "," + << p.roll << "," << p.pitch << "," << p.yaw << ")"; + return (os); + } + + // a * k + inline pcl::tracking::ParticleXYRPY operator * (const ParticleXYRPY& p, double val) + { + pcl::tracking::ParticleXYRPY newp; + newp.x = static_cast (p.x * val); + newp.y = static_cast (p.y * val); + newp.z = static_cast (p.z * val); + newp.roll = static_cast (p.roll * val); + newp.pitch = static_cast (p.pitch * val); + newp.yaw = static_cast (p.yaw * val); + return (newp); + } + + // a + b + inline pcl::tracking::ParticleXYRPY operator + (const ParticleXYRPY& a, const ParticleXYRPY& b) + { + pcl::tracking::ParticleXYRPY newp; + newp.x = a.x + b.x; + newp.y = 0; + newp.z = a.z + b.z; + newp.roll = a.roll + b.roll; + newp.pitch = a.pitch + b.pitch; + newp.yaw = a.yaw + b.yaw; + return (newp); + } + + // a - b + inline pcl::tracking::ParticleXYRPY operator - (const ParticleXYRPY& a, const ParticleXYRPY& b) + { + pcl::tracking::ParticleXYRPY newp; + newp.x = a.x - b.x; + newp.z = a.z - b.z; + newp.y = 0; + newp.roll = a.roll - b.roll; + newp.pitch = a.pitch - b.pitch; + newp.yaw = a.yaw - b.yaw; + return (newp); + } + + } +} + +//########################################################################33 + +namespace pcl +{ + namespace tracking + { + struct _ParticleXYRP + { + PCL_ADD_POINT4D; + union + { + struct + { + float roll; + float pitch; + float yaw; + float weight; + }; + float data_c[4]; + }; + }; + + // particle definition + struct EIGEN_ALIGN16 ParticleXYRP : public _ParticleXYRP + { + inline ParticleXYRP () + { + x = y = z = roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYRP (float _x, float, float _z) + { + x = _x; y = 0; z = _z; + roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYRP (float _x, float, float _z, float, float _pitch, float _yaw) + { + x = _x; y = 0; z = _z; + roll = 0; pitch = _pitch; yaw = _yaw; + data[3] = 1.0f; + } + + inline static int + stateDimension () { return 6; } + + void + sample (const std::vector& mean, const std::vector& cov) + { + x += static_cast (sampleNormal (mean[0], cov[0])); + y = 0; + z += static_cast (sampleNormal (mean[2], cov[2])); + roll = 0; + pitch += static_cast (sampleNormal (mean[4], cov[4])); + yaw += static_cast (sampleNormal (mean[5], cov[5])); + } + + void + zero () + { + x = 0.0; + y = 0.0; + z = 0.0; + roll = 0.0; + pitch = 0.0; + yaw = 0.0; + } + + inline Eigen::Affine3f + toEigenMatrix () const + { + return getTransformation(x, y, z, roll, pitch, yaw); + } + + static pcl::tracking::ParticleXYRP + toState (const Eigen::Affine3f &trans) + { + float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw; + getTranslationAndEulerAngles (trans, + trans_x, trans_y, trans_z, + trans_roll, trans_pitch, trans_yaw); + return (pcl::tracking::ParticleXYRP (trans_x, 0, trans_z, 0, trans_pitch, trans_yaw)); + } + + // a[i] + inline float operator [] (unsigned int i) + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return roll; + case 4: return pitch; + case 5: return yaw; + default: return 0.0; + } + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + inline std::ostream& operator << (std::ostream& os, const ParticleXYRP& p) + { + os << "(" << p.x << "," << p.y << "," << p.z << "," + << p.roll << "," << p.pitch << "," << p.yaw << ")"; + return (os); + } + + // a * k + inline pcl::tracking::ParticleXYRP operator * (const ParticleXYRP& p, double val) + { + pcl::tracking::ParticleXYRP newp; + newp.x = static_cast (p.x * val); + newp.y = static_cast (p.y * val); + newp.z = static_cast (p.z * val); + newp.roll = static_cast (p.roll * val); + newp.pitch = static_cast (p.pitch * val); + newp.yaw = static_cast (p.yaw * val); + return (newp); + } + + // a + b + inline pcl::tracking::ParticleXYRP operator + (const ParticleXYRP& a, const ParticleXYRP& b) + { + pcl::tracking::ParticleXYRP newp; + newp.x = a.x + b.x; + newp.y = 0; + newp.z = a.z + b.z; + newp.roll = 0; + newp.pitch = a.pitch + b.pitch; + newp.yaw = a.yaw + b.yaw; + return (newp); + } + + // a - b + inline pcl::tracking::ParticleXYRP operator - (const ParticleXYRP& a, const ParticleXYRP& b) + { + pcl::tracking::ParticleXYRP newp; + newp.x = a.x - b.x; + newp.z = a.z - b.z; + newp.y = 0; + newp.roll = 0.0; + newp.pitch = a.pitch - b.pitch; + newp.yaw = a.yaw - b.yaw; + return (newp); + } + + } +} + +//########################################################################33 + +namespace pcl +{ + namespace tracking + { + struct _ParticleXYR + { + PCL_ADD_POINT4D; + union + { + struct + { + float roll; + float pitch; + float yaw; + float weight; + }; + float data_c[4]; + }; + }; + + // particle definition + struct EIGEN_ALIGN16 ParticleXYR : public _ParticleXYR + { + inline ParticleXYR () + { + x = y = z = roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYR (float _x, float, float _z) + { + x = _x; y = 0; z = _z; + roll = pitch = yaw = 0.0; + data[3] = 1.0f; + } + + inline ParticleXYR (float _x, float, float _z, float, float _pitch, float) + { + x = _x; y = 0; z = _z; + roll = 0; pitch = _pitch; yaw = 0; + data[3] = 1.0f; + } + + inline static int + stateDimension () { return 6; } + + void + sample (const std::vector& mean, const std::vector& cov) + { + x += static_cast (sampleNormal (mean[0], cov[0])); + y = 0; + z += static_cast (sampleNormal (mean[2], cov[2])); + roll = 0; + pitch += static_cast (sampleNormal (mean[4], cov[4])); + yaw = 0; + } + + void + zero () + { + x = 0.0; + y = 0.0; + z = 0.0; + roll = 0.0; + pitch = 0.0; + yaw = 0.0; + } + + inline Eigen::Affine3f + toEigenMatrix () const + { + return getTransformation(x, y, z, roll, pitch, yaw); + } + + static pcl::tracking::ParticleXYR + toState (const Eigen::Affine3f &trans) + { + float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw; + getTranslationAndEulerAngles (trans, + trans_x, trans_y, trans_z, + trans_roll, trans_pitch, trans_yaw); + return (pcl::tracking::ParticleXYR (trans_x, 0, trans_z, 0, trans_pitch, 0)); + } + + // a[i] + inline float operator [] (unsigned int i) + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return roll; + case 4: return pitch; + case 5: return yaw; + default: return 0.0; + } + } + + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + inline std::ostream& operator << (std::ostream& os, const ParticleXYR& p) + { + os << "(" << p.x << "," << p.y << "," << p.z << "," + << p.roll << "," << p.pitch << "," << p.yaw << ")"; + return (os); + } + + // a * k + inline pcl::tracking::ParticleXYR operator * (const ParticleXYR& p, double val) + { + pcl::tracking::ParticleXYR newp; + newp.x = static_cast (p.x * val); + newp.y = static_cast (p.y * val); + newp.z = static_cast (p.z * val); + newp.roll = static_cast (p.roll * val); + newp.pitch = static_cast (p.pitch * val); + newp.yaw = static_cast (p.yaw * val); + return (newp); + } + + // a + b + inline pcl::tracking::ParticleXYR operator + (const ParticleXYR& a, const ParticleXYR& b) + { + pcl::tracking::ParticleXYR newp; + newp.x = a.x + b.x; + newp.y = 0; + newp.z = a.z + b.z; + newp.roll = 0; + newp.pitch = a.pitch + b.pitch; + newp.yaw = 0.0; + return (newp); + } + + // a - b + inline pcl::tracking::ParticleXYR operator - (const ParticleXYR& a, const ParticleXYR& b) + { + pcl::tracking::ParticleXYR newp; + newp.x = a.x - b.x; + newp.z = a.z - b.z; + newp.y = 0; + newp.roll = 0.0; + newp.pitch = a.pitch - b.pitch; + newp.yaw = 0.0; + return (newp); + } + + } +} + +#define PCL_STATE_POINT_TYPES \ + (pcl::tracking::ParticleXYR) \ + (pcl::tracking::ParticleXYZRPY) \ + (pcl::tracking::ParticleXYZR) \ + (pcl::tracking::ParticleXYRPY) \ + (pcl::tracking::ParticleXYRP) + +#endif // diff --git a/deps_install/include/pcl-1.10/pcl/tracking/kld_adaptive_particle_filter.h b/deps_install/include/pcl-1.10/pcl/tracking/kld_adaptive_particle_filter.h new file mode 100755 index 0000000..dd8a3dc --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/kld_adaptive_particle_filter.h @@ -0,0 +1,217 @@ +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace tracking + { + + /** \brief @b KLDAdaptiveParticleFilterTracker tracks the PointCloud which is given by + setReferenceCloud within the measured PointCloud using particle filter method. + The number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01], [D.Fox, IJRR03]. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class KLDAdaptiveParticleFilterTracker: public ParticleFilterTracker + { + public: + using Tracker::tracker_name_; + using Tracker::search_; + using Tracker::input_; + using Tracker::getClassName; + using ParticleFilterTracker::transed_reference_vector_; + using ParticleFilterTracker::coherence_; + using ParticleFilterTracker::initParticles; + using ParticleFilterTracker::weight; + using ParticleFilterTracker::update; + using ParticleFilterTracker::iteration_num_; + using ParticleFilterTracker::particle_num_; + using ParticleFilterTracker::particles_; + using ParticleFilterTracker::use_normal_; + using ParticleFilterTracker::use_change_detector_; + using ParticleFilterTracker::change_detector_resolution_; + using ParticleFilterTracker::change_detector_; + using ParticleFilterTracker::motion_; + using ParticleFilterTracker::motion_ratio_; + using ParticleFilterTracker::step_noise_covariance_; + using ParticleFilterTracker::representative_state_; + using ParticleFilterTracker::sampleWithReplacement; + + using BaseClass = Tracker; + + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + using PointCloudIn = typename Tracker::PointCloudIn; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudState = typename Tracker::PointCloudState; + using PointCloudStatePtr = typename PointCloudState::Ptr; + using PointCloudStateConstPtr = typename PointCloudState::ConstPtr; + + using Coherence = PointCoherence; + using CoherencePtr = typename Coherence::Ptr; + using CoherenceConstPtr = typename Coherence::ConstPtr; + + using CloudCoherence = PointCloudCoherence; + using CloudCoherencePtr = typename CloudCoherence::Ptr; + using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr; + + /** \brief Empty constructor. */ + KLDAdaptiveParticleFilterTracker () + : ParticleFilterTracker () + , maximum_particle_number_ () + , epsilon_ (0) + , delta_ (0.99) + , bin_size_ () + { + tracker_name_ = "KLDAdaptiveParticleFilterTracker"; + } + + /** \brief set the bin size. + * \param bin_size the size of a bin + */ + inline void setBinSize (const StateT& bin_size) { bin_size_ = bin_size; } + + /** \brief get the bin size. */ + inline StateT getBinSize () const { return (bin_size_); } + + /** \brief set the maximum number of the particles. + * \param nr the maximum number of the particles. + */ + inline void setMaximumParticleNum (unsigned int nr) { maximum_particle_number_ = nr; } + + /** \brief get the maximum number of the particles.*/ + inline unsigned int getMaximumParticleNum () const { return (maximum_particle_number_); } + + /** \brief set epsilon to be used to calc K-L boundary. + * \param eps epsilon + */ + inline void setEpsilon (double eps) { epsilon_ = eps; } + + /** \brief get epsilon to be used to calc K-L boundary. */ + inline double getEpsilon () const { return (epsilon_); } + + /** \brief set delta to be used in chi-squared distribution. + * \param delta delta of chi-squared distribution. + */ + inline void setDelta (double delta) { delta_ = delta; } + + /** \brief get delta to be used in chi-squared distribution.*/ + inline double getDelta () const { return (delta_); } + + protected: + + /** \brief return true if the two bins are equal. + * \param a index of the bin + * \param b index of the bin + */ + virtual bool + equalBin (const std::vector &a, const std::vector &b) + { + int dimension = StateT::stateDimension (); + for (int i = 0; i < dimension; i++) + if (a[i] != b[i]) + return (false); + return (true); + } + + /** \brief return upper quantile of standard normal distribution. + * \param[in] u ratio of quantile. + */ + double + normalQuantile (double u) + { + const double a[9] = { 1.24818987e-4, -1.075204047e-3, 5.198775019e-3, + -0.019198292004, 0.059054035642,-0.151968751364, + 0.319152932694,-0.5319230073, 0.797884560593}; + const double b[15] = { -4.5255659e-5, 1.5252929e-4, -1.9538132e-5, + -6.76904986e-4, 1.390604284e-3,-7.9462082e-4, + -2.034254874e-3, 6.549791214e-3,-0.010557625006, + 0.011630447319,-9.279453341e-3, 5.353579108e-3, + -2.141268741e-3, 5.35310549e-4, 0.999936657524}; + double w, y, z; + + if (u == 0.) + return (0.5); + y = u / 2.0; + if (y < -3.) + return (0.0); + if (y > 3.) + return (1.0); + if (y < 0.0) + y = - y; + if (y < 1.0) + { + w = y * y; + z = a[0]; + for (int i = 1; i < 9; i++) + z = z * w + a[i]; + z *= (y * 2.0); + } + else + { + y -= 2.0; + z = b[0]; + for (int i = 1; i < 15; i++) + z = z * y + b[i]; + } + + if (u < 0.0) + return ((1. - z) / 2.0); + return ((1. + z) / 2.0); + } + + /** \brief calculate K-L boundary. K-L boundary follows 1/2e*chi(k-1, 1-d)^2. + * \param[in] k the number of bins and the first parameter of chi distribution. + */ + virtual + double calcKLBound (int k) + { + double z = normalQuantile (delta_); + double chi = 1.0 - 2.0 / (9.0 * (k - 1)) + sqrt (2.0 / (9.0 * (k - 1))) * z; + return ((k - 1.0) / (2.0 * epsilon_) * chi * chi * chi); + } + + /** \brief insert a bin into the set of the bins. if that bin is already registered, + return false. if not, return true. + * \param new_bin a bin to be inserted. + * \param bins a set of the bins + */ + virtual bool + insertIntoBins (std::vector &&new_bin, std::vector > &bins); + + /** \brief This method should get called before starting the actual computation. */ + bool + initCompute () override; + + /** \brief resampling phase of particle filter method. + sampling the particles according to the weights calculated in weight method. + in particular, "sample with replacement" is archieved by walker's alias method. + */ + void + resample () override; + + /** \brief the maximum number of the particles. */ + unsigned int maximum_particle_number_; + + /** \brief error between K-L distance and MLE*/ + double epsilon_; + + /** \brief probability of distance between K-L distance and MLE is less than epsilon_*/ + double delta_; + + /** \brief the size of a bin.*/ + StateT bin_size_; + }; + } +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/kld_adaptive_particle_filter_omp.h b/deps_install/include/pcl-1.10/pcl/tracking/kld_adaptive_particle_filter_omp.h new file mode 100755 index 0000000..6a46670 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/kld_adaptive_particle_filter_omp.h @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace tracking + { + /** \brief @b KLDAdaptiveParticleFilterOMPTracker tracks the PointCloud which is given by + setReferenceCloud within the measured PointCloud using particle filter method. + The number of the particles changes adaptively based on KLD sampling [D. Fox, NIPS-01], [D.Fox, IJRR03]. + and the computation of the weights of the particles is parallelized using OpenMP. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class KLDAdaptiveParticleFilterOMPTracker: public KLDAdaptiveParticleFilterTracker + { + public: + using Tracker::tracker_name_; + using Tracker::search_; + using Tracker::input_; + using Tracker::indices_; + using Tracker::getClassName; + using KLDAdaptiveParticleFilterTracker::particles_; + using KLDAdaptiveParticleFilterTracker::change_detector_; + using KLDAdaptiveParticleFilterTracker::change_counter_; + using KLDAdaptiveParticleFilterTracker::change_detector_interval_; + using KLDAdaptiveParticleFilterTracker::use_change_detector_; + using KLDAdaptiveParticleFilterTracker::pass_x_; + using KLDAdaptiveParticleFilterTracker::pass_y_; + using KLDAdaptiveParticleFilterTracker::pass_z_; + using KLDAdaptiveParticleFilterTracker::alpha_; + using KLDAdaptiveParticleFilterTracker::changed_; + using KLDAdaptiveParticleFilterTracker::coherence_; + using KLDAdaptiveParticleFilterTracker::use_normal_; + using KLDAdaptiveParticleFilterTracker::particle_num_; + using KLDAdaptiveParticleFilterTracker::change_detector_filter_; + using KLDAdaptiveParticleFilterTracker::transed_reference_vector_; + //using KLDAdaptiveParticleFilterTracker::calcLikelihood; + using KLDAdaptiveParticleFilterTracker::normalizeWeight; + using KLDAdaptiveParticleFilterTracker::normalizeParticleWeight; + using KLDAdaptiveParticleFilterTracker::calcBoundingBox; + + using BaseClass = Tracker; + + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + using PointCloudIn = typename Tracker::PointCloudIn; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudState = typename Tracker::PointCloudState; + using PointCloudStatePtr = typename PointCloudState::Ptr; + using PointCloudStateConstPtr = typename PointCloudState::ConstPtr; + + using Coherence = PointCoherence; + using CoherencePtr = typename Coherence::Ptr; + using CoherenceConstPtr = typename Coherence::ConstPtr; + + using CloudCoherence = PointCloudCoherence; + using CloudCoherencePtr = typename CloudCoherence::Ptr; + using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr; + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + KLDAdaptiveParticleFilterOMPTracker (unsigned int nr_threads = 0) + : KLDAdaptiveParticleFilterTracker () + { + tracker_name_ = "KLDAdaptiveParticleFilterOMPTracker"; + + setNumberOfThreads(nr_threads); + } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + protected: + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + + /** \brief weighting phase of particle filter method. + calculate the likelihood of all of the particles and set the weights. + */ + void weight () override; + + }; + } +} + +//#include +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/nearest_pair_point_cloud_coherence.h b/deps_install/include/pcl-1.10/pcl/tracking/nearest_pair_point_cloud_coherence.h new file mode 100755 index 0000000..0ee9208 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/nearest_pair_point_cloud_coherence.h @@ -0,0 +1,97 @@ +#pragma once + +#include + +#include + +namespace pcl +{ + namespace tracking + { + /** \brief @b NearestPairPointCloudCoherence computes coherence between two pointclouds using the + nearest point pairs. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class NearestPairPointCloudCoherence: public PointCloudCoherence + { + public: + using PointCloudCoherence::getClassName; + using PointCloudCoherence::coherence_name_; + using PointCloudCoherence::target_input_; + + using PointCoherencePtr = typename PointCloudCoherence::PointCoherencePtr; + using PointCloudInConstPtr = typename PointCloudCoherence::PointCloudInConstPtr; + using BaseClass = PointCloudCoherence; + + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + using SearchPtr = typename pcl::search::Search::Ptr; + using SearchConstPtr = typename pcl::search::Search::ConstPtr; + + /** \brief empty constructor */ + NearestPairPointCloudCoherence () + : new_target_ (false) + , search_ () + , maximum_distance_ (std::numeric_limits::max ()) + { + coherence_name_ = "NearestPairPointCloudCoherence"; + } + + /** \brief Provide a pointer to a dataset to add additional information + * to estimate the features for every point in the input dataset. This + * is optional, if this is not set, it will only use the data in the + * input cloud to estimate the features. This is useful when you only + * need to compute the features for a downsampled cloud. + * \param search a pointer to a PointCloud message + */ + inline void + setSearchMethod (const SearchPtr &search) { search_ = search; } + + /** \brief Get a pointer to the point cloud dataset. */ + inline SearchPtr + getSearchMethod () { return (search_); } + + /** \brief add a PointCoherence to the PointCloudCoherence. + * \param[in] cloud coherence a pointer to PointCoherence. + */ + inline void + setTargetCloud (const PointCloudInConstPtr &cloud) override + { + new_target_ = true; + PointCloudCoherence::setTargetCloud (cloud); + } + + /** \brief set maximum distance to be taken into account. + * \param[in] val maximum distance. + */ + inline void setMaximumDistance (double val) { maximum_distance_ = val; } + + protected: + using PointCloudCoherence::point_coherences_; + + /** \brief This method should get called before starting the actual computation. */ + bool initCompute () override; + + /** \brief A flag which is true if target_input_ is updated */ + bool new_target_; + + /** \brief A pointer to the spatial search object. */ + SearchPtr search_; + + /** \brief max of distance for points to be taken into account*/ + double maximum_distance_; + + /** \brief compute the nearest pairs and compute coherence using point_coherences_ */ + void + computeCoherence (const PointCloudInConstPtr &cloud, const IndicesConstPtr &indices, float &w_j) override; + + }; + } +} + +// #include +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/normal_coherence.h b/deps_install/include/pcl-1.10/pcl/tracking/normal_coherence.h new file mode 100755 index 0000000..329e404 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/normal_coherence.h @@ -0,0 +1,50 @@ +#pragma once + +#include +namespace pcl +{ + namespace tracking + { + /** \brief @b NormalCoherence computes coherence between two points from the angle + between their normals. the coherence is calculated by 1 / (1 + weight * theta^2 ). + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class NormalCoherence: public PointCoherence + { + public: + + /** \brief initialize the weight to 1.0. */ + NormalCoherence () + : PointCoherence () + , weight_ (1.0) + {} + + /** \brief set the weight of coherence + * \param weight the weight of coherence + */ + inline void setWeight (double weight) { weight_ = weight; } + + /** \brief get the weight of coherence */ + inline double getWeight () { return weight_; } + + protected: + + /** \brief return the normal coherence between the two points. + * \param source instance of source point. + * \param target instance of target point. + */ + double computeCoherence (PointInT &source, PointInT &target) override; + + /** \brief the weight of coherence */ + double weight_; + + }; + } +} + +// #include +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/particle_filter.h b/deps_install/include/pcl-1.10/pcl/tracking/particle_filter.h new file mode 100755 index 0000000..f954f3e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/particle_filter.h @@ -0,0 +1,509 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include + +namespace pcl +{ + namespace tracking + { + /** \brief @b ParticleFilterTracker tracks the PointCloud which is given by + setReferenceCloud within the measured PointCloud using particle filter method. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class ParticleFilterTracker: public Tracker + { + protected: + using Tracker::deinitCompute; + + public: + using Tracker::tracker_name_; + using Tracker::search_; + using Tracker::input_; + using Tracker::indices_; + using Tracker::getClassName; + + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + using BaseClass = Tracker; + + using PointCloudIn = typename Tracker::PointCloudIn; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudState = typename Tracker::PointCloudState; + using PointCloudStatePtr = typename PointCloudState::Ptr; + using PointCloudStateConstPtr = typename PointCloudState::ConstPtr; + + using Coherence = PointCoherence; + using CoherencePtr = typename Coherence::Ptr; + using CoherenceConstPtr = typename Coherence::ConstPtr; + + using CloudCoherence = PointCloudCoherence; + using CloudCoherencePtr = typename CloudCoherence::Ptr; + using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr; + + /** \brief Empty constructor. */ + ParticleFilterTracker () + : iteration_num_ (1) + , particle_num_ () + , min_indices_ (1) + , ref_ () + , particles_ () + , coherence_ () + , resample_likelihood_thr_ (0.0) + , occlusion_angle_thr_ (M_PI / 2.0) + , alpha_ (15.0) + , representative_state_ () + , use_normal_ (false) + , motion_ () + , motion_ratio_ (0.25) + , pass_x_ () + , pass_y_ () + , pass_z_ () + , transed_reference_vector_ () + , change_detector_ () + , changed_ (false) + , change_counter_ (0) + , change_detector_filter_ (10) + , change_detector_interval_ (10) + , change_detector_resolution_ (0.01) + , use_change_detector_ (false) + { + tracker_name_ = "ParticleFilterTracker"; + pass_x_.setFilterFieldName ("x"); + pass_y_.setFilterFieldName ("y"); + pass_z_.setFilterFieldName ("z"); + pass_x_.setKeepOrganized (false); + pass_y_.setKeepOrganized (false); + pass_z_.setKeepOrganized (false); + } + + /** \brief Set the number of iteration. + * \param[in] iteration_num the number of iteration. + */ + inline void + setIterationNum (const int iteration_num) { iteration_num_ = iteration_num; } + + /** \brief Get the number of iteration. */ + inline int + getIterationNum () const { return iteration_num_; } + + /** \brief Set the number of the particles. + * \param[in] particle_num the number of the particles. + */ + inline void + setParticleNum (const int particle_num) { particle_num_ = particle_num; } + + /** \brief Get the number of the particles. */ + inline int + getParticleNum () const { return particle_num_; } + + /** \brief Set a pointer to a reference dataset to be tracked. + * \param[in] ref a pointer to a PointCloud message + */ + inline void + setReferenceCloud (const PointCloudInConstPtr &ref) { ref_ = ref; } + + /** \brief Get a pointer to a reference dataset to be tracked. */ + inline PointCloudInConstPtr const + getReferenceCloud () { return ref_; } + + /** \brief Set the PointCloudCoherence as likelihood. + * \param[in] coherence a pointer to PointCloudCoherence. + */ + inline void + setCloudCoherence (const CloudCoherencePtr &coherence) { coherence_ = coherence; } + + /** \brief Get the PointCloudCoherence to compute likelihood. */ + inline CloudCoherencePtr + getCloudCoherence () const { return coherence_; } + + + /** \brief Set the covariance of step noise. + * \param[in] step_noise_covariance the diagonal elements of covariance matrix of step noise. + */ + inline void + setStepNoiseCovariance (const std::vector &step_noise_covariance) + { + step_noise_covariance_ = step_noise_covariance; + } + + /** \brief Set the covariance of the initial noise. It will be used when initializing the particles. + * \param[in] initial_noise_covariance the diagonal elements of covariance matrix of initial noise. + */ + inline void + setInitialNoiseCovariance (const std::vector &initial_noise_covariance) + { + initial_noise_covariance_ = initial_noise_covariance; + } + + /** \brief Set the mean of the initial noise. It will be used when initializing the particles. + * \param[in] initial_noise_mean the mean values of initial noise. + */ + inline void + setInitialNoiseMean (const std::vector &initial_noise_mean) + { + initial_noise_mean_ = initial_noise_mean; + } + + /** \brief Set the threshold to re-initialize the particles. + * \param[in] resample_likelihood_thr threshold to re-initialize. + */ + inline void + setResampleLikelihoodThr (const double resample_likelihood_thr) + { + resample_likelihood_thr_ = resample_likelihood_thr; + } + + /** \brief Set the threshold of angle to be considered occlusion (default: pi/2). + * ParticleFilterTracker does not take the occluded points into account according to the angle + * between the normal and the position. + * \param[in] occlusion_angle_thr threshold of angle to be considered occlusion. + */ + inline void + setOcclusionAngleThe (const double occlusion_angle_thr) + { + occlusion_angle_thr_ = occlusion_angle_thr; + } + + /** \brief Set the minimum number of indices (default: 1). + * ParticleFilterTracker does not take into account the hypothesis + * whose the number of points is smaller than the minimum indices. + * \param[in] min_indices the minimum number of indices. + */ + inline void + setMinIndices (const int min_indices) { min_indices_ = min_indices; } + + /** \brief Set the transformation from the world coordinates to the frame of the particles. + * \param[in] trans Affine transformation from the worldcoordinates to the frame of the particles. + */ + inline void setTrans (const Eigen::Affine3f &trans) { trans_ = trans; } + + /** \brief Get the transformation from the world coordinates to the frame of the particles. */ + inline Eigen::Affine3f getTrans () const { return trans_; } + + /** \brief Get an instance of the result of tracking. + * This function returns the particle that represents the transform between the reference point cloud at the + * beginning and the best guess about its location in the most recent frame. + */ + inline StateT getResult () const override { return representative_state_; } + + /** \brief Convert a state to affine transformation from the world coordinates frame. + * \param[in] particle an instance of StateT. + */ + Eigen::Affine3f toEigenMatrix (const StateT& particle) + { + return particle.toEigenMatrix (); + } + + /** \brief Get a pointer to a pointcloud of the particles. */ + inline PointCloudStatePtr getParticles () const { return particles_; } + + /** \brief Normalize the weight of a particle using \f$ std::exp(1- alpha ( w - w_{min}) / (w_max - w_min)) \f$ + * \note This method is described in [P.Azad et. al, ICRA11]. + * \param[in] w the weight to be normalized + * \param[in] w_min the minimum weight of the particles + * \param[in] w_max the maximum weight of the particles + */ + inline double normalizeParticleWeight (double w, double w_min, double w_max) + { + return std::exp (1.0 - alpha_ * (w - w_min) / (w_max - w_min)); + } + + /** \brief Set the value of alpha. + * \param[in] alpha the value of alpha + */ + inline void setAlpha (double alpha) { alpha_ = alpha; } + + /** \brief Get the value of alpha. */ + inline double getAlpha () { return alpha_; } + + /** \brief Set the value of use_normal_. + * \param[in] use_normal the value of use_normal_. + */ + inline void setUseNormal (bool use_normal) { use_normal_ = use_normal; } + + /** \brief Get the value of use_normal_. */ + inline bool getUseNormal () { return use_normal_; } + + /** \brief Set the value of use_change_detector_. + * \param[in] use_change_detector the value of use_change_detector_. + */ + inline void setUseChangeDetector (bool use_change_detector) { use_change_detector_ = use_change_detector; } + + /** \brief Get the value of use_change_detector_. */ + inline bool getUseChangeDetector () { return use_change_detector_; } + + /** \brief Set the motion ratio + * \param[in] motion_ratio the ratio of hypothesis to use motion model. + */ + inline void setMotionRatio (double motion_ratio) { motion_ratio_ = motion_ratio; } + + /** \brief Get the motion ratio. */ + inline double getMotionRatio () { return motion_ratio_;} + + /** \brief Set the number of interval frames to run change detection. + * \param[in] change_detector_interval the number of interval frames. + */ + inline void setIntervalOfChangeDetection (unsigned int change_detector_interval) + { + change_detector_interval_ = change_detector_interval; + } + + /** \brief Get the number of interval frames to run change detection. */ + inline unsigned int getIntervalOfChangeDetection () + { + return change_detector_interval_; + } + + /** \brief Set the minimum amount of points required within leaf node to become serialized in change detection + * \param[in] change_detector_filter the minimum amount of points required within leaf node + */ + inline void setMinPointsOfChangeDetection (unsigned int change_detector_filter) + { + change_detector_filter_ = change_detector_filter; + } + + /** \brief Set the resolution of change detection. + * \param[in] resolution resolution of change detection octree + */ + inline void setResolutionOfChangeDetection (double resolution) { change_detector_resolution_ = resolution; } + + /** \brief Get the resolution of change detection. */ + inline double getResolutionOfChangeDetection () { return change_detector_resolution_; } + + /** \brief Get the minimum amount of points required within leaf node to become serialized in change detection. */ + inline unsigned int getMinPointsOfChangeDetection () + { + return change_detector_filter_; + } + + /** \brief Get the adjustment ratio. */ + inline double + getFitRatio() const { return fit_ratio_; } + + /** \brief Reset the particles to restart tracking*/ + virtual inline void resetTracking () + { + if (particles_) + particles_->points.clear (); + } + + protected: + + /** \brief Compute the parameters for the bounding box of hypothesis pointclouds. + * \param[out] x_min the minimum value of x axis. + * \param[out] x_max the maximum value of x axis. + * \param[out] y_min the minimum value of y axis. + * \param[out] y_max the maximum value of y axis. + * \param[out] z_min the minimum value of z axis. + * \param[out] z_max the maximum value of z axis. + */ + void calcBoundingBox (double &x_min, double &x_max, + double &y_min, double &y_max, + double &z_min, double &z_max); + + /** \brief Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud. + * \param[in] cloud a pointer to pointcloud to be cropped. + * \param[out] output a pointer to be assigned the cropped pointcloud. + */ + void cropInputPointCloud (const PointCloudInConstPtr &cloud, PointCloudIn &output); + + + + /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents. + * \param[in] hypothesis a particle which represents a hypothesis. + * \param[in] indices the indices which should be taken into account. + * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis. + **/ + void computeTransformedPointCloud (const StateT& hypothesis, + std::vector& indices, + PointCloudIn &cloud); + + /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate + * indices taking occlusion into account. + * \param[in] hypothesis a particle which represents a hypothesis. + * \param[in] indices the indices which should be taken into account. + * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis. + **/ + void computeTransformedPointCloudWithNormal (const StateT& hypothesis, + std::vector& indices, + PointCloudIn &cloud); + + /** \brief Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate + * indices without taking occlusion into account. + * \param[in] hypothesis a particle which represents a hypothesis. + * \param[out] cloud the resultant point cloud model dataset which is transformed to hypothesis. + **/ + void computeTransformedPointCloudWithoutNormal (const StateT& hypothesis, + PointCloudIn &cloud); + + + /** \brief This method should get called before starting the actual computation. */ + bool initCompute () override; + + /** \brief Weighting phase of particle filter method. Calculate the likelihood of all of the particles and set the weights. */ + virtual void weight (); + + /** \brief Resampling phase of particle filter method. Sampling the particles according to the weights calculated + * in weight method. In particular, "sample with replacement" is archieved by walker's alias method. + */ + virtual void resample (); + + /** \brief Calculate the weighted mean of the particles and set it as the result. */ + virtual void update (); + + /** \brief Normalize the weights of all the particels. */ + virtual void normalizeWeight (); + + /** \brief Initialize the particles. initial_noise_covariance_ and initial_noise_mean_ are used for Gaussian sampling. */ + void initParticles (bool reset); + + /** \brief Track the pointcloud using particle filter method. */ + void computeTracking () override; + + /** \brief Implementation of "sample with replacement" using Walker's alias method. + about Walker's alias method, you can check the paper below: + article{355749}, + author = {Walker, Alastair J.}, + title = {An Efficient Method for Generating Discrete + Random Variables with General Distributions}, + journal = {ACM Trans. Math. Softw.}, + volume = {3}, + number = {3}, + year = {1977}, + issn = {0098-3500}, + pages = {253--256}, + doi = {http://doi.acm.org/10.1145/355744.355749}, + publisher = {ACM}, + address = {New York, NY, USA}, + } + \param a an alias table, which generated by genAliasTable. + \param q a table of weight, which generated by genAliasTable. + */ + int sampleWithReplacement (const std::vector& a, const std::vector& q); + + /** \brief Generate the tables for walker's alias method. */ + void genAliasTable (std::vector &a, std::vector &q, const PointCloudStateConstPtr &particles); + + /** \brief Resampling the particle with replacement. */ + void + resampleWithReplacement (); + + /** \brief Resampling the particle in deterministic way. */ + void + resampleDeterministic (); + + /** \brief Run change detection and return true if there is a change. + * \param[in] input a pointer to the input pointcloud. + */ + bool + testChangeDetection (const PointCloudInConstPtr &input); + + /** \brief The number of iteration of particlefilter. */ + int iteration_num_; + + /** \brief The number of the particles. */ + int particle_num_; + + /** \brief The minimum number of points which the hypothesis should have. */ + int min_indices_; + + /** \brief Adjustment of the particle filter. */ + double fit_ratio_; + + /** \brief A pointer to reference point cloud. */ + PointCloudInConstPtr ref_; + + /** \brief A pointer to the particles */ + PointCloudStatePtr particles_; + + /** \brief A pointer to PointCloudCoherence. */ + CloudCoherencePtr coherence_; + + /** \brief The diagonal elements of covariance matrix of the step noise. the covariance matrix is used + * at every resample method. + */ + std::vector step_noise_covariance_; + + /** \brief The diagonal elements of covariance matrix of the initial noise. the covariance matrix is used + * when initialize the particles. + */ + std::vector initial_noise_covariance_; + + /** \brief The mean values of initial noise. */ + std::vector initial_noise_mean_; + + /** \brief The threshold for the particles to be re-initialized. */ + double resample_likelihood_thr_; + + /** \brief The threshold for the points to be considered as occluded. */ + double occlusion_angle_thr_; + + /** \brief The weight to be used in normalization of the weights of the particles. */ + double alpha_; + + /** \brief The result of tracking. */ + StateT representative_state_; + + /** \brief An affine transformation from the world coordinates frame to the origin of the particles. */ + Eigen::Affine3f trans_; + + /** \brief A flag to use normal or not. defaults to false. */ + bool use_normal_; + + /** \brief Difference between the result in t and t-1. */ + StateT motion_; + + /** \brief Ratio of hypothesis to use motion model. */ + double motion_ratio_; + + /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */ + pcl::PassThrough pass_x_; + /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */ + pcl::PassThrough pass_y_; + /** \brief Pass through filter to crop the pointclouds within the hypothesis bounding box. */ + pcl::PassThrough pass_z_; + + /** \brief A list of the pointers to pointclouds. */ + std::vector transed_reference_vector_; + + /** \brief Change detector used as a trigger to track. */ + typename pcl::octree::OctreePointCloudChangeDetector::Ptr change_detector_; + + /** \brief A flag to be true when change of pointclouds is detected. */ + bool changed_; + + /** \brief A counter to skip change detection. */ + unsigned int change_counter_; + + /** \brief Minimum points in a leaf when calling change detector. defaults to 10. */ + unsigned int change_detector_filter_; + + /** \brief The number of interval frame to run change detection. defaults to 10. */ + unsigned int change_detector_interval_; + + /** \brief Resolution of change detector. defaults to 0.01. */ + double change_detector_resolution_; + + /** \brief The flag which will be true if using change detection. */ + bool use_change_detector_; + }; + } +} + +// #include +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/particle_filter_omp.h b/deps_install/include/pcl-1.10/pcl/tracking/particle_filter_omp.h new file mode 100755 index 0000000..86fe280 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/particle_filter_omp.h @@ -0,0 +1,94 @@ +#pragma once + +#include +#include +#include + +namespace pcl +{ + namespace tracking + { + /** \brief @b ParticleFilterOMPTracker tracks the PointCloud which is given by + setReferenceCloud within the measured PointCloud using particle filter method + in parallel, using the OpenMP standard. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class ParticleFilterOMPTracker: public ParticleFilterTracker + { + public: + using Tracker::tracker_name_; + using Tracker::search_; + using Tracker::input_; + using Tracker::indices_; + using Tracker::getClassName; + using ParticleFilterTracker::particles_; + using ParticleFilterTracker::change_detector_; + using ParticleFilterTracker::change_counter_; + using ParticleFilterTracker::change_detector_interval_; + using ParticleFilterTracker::use_change_detector_; + using ParticleFilterTracker::alpha_; + using ParticleFilterTracker::changed_; + using ParticleFilterTracker::coherence_; + using ParticleFilterTracker::use_normal_; + using ParticleFilterTracker::particle_num_; + using ParticleFilterTracker::change_detector_filter_; + using ParticleFilterTracker::transed_reference_vector_; + //using ParticleFilterTracker::calcLikelihood; + using ParticleFilterTracker::normalizeWeight; + using ParticleFilterTracker::normalizeParticleWeight; + using ParticleFilterTracker::calcBoundingBox; + + using BaseClass = Tracker; + + using PointCloudIn = typename Tracker::PointCloudIn; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudState = typename Tracker::PointCloudState; + using PointCloudStatePtr = typename PointCloudState::Ptr; + using PointCloudStateConstPtr = typename PointCloudState::ConstPtr; + + using Coherence = PointCoherence; + using CoherencePtr = typename Coherence::Ptr; + using CoherenceConstPtr = typename Coherence::ConstPtr; + + using CloudCoherence = PointCloudCoherence; + using CloudCoherencePtr = typename CloudCoherence::Ptr; + using CloudCoherenceConstPtr = typename CloudCoherence::ConstPtr; + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + ParticleFilterOMPTracker (unsigned int nr_threads = 0) + : ParticleFilterTracker () + { + tracker_name_ = "ParticleFilterOMPTracker"; + + setNumberOfThreads(nr_threads); + } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic) + */ + void + setNumberOfThreads (unsigned int nr_threads = 0); + + protected: + /** \brief The number of threads the scheduler should use. */ + unsigned int threads_; + + /** \brief weighting phase of particle filter method. + calculate the likelihood of all of the particles and set the weights. + */ + void weight () override; + + }; + } +} + +//#include +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/include/pcl-1.10/pcl/tracking/pyramidal_klt.h b/deps_install/include/pcl-1.10/pcl/tracking/pyramidal_klt.h new file mode 100755 index 0000000..3e379fb --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/pyramidal_klt.h @@ -0,0 +1,378 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace tracking + { + /** Pyramidal Kanade Lucas Tomasi tracker. + * This is an implementation of the Pyramidal Kanade Lucas Tomasi tracker that operates on + * organized 3D keypoints with color/intensity information (this is the default behaviour but you + * can alterate it by providing another operator as second template argument). It is an affine + * tracker that iteratively computes the optical flow to find the best guess for a point p at t + * given its location at t-1. + * User is advised to respect the Tomasi condition: the response computed is the maximum eigenvalue + * of the second moment matrix but no restrictin are applied to points to track so you can use a + * detector of your choice to indicate points to track. + * + * \author Nizar Sallem + */ + template > + class PyramidalKLTTracker : public Tracker + { + public: + using TrackerBase = pcl::tracking::Tracker; + using PointCloudIn = typename TrackerBase::PointCloudIn; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + using FloatImage = pcl::PointCloud; + using FloatImagePtr = FloatImage::Ptr; + using FloatImageConstPtr = FloatImage::ConstPtr; + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + using TrackerBase::tracker_name_; + using TrackerBase::input_; + using TrackerBase::indices_; + + /// Constructor + PyramidalKLTTracker (int nb_levels = 5, int tracking_window_width = 7, int tracking_window_height = 7) + : ref_ () + , nb_levels_ (nb_levels) + , track_width_ (tracking_window_width) + , track_height_ (tracking_window_height) + , threads_ (0) + , initialized_ (false) + { + tracker_name_ = "PyramidalKLTTracker"; + accuracy_ = 0.1; + epsilon_ = 1e-3; + max_iterations_ = 10; + keypoints_nbr_ = 100; + min_eigenvalue_threshold_ = 1e-4; + kernel_ << 1.f/16 ,1.f/4 ,3.f/8 ,1.f/4 ,1.f/16; + kernel_size_2_ = kernel_.size () / 2; + kernel_last_ = kernel_.size () -1; + } + + /// Destructor + ~PyramidalKLTTracker () {} + + /** \brief Set the number of pyramid levels + * \param levels desired number of pyramid levels + */ + inline void + setNumberOfPyramidLevels (int levels) { nb_levels_ = levels; } + + /// \brief \return the number of pyramid levels + inline int + getNumberOfPyramidLevels () const { return (nb_levels_); } + + /** Set accuracy + * \param[in] accuracy desired accuracy. + */ + inline void + setAccuracy (float accuracy) { accuracy_ = accuracy; } + + /// \return the accuracy + inline float + getAccuracy () const { return (accuracy_); } + + /** Set epsilon + * \param[in] epsilon desired epsilon. + */ + inline void + setEpsilon (float epsilon) { epsilon_ = epsilon; } + + /// \return the epsilon + inline float + getEpsilon () const { return (epsilon_); } + + /** \brief Set the maximum number of points to track. Only the first keypoints_nbr_ + * are used as points to track after sorting detected keypoints according to their + * response measure. + * \param[in] number the desired number of points to detect. + */ + inline void + setNumberOfKeypoints (std::size_t number) { keypoints_nbr_ = number; } + + /// \return the maximum number of keypoints to keep + inline std::size_t + getNumberOfKeypoints () { return (keypoints_nbr_); } + + /** \brief set the tracking window size + * \param[in] width the tracking window width + * \param[in] height the tracking window height + */ + inline void + setTrackingWindowSize (int width, int height); + + /// \brief Set tracking window width + inline void + setTrackingWindowWidth (int width) {track_width_ = width; }; + + /// \brief \return the tracking window size + inline int + getTrackingWindowWidth () { return (track_width_); } + + /// \brief Set tracking window height + inline void + setTrackingWindowHeight (int height) {track_height_ = height; }; + + /// \brief \return the tracking window size + inline int + getTrackingWindowHeight () { return (track_height_); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to + * automatic). + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /** \brief Get a pointer of the cloud at t-1. */ + inline PointCloudInConstPtr + getReferenceCloud () const { return (ref_); } + + /** \brief Set the maximum number of iterations in the Lucas Kanade loop. + * \param[in] max the desired maximum number of iterations + */ + inline void + setMaxIterationsNumber (unsigned int max) { max_iterations_ = max; } + + /// \brief \return the maximum iterations number + inline unsigned int + getMaxIterationsNumber () const { return (max_iterations_); } + + /** \brief Provide a pointer to points to track. + * \param points the const boost shared pointer to a PointIndices message + */ + inline void + setPointsToTrack (const pcl::PointIndicesConstPtr& points); + + /** \brief Provide a pointer to points to track. + * \param points the const boost shared pointer to a PointIndices message + */ + inline void + setPointsToTrack (const pcl::PointCloud::ConstPtr& points); + + /// \brief \return a pointer to the points successfully tracked. + inline pcl::PointCloud::ConstPtr + getTrackedPoints () const { return (keypoints_); }; + + /** \brief \return the status of points to track. + * Status == 0 --> points successfully tracked; + * Status < 0 --> point is lost; + * Status == -1 --> point is out of bond; + * Status == -2 --> optical flow can not be computed for this point. + */ + inline pcl::PointIndicesConstPtr + getPointsToTrackStatus () const { return (keypoints_status_); } + + /** \brief Return the computed transformation from tracked points. */ + Eigen::Affine3f + getResult () const override { return (motion_); } + + /// \brief \return initialization state + bool + getInitialized () const { return (initialized_); } + + protected: + bool + initCompute () override; + + /** \brief compute Scharr derivatives of a source cloud. + * \param[in] src the image for which gradients are to be computed + * \param[out] grad_x image gradient along X direction + * \param[out] grad_y image gradient along Y direction + */ + void + derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const; + + /** \brief downsample input + * \param[in] input the image to downsample + * \param[out] output the downsampled image + */ + void + downsample (const FloatImageConstPtr& input, FloatImageConstPtr& output) const; + + /** \brief downsample input and compute output gradients. + * \param[in] input the image to downsample + * \param[out] output the downsampled image + * \param[out] output_grad_x downsampled image gradient along X direction + * \param[out] output_grad_y downsampled image gradient along Y direction + */ + void + downsample (const FloatImageConstPtr& input, FloatImageConstPtr& output, + FloatImageConstPtr& output_grad_x, FloatImageConstPtr& output_grad_y) const; + + /** \brief Separately convolve image with decomposable convolution kernel. + * \param[in] input input the image to convolve + * \param[out] output output the convolved image + */ + void + convolve (const FloatImageConstPtr& input, FloatImage& output) const; + + /** \brief Convolve image columns. + * \param[in] input input the image to convolve + * \param[out] output output the convolved image + */ + void + convolveCols (const FloatImageConstPtr& input, FloatImage& output) const; + + /** \brief Convolve image rows. + * \param[in] input input the image to convolve + * \param[out] output output the convolved image + */ + void + convolveRows (const FloatImageConstPtr& input, FloatImage& output) const; + + /** \brief extract the patch from the previous image, previous image gradients surrounding + * pixel alocation while interpolating image and gradients data and compute covariation + * matrix of derivatives. + * \param[in] img original image + * \param[in] grad_x original image gradient along X direction + * \param[in] grad_y original image gradient along Y direction + * \param[in] location pixel at the center of the patch + * \param[in] weights bilinear interpolation weights at this location computed from subpixel + * location + * \param[out] win patch with interpolated intensity values + * \param[out] grad_x_win patch with interpolated gradient along X values + * \param[out] grad_y_win patch with interpolated gradient along Y values + * \param[out] covariance covariance matrix coefficients + */ + virtual void + spatialGradient (const FloatImage& img, + const FloatImage& grad_x, + const FloatImage& grad_y, + const Eigen::Array2i& location, + const Eigen::Array4f& weights, + Eigen::ArrayXXf& win, + Eigen::ArrayXXf& grad_x_win, + Eigen::ArrayXXf& grad_y_win, + Eigen::Array3f & covariance) const; + void + mismatchVector (const Eigen::ArrayXXf& prev, + const Eigen::ArrayXXf& prev_grad_x, + const Eigen::ArrayXXf& prev_grad_y, + const FloatImage& next, + const Eigen::Array2i& location, + const Eigen::Array4f& weights, + Eigen::Array2f &b) const; + + /** \brief Compute the pyramidal representation of an image. + * \param[in] input the input cloud + * \param[out] pyramid computed pyramid levels along with their respective gradients + * \param[in] border_type + */ + virtual void + computePyramids (const PointCloudInConstPtr& input, + std::vector& pyramid, + pcl::InterpolationType border_type) const; + + virtual void + track (const PointCloudInConstPtr& previous_input, + const PointCloudInConstPtr& current_input, + const std::vector& previous_pyramid, + const std::vector& current_pyramid, + const pcl::PointCloud::ConstPtr& previous_keypoints, + pcl::PointCloud::Ptr& current_keypoints, + std::vector& status, + Eigen::Affine3f& motion) const; + + void + computeTracking () override; + + /// \brief input pyranid at t-1 + std::vector ref_pyramid_; + /// \brief point cloud at t-1 + PointCloudInConstPtr ref_; + /// \brief number of pyramid levels + int nb_levels_; + /// \brief detected keypoints 2D coordinates + pcl::PointCloud::ConstPtr keypoints_; + /// \brief status of keypoints of t-1 at t + pcl::PointIndicesPtr keypoints_status_; + /// \brief number of points to detect + std::size_t keypoints_nbr_; + /// \brief tracking width + int track_width_; + /// \brief half of tracking window width + int track_width_2_; + /// \brief tracking height + int track_height_; + /// \brief half of tracking window height + int track_height_2_; + /// \brief maximum number of iterations + unsigned int max_iterations_; + /// \brief accuracy criterion to stop iterating + float accuracy_; + float min_eigenvalue_threshold_; + /// \brief epsilon for subpixel computation + float epsilon_; + float max_residue_; + /// \brief number of hardware threads + unsigned int threads_; + /// \brief intensity accessor + IntensityT intensity_; + /// \brief is the tracker initialized ? + bool initialized_; + /// \brief compute transformation from successfully tracked points + pcl::TransformationFromCorrespondences transformation_computer_; + /// \brief computed transformation between tracked points + Eigen::Affine3f motion_; + /// \brief smoothing kernel + Eigen::Array kernel_; + /// \brief smoothing kernel half size + int kernel_size_2_; + /// \brief index of last element in kernel + int kernel_last_; + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/tracking/tracker.h b/deps_install/include/pcl-1.10/pcl/tracking/tracker.h new file mode 100755 index 0000000..0308e16 --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/tracker.h @@ -0,0 +1,134 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: point_cloud.h 4696 2012-02-23 06:12:55Z rusu $ + * + */ + +#pragma once + +#include +#include +#include +#include + +namespace pcl +{ + namespace tracking + { + /** \brief @b Tracker represents the base tracker class. + * \author Ryohei Ueda + * \ingroup tracking + */ + template + class Tracker: public PCLBase + { + protected: + using PCLBase::deinitCompute; + + public: + using PCLBase::indices_; + using PCLBase::input_; + + using BaseClass = PCLBase; + using Ptr = shared_ptr< Tracker >; + using ConstPtr = shared_ptr< const Tracker >; + + using SearchPtr = typename pcl::search::Search::Ptr; + using SearchConstPtr = typename pcl::search::Search::ConstPtr; + + using PointCloudIn = pcl::PointCloud; + using PointCloudInPtr = typename PointCloudIn::Ptr; + using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; + + using PointCloudState = pcl::PointCloud; + using PointCloudStatePtr = typename PointCloudState::Ptr; + using PointCloudStateConstPtr = typename PointCloudState::ConstPtr; + + public: + /** \brief Empty constructor. */ + Tracker (): search_ () {} + + /** \brief Base method for tracking for all points given in + * using the indices in setIndices () + */ + void + compute (); + + protected: + /** \brief The tracker name. */ + std::string tracker_name_; + + /** \brief A pointer to the spatial search object. */ + SearchPtr search_; + + /** \brief Get a string representation of the name of this class. */ + inline const std::string& + getClassName () const { return (tracker_name_); } + + /** \brief This method should get called before starting the actual computation. */ + virtual bool + initCompute (); + + /** \brief Provide a pointer to a dataset to add additional information + * to estimate the features for every point in the input dataset. This + * is optional, if this is not set, it will only use the data in the + * input cloud to estimate the features. This is useful when you only + * need to compute the features for a downsampled cloud. + * \param search a pointer to a PointCloud message + */ + inline void + setSearchMethod (const SearchPtr &search) { search_ = search; } + + /** \brief Get a pointer to the point cloud dataset. */ + inline SearchPtr + getSearchMethod () { return (search_); } + + /** \brief Get an instance of the result of tracking. */ + virtual StateT + getResult () const = 0; + + private: + /** \brief Abstract tracking method. */ + virtual void + computeTracking () = 0; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + } +} + +#include diff --git a/deps_install/include/pcl-1.10/pcl/tracking/tracking.h b/deps_install/include/pcl-1.10/pcl/tracking/tracking.h new file mode 100755 index 0000000..110e14e --- /dev/null +++ b/deps_install/include/pcl-1.10/pcl/tracking/tracking.h @@ -0,0 +1,132 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: point_cloud.h 4696 2012-02-23 06:12:55Z rusu $ + * + */ + +#pragma once + +#include + +#ifdef BUILD_Maintainer +# if defined __GNUC__ +# pragma GCC system_header +# elif defined _MSC_VER +# pragma warning(push, 1) +# endif +#endif + +namespace pcl +{ + namespace tracking + { + /* state definition */ + struct ParticleXYZRPY; + struct ParticleXYR; + + /* \brief return the value of normal distribution */ + PCL_EXPORTS double + sampleNormal (double mean, double sigma); + } +} + +#include + +// ============================== +// =====POINT_CLOUD_REGISTER===== +// ============================== +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::tracking::_ParticleXYZRPY, + (float, x, x) + (float, y, y) + (float, z, z) + (float, roll, roll) + (float, pitch, pitch) + (float, yaw, yaw) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleXYZRPY, pcl::tracking::_ParticleXYZRPY) + + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::tracking::_ParticleXYRPY, + (float, x, x) + (float, y, y) + (float, z, z) + (float, roll, roll) + (float, pitch, pitch) + (float, yaw, yaw) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleXYRPY, pcl::tracking::_ParticleXYRPY) + + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::tracking::_ParticleXYRP, + (float, x, x) + (float, y, y) + (float, z, z) + (float, roll, roll) + (float, pitch, pitch) + (float, yaw, yaw) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleXYRP, pcl::tracking::_ParticleXYRP) + + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::tracking::_ParticleXYR, + (float, x, x) + (float, y, y) + (float, z, z) + (float, roll, roll) + (float, pitch, pitch) + (float, yaw, yaw) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleXYR, pcl::tracking::_ParticleXYR) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::tracking::_ParticleXYZR, + (float, x, x) + (float, y, y) + (float, z, z) + (float, roll, roll) + (float, pitch, pitch) + (float, yaw, yaw) +) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleXYZR, pcl::tracking::_ParticleXYZR) + +#ifdef BUILD_Maintainer +# if defined _MSC_VER +# pragma warning(pop) +# endif +#endif + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/deps_install/lib/libpcl_common.so b/deps_install/lib/libpcl_common.so new file mode 120000 index 0000000..7064a1b --- /dev/null +++ b/deps_install/lib/libpcl_common.so @@ -0,0 +1 @@ +libpcl_common.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_common.so.1.10 b/deps_install/lib/libpcl_common.so.1.10 new file mode 120000 index 0000000..cb586c1 --- /dev/null +++ b/deps_install/lib/libpcl_common.so.1.10 @@ -0,0 +1 @@ +libpcl_common.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_common.so.1.10.1 b/deps_install/lib/libpcl_common.so.1.10.1 new file mode 100755 index 0000000..5768ae0 Binary files /dev/null and b/deps_install/lib/libpcl_common.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_features.so b/deps_install/lib/libpcl_features.so new file mode 120000 index 0000000..07d1e46 --- /dev/null +++ b/deps_install/lib/libpcl_features.so @@ -0,0 +1 @@ +libpcl_features.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_features.so.1.10 b/deps_install/lib/libpcl_features.so.1.10 new file mode 120000 index 0000000..005abf1 --- /dev/null +++ b/deps_install/lib/libpcl_features.so.1.10 @@ -0,0 +1 @@ +libpcl_features.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_features.so.1.10.1 b/deps_install/lib/libpcl_features.so.1.10.1 new file mode 100755 index 0000000..5508322 Binary files /dev/null and b/deps_install/lib/libpcl_features.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_filters.so b/deps_install/lib/libpcl_filters.so new file mode 120000 index 0000000..7c4cbf8 --- /dev/null +++ b/deps_install/lib/libpcl_filters.so @@ -0,0 +1 @@ +libpcl_filters.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_filters.so.1.10 b/deps_install/lib/libpcl_filters.so.1.10 new file mode 120000 index 0000000..cd8e1d0 --- /dev/null +++ b/deps_install/lib/libpcl_filters.so.1.10 @@ -0,0 +1 @@ +libpcl_filters.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_filters.so.1.10.1 b/deps_install/lib/libpcl_filters.so.1.10.1 new file mode 100755 index 0000000..361ed99 Binary files /dev/null and b/deps_install/lib/libpcl_filters.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_io.so b/deps_install/lib/libpcl_io.so new file mode 120000 index 0000000..971197e --- /dev/null +++ b/deps_install/lib/libpcl_io.so @@ -0,0 +1 @@ +libpcl_io.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_io.so.1.10 b/deps_install/lib/libpcl_io.so.1.10 new file mode 120000 index 0000000..53c1f2e --- /dev/null +++ b/deps_install/lib/libpcl_io.so.1.10 @@ -0,0 +1 @@ +libpcl_io.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_io.so.1.10.1 b/deps_install/lib/libpcl_io.so.1.10.1 new file mode 100755 index 0000000..9d66118 Binary files /dev/null and b/deps_install/lib/libpcl_io.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_io_ply.so b/deps_install/lib/libpcl_io_ply.so new file mode 120000 index 0000000..6fc6768 --- /dev/null +++ b/deps_install/lib/libpcl_io_ply.so @@ -0,0 +1 @@ +libpcl_io_ply.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_io_ply.so.1.10 b/deps_install/lib/libpcl_io_ply.so.1.10 new file mode 120000 index 0000000..b108547 --- /dev/null +++ b/deps_install/lib/libpcl_io_ply.so.1.10 @@ -0,0 +1 @@ +libpcl_io_ply.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_io_ply.so.1.10.1 b/deps_install/lib/libpcl_io_ply.so.1.10.1 new file mode 100755 index 0000000..c77b99b Binary files /dev/null and b/deps_install/lib/libpcl_io_ply.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_kdtree.so b/deps_install/lib/libpcl_kdtree.so new file mode 120000 index 0000000..5c6a3ff --- /dev/null +++ b/deps_install/lib/libpcl_kdtree.so @@ -0,0 +1 @@ +libpcl_kdtree.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_kdtree.so.1.10 b/deps_install/lib/libpcl_kdtree.so.1.10 new file mode 120000 index 0000000..03dd0b6 --- /dev/null +++ b/deps_install/lib/libpcl_kdtree.so.1.10 @@ -0,0 +1 @@ +libpcl_kdtree.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_kdtree.so.1.10.1 b/deps_install/lib/libpcl_kdtree.so.1.10.1 new file mode 100755 index 0000000..9704d3a Binary files /dev/null and b/deps_install/lib/libpcl_kdtree.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_keypoints.so b/deps_install/lib/libpcl_keypoints.so new file mode 120000 index 0000000..7690769 --- /dev/null +++ b/deps_install/lib/libpcl_keypoints.so @@ -0,0 +1 @@ +libpcl_keypoints.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_keypoints.so.1.10 b/deps_install/lib/libpcl_keypoints.so.1.10 new file mode 120000 index 0000000..0e3da49 --- /dev/null +++ b/deps_install/lib/libpcl_keypoints.so.1.10 @@ -0,0 +1 @@ +libpcl_keypoints.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_keypoints.so.1.10.1 b/deps_install/lib/libpcl_keypoints.so.1.10.1 new file mode 100755 index 0000000..323b100 Binary files /dev/null and b/deps_install/lib/libpcl_keypoints.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_ml.so b/deps_install/lib/libpcl_ml.so new file mode 120000 index 0000000..3022608 --- /dev/null +++ b/deps_install/lib/libpcl_ml.so @@ -0,0 +1 @@ +libpcl_ml.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_ml.so.1.10 b/deps_install/lib/libpcl_ml.so.1.10 new file mode 120000 index 0000000..344df91 --- /dev/null +++ b/deps_install/lib/libpcl_ml.so.1.10 @@ -0,0 +1 @@ +libpcl_ml.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_ml.so.1.10.1 b/deps_install/lib/libpcl_ml.so.1.10.1 new file mode 100755 index 0000000..d8073e7 Binary files /dev/null and b/deps_install/lib/libpcl_ml.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_octree.so b/deps_install/lib/libpcl_octree.so new file mode 120000 index 0000000..31269d6 --- /dev/null +++ b/deps_install/lib/libpcl_octree.so @@ -0,0 +1 @@ +libpcl_octree.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_octree.so.1.10 b/deps_install/lib/libpcl_octree.so.1.10 new file mode 120000 index 0000000..eaf9aa0 --- /dev/null +++ b/deps_install/lib/libpcl_octree.so.1.10 @@ -0,0 +1 @@ +libpcl_octree.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_octree.so.1.10.1 b/deps_install/lib/libpcl_octree.so.1.10.1 new file mode 100755 index 0000000..2495c07 Binary files /dev/null and b/deps_install/lib/libpcl_octree.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_recognition.so b/deps_install/lib/libpcl_recognition.so new file mode 120000 index 0000000..4148efe --- /dev/null +++ b/deps_install/lib/libpcl_recognition.so @@ -0,0 +1 @@ +libpcl_recognition.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_recognition.so.1.10 b/deps_install/lib/libpcl_recognition.so.1.10 new file mode 120000 index 0000000..246a794 --- /dev/null +++ b/deps_install/lib/libpcl_recognition.so.1.10 @@ -0,0 +1 @@ +libpcl_recognition.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_recognition.so.1.10.1 b/deps_install/lib/libpcl_recognition.so.1.10.1 new file mode 100755 index 0000000..d979897 Binary files /dev/null and b/deps_install/lib/libpcl_recognition.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_registration.so b/deps_install/lib/libpcl_registration.so new file mode 120000 index 0000000..708d8bd --- /dev/null +++ b/deps_install/lib/libpcl_registration.so @@ -0,0 +1 @@ +libpcl_registration.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_registration.so.1.10 b/deps_install/lib/libpcl_registration.so.1.10 new file mode 120000 index 0000000..eceec79 --- /dev/null +++ b/deps_install/lib/libpcl_registration.so.1.10 @@ -0,0 +1 @@ +libpcl_registration.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_registration.so.1.10.1 b/deps_install/lib/libpcl_registration.so.1.10.1 new file mode 100755 index 0000000..acfbc1d Binary files /dev/null and b/deps_install/lib/libpcl_registration.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_sample_consensus.so b/deps_install/lib/libpcl_sample_consensus.so new file mode 120000 index 0000000..4f3e2aa --- /dev/null +++ b/deps_install/lib/libpcl_sample_consensus.so @@ -0,0 +1 @@ +libpcl_sample_consensus.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_sample_consensus.so.1.10 b/deps_install/lib/libpcl_sample_consensus.so.1.10 new file mode 120000 index 0000000..6d8fd26 --- /dev/null +++ b/deps_install/lib/libpcl_sample_consensus.so.1.10 @@ -0,0 +1 @@ +libpcl_sample_consensus.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_sample_consensus.so.1.10.1 b/deps_install/lib/libpcl_sample_consensus.so.1.10.1 new file mode 100755 index 0000000..f16642a Binary files /dev/null and b/deps_install/lib/libpcl_sample_consensus.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_search.so b/deps_install/lib/libpcl_search.so new file mode 120000 index 0000000..0f64b95 --- /dev/null +++ b/deps_install/lib/libpcl_search.so @@ -0,0 +1 @@ +libpcl_search.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_search.so.1.10 b/deps_install/lib/libpcl_search.so.1.10 new file mode 120000 index 0000000..9fd63a6 --- /dev/null +++ b/deps_install/lib/libpcl_search.so.1.10 @@ -0,0 +1 @@ +libpcl_search.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_search.so.1.10.1 b/deps_install/lib/libpcl_search.so.1.10.1 new file mode 100755 index 0000000..97cbfaf Binary files /dev/null and b/deps_install/lib/libpcl_search.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_segmentation.so b/deps_install/lib/libpcl_segmentation.so new file mode 120000 index 0000000..5bb0ef6 --- /dev/null +++ b/deps_install/lib/libpcl_segmentation.so @@ -0,0 +1 @@ +libpcl_segmentation.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_segmentation.so.1.10 b/deps_install/lib/libpcl_segmentation.so.1.10 new file mode 120000 index 0000000..09abaf7 --- /dev/null +++ b/deps_install/lib/libpcl_segmentation.so.1.10 @@ -0,0 +1 @@ +libpcl_segmentation.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_segmentation.so.1.10.1 b/deps_install/lib/libpcl_segmentation.so.1.10.1 new file mode 100755 index 0000000..d2aafb5 Binary files /dev/null and b/deps_install/lib/libpcl_segmentation.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_stereo.so b/deps_install/lib/libpcl_stereo.so new file mode 120000 index 0000000..57ef159 --- /dev/null +++ b/deps_install/lib/libpcl_stereo.so @@ -0,0 +1 @@ +libpcl_stereo.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_stereo.so.1.10 b/deps_install/lib/libpcl_stereo.so.1.10 new file mode 120000 index 0000000..a1d99ae --- /dev/null +++ b/deps_install/lib/libpcl_stereo.so.1.10 @@ -0,0 +1 @@ +libpcl_stereo.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_stereo.so.1.10.1 b/deps_install/lib/libpcl_stereo.so.1.10.1 new file mode 100755 index 0000000..0ad042f Binary files /dev/null and b/deps_install/lib/libpcl_stereo.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_surface.so b/deps_install/lib/libpcl_surface.so new file mode 120000 index 0000000..42db42f --- /dev/null +++ b/deps_install/lib/libpcl_surface.so @@ -0,0 +1 @@ +libpcl_surface.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_surface.so.1.10 b/deps_install/lib/libpcl_surface.so.1.10 new file mode 120000 index 0000000..c3e4577 --- /dev/null +++ b/deps_install/lib/libpcl_surface.so.1.10 @@ -0,0 +1 @@ +libpcl_surface.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_surface.so.1.10.1 b/deps_install/lib/libpcl_surface.so.1.10.1 new file mode 100755 index 0000000..ca18710 Binary files /dev/null and b/deps_install/lib/libpcl_surface.so.1.10.1 differ diff --git a/deps_install/lib/libpcl_tracking.so b/deps_install/lib/libpcl_tracking.so new file mode 120000 index 0000000..e5b5d8b --- /dev/null +++ b/deps_install/lib/libpcl_tracking.so @@ -0,0 +1 @@ +libpcl_tracking.so.1.10 \ No newline at end of file diff --git a/deps_install/lib/libpcl_tracking.so.1.10 b/deps_install/lib/libpcl_tracking.so.1.10 new file mode 120000 index 0000000..7aee6cb --- /dev/null +++ b/deps_install/lib/libpcl_tracking.so.1.10 @@ -0,0 +1 @@ +libpcl_tracking.so.1.10.1 \ No newline at end of file diff --git a/deps_install/lib/libpcl_tracking.so.1.10.1 b/deps_install/lib/libpcl_tracking.so.1.10.1 new file mode 100755 index 0000000..8067495 Binary files /dev/null and b/deps_install/lib/libpcl_tracking.so.1.10.1 differ diff --git a/deps_install/lib/pkgconfig/pcl_2d-1.10.pc b/deps_install/lib/pkgconfig/pcl_2d-1.10.pc new file mode 100755 index 0000000..38f1326 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_2d-1.10.pc @@ -0,0 +1,12 @@ +# This file was generated by CMake for PCL library pcl_2d +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_2d +Description: Point cloud 2d +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_filters-1.10 +Libs: +Cflags: -I${includedir} diff --git a/deps_install/lib/pkgconfig/pcl_common-1.10.pc b/deps_install/lib/pkgconfig/pcl_common-1.10.pc new file mode 100755 index 0000000..680d26b --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_common-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_common +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_common +Description: Point cloud common library +Version: 1.10.1 +Requires: +Libs: -L${libdir} -lpcl_common +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_features-1.10.pc b/deps_install/lib/pkgconfig/pcl_features-1.10.pc new file mode 100755 index 0000000..4746a5b --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_features-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_features +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_features +Description: Point cloud features library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_search-1.10 pcl_kdtree-1.10 pcl_octree-1.10 pcl_filters-1.10 pcl_2d-1.10 +Libs: -L${libdir} -lpcl_features +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_filters-1.10.pc b/deps_install/lib/pkgconfig/pcl_filters-1.10.pc new file mode 100755 index 0000000..f094b97 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_filters-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_filters +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_filters +Description: Point cloud filters library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_sample_consensus-1.10 pcl_search-1.10 pcl_kdtree-1.10 pcl_octree-1.10 +Libs: -L${libdir} -lpcl_filters +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_geometry-1.10.pc b/deps_install/lib/pkgconfig/pcl_geometry-1.10.pc new file mode 100755 index 0000000..49351bf --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_geometry-1.10.pc @@ -0,0 +1,12 @@ +# This file was generated by CMake for PCL library pcl_geometry +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_geometry +Description: Point cloud geometry library +Version: 1.10.1 +Requires: pcl_common-1.10 +Libs: +Cflags: -I${includedir} diff --git a/deps_install/lib/pkgconfig/pcl_io-1.10.pc b/deps_install/lib/pkgconfig/pcl_io-1.10.pc new file mode 100755 index 0000000..9ce8671 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_io-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_io +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_io +Description: Point cloud IO library +Version: 1.10.1 +Requires: eigen3 libopenni libopenni2 pcl_common-1.10 pcl_octree-1.10 +Libs: -L${libdir} -lpcl_io +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_kdtree-1.10.pc b/deps_install/lib/pkgconfig/pcl_kdtree-1.10.pc new file mode 100755 index 0000000..25c9be9 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_kdtree-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_kdtree +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_kdtree +Description: Point cloud kd-tree library +Version: 1.10.1 +Requires: flann pcl_common-1.10 +Libs: -L${libdir} -lpcl_kdtree +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_keypoints-1.10.pc b/deps_install/lib/pkgconfig/pcl_keypoints-1.10.pc new file mode 100755 index 0000000..da8dce9 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_keypoints-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_keypoints +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_keypoints +Description: Point cloud keypoints library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_search-1.10 pcl_kdtree-1.10 pcl_octree-1.10 pcl_features-1.10 pcl_filters-1.10 +Libs: -L${libdir} -lpcl_keypoints +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_ml-1.10.pc b/deps_install/lib/pkgconfig/pcl_ml-1.10.pc new file mode 100755 index 0000000..d339560 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_ml-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_ml +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_ml +Description: Point cloud machine learning library +Version: 1.10.1 +Requires: pcl_common-1.10 +Libs: -L${libdir} -lpcl_ml +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_octree-1.10.pc b/deps_install/lib/pkgconfig/pcl_octree-1.10.pc new file mode 100755 index 0000000..c46ca47 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_octree-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_octree +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_octree +Description: Point cloud octree library +Version: 1.10.1 +Requires: pcl_common-1.10 +Libs: -L${libdir} -lpcl_octree +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_recognition-1.10.pc b/deps_install/lib/pkgconfig/pcl_recognition-1.10.pc new file mode 100755 index 0000000..1bc648e --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_recognition-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_recognition +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_recognition +Description: Point cloud recognition library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_io-1.10 pcl_search-1.10 pcl_kdtree-1.10 pcl_octree-1.10 pcl_features-1.10 pcl_filters-1.10 pcl_registration-1.10 pcl_sample_consensus-1.10 pcl_ml-1.10 +Libs: -L${libdir} -lpcl_recognition +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_registration-1.10.pc b/deps_install/lib/pkgconfig/pcl_registration-1.10.pc new file mode 100755 index 0000000..c8799b6 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_registration-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_registration +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_registration +Description: Point cloud registration library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_octree-1.10 pcl_kdtree-1.10 pcl_search-1.10 pcl_sample_consensus-1.10 pcl_features-1.10 pcl_filters-1.10 +Libs: -L${libdir} -lpcl_registration +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_sample_consensus-1.10.pc b/deps_install/lib/pkgconfig/pcl_sample_consensus-1.10.pc new file mode 100755 index 0000000..7d7f365 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_sample_consensus-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_sample_consensus +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_sample_consensus +Description: Point cloud sample consensus library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_search-1.10 +Libs: -L${libdir} -lpcl_sample_consensus +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_search-1.10.pc b/deps_install/lib/pkgconfig/pcl_search-1.10.pc new file mode 100755 index 0000000..6a88356 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_search-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_search +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_search +Description: Point cloud generic search library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_kdtree-1.10 pcl_octree-1.10 +Libs: -L${libdir} -lpcl_search +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_segmentation-1.10.pc b/deps_install/lib/pkgconfig/pcl_segmentation-1.10.pc new file mode 100755 index 0000000..0700826 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_segmentation-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_segmentation +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_segmentation +Description: Point cloud segmentation library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_geometry-1.10 pcl_search-1.10 pcl_sample_consensus-1.10 pcl_kdtree-1.10 pcl_octree-1.10 pcl_features-1.10 pcl_filters-1.10 pcl_ml-1.10 +Libs: -L${libdir} -lpcl_segmentation +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_stereo-1.10.pc b/deps_install/lib/pkgconfig/pcl_stereo-1.10.pc new file mode 100755 index 0000000..d71adb2 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_stereo-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_stereo +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_stereo +Description: Point cloud stereo library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_io-1.10 +Libs: -L${libdir} -lpcl_stereo +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_surface-1.10.pc b/deps_install/lib/pkgconfig/pcl_surface-1.10.pc new file mode 100755 index 0000000..f6b20f4 --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_surface-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_surface +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_surface +Description: Point cloud surface library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_search-1.10 pcl_kdtree-1.10 pcl_octree-1.10 +Libs: -L${libdir} -lpcl_surface +Cflags: -I${includedir} + diff --git a/deps_install/lib/pkgconfig/pcl_tracking-1.10.pc b/deps_install/lib/pkgconfig/pcl_tracking-1.10.pc new file mode 100755 index 0000000..0909c2b --- /dev/null +++ b/deps_install/lib/pkgconfig/pcl_tracking-1.10.pc @@ -0,0 +1,13 @@ +# This file was generated by CMake for PCL library pcl_tracking +prefix=/home/solomon/depends_install +exec_prefix=${prefix} +libdir=${prefix}/lib +#includedir=${prefix}/include/pcl-1.10/pcl +includedir=${prefix}/include/pcl-1.10 +Name: pcl_tracking +Description: Point cloud tracking library +Version: 1.10.1 +Requires: pcl_common-1.10 pcl_search-1.10 pcl_kdtree-1.10 pcl_filters-1.10 pcl_octree-1.10 +Libs: -L${libdir} -lpcl_tracking +Cflags: -I${includedir} + diff --git a/deps_install/share/pcl-1.10/Modules/FindClangFormat.cmake b/deps_install/share/pcl-1.10/Modules/FindClangFormat.cmake new file mode 100755 index 0000000..0773e99 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindClangFormat.cmake @@ -0,0 +1,72 @@ +# +# .rst: FindClangFormat +# --------------- +# +# The module defines the following variables +# +# ``ClangFormat_EXECUTABLE`` Path to clang-format executable +# ``ClangFormat_FOUND`` True if the clang-format executable was found. +# ``ClangFormat_VERSION`` The version of clang-format found +# +# Example usage: +# +# .. code-block:: cmake +# +# find_package(ClangFormat) +# if(ClangFormat_FOUND) +# message("clang-format executable found: ${ClangFormat_EXECUTABLE}\n" +# "version: ${ClangFormat_VERSION}") +# endif() + +find_program(ClangFormat_EXECUTABLE + NAMES + clang-format-9 + clang-format-9.0 + clang-format-8 + clang-format-8.0 + clang-format-7 + clang-format-7.0 + clang-format-6.0 + clang-format-5.0 + clang-format-4.0 + clang-format-3.9 + clang-format-3.8 + clang-format-3.7 + clang-format-3.6 + clang-format-3.5 + clang-format-3.4 + clang-format-3.3 + clang-format # least priority + DOC "clang-format executable") +mark_as_advanced(ClangFormat_EXECUTABLE) + +# Extract version from command "clang-format -version" +if(ClangFormat_EXECUTABLE) + execute_process(COMMAND ${ClangFormat_EXECUTABLE} -version + OUTPUT_VARIABLE clang_format_version + ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) +endif() +set(version_regex "^.*clang-format version ([.0-9]+).*") +if(clang_format_version MATCHES ${version_regex}) + # clang_format_version samples: + # * clang-format version 3.9.1-4ubuntu3~16.04.1 (tags/RELEASE_391/rc2) + # * Alpine clang-format version 8.0.0 (tags/RELEASE_800/final) (based on LLVM 8.0.0) + string(REGEX + REPLACE ${version_regex} + "\\1" + ClangFormat_VERSION + "${clang_format_version}") + # ClangFormat_VERSION sample: "3.9.1", "8.0.0" +endif() +unset(clang_format_version) +unset(version_regex) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args( + ClangFormat + REQUIRED_VARS + ClangFormat_EXECUTABLE + ClangFormat_VERSION + VERSION_VAR + ClangFormat_VERSION +) diff --git a/deps_install/share/pcl-1.10/Modules/FindDSSDK.cmake b/deps_install/share/pcl-1.10/Modules/FindDSSDK.cmake new file mode 100755 index 0000000..3212a0c --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindDSSDK.cmake @@ -0,0 +1,59 @@ +############################################################################### +# Find SoftKinetic DepthSense SDK +# +# find_package(DSSDK) +# +# Variables defined by this module: +# +# DSSDK_FOUND True if DepthSense SDK was found +# DSSDK_VERSION The version of DepthSense SDK +# DSSDK_INCLUDE_DIRS The location(s) of DepthSense SDK headers +# DSSDK_LIBRARIES Libraries needed to use DepthSense SDK + +find_path(DSSDK_DIR include/DepthSenseVersion.hxx + HINTS "$ENV{DEPTHSENSESDK32}" + "$ENV{DEPTHSENSESDK64}" + PATHS "$ENV{PROGRAMFILES}/SoftKinetic/DepthSenseSDK" + "$ENV{PROGRAMW6432}/SoftKinetic/DepthSenseSDK" + "C:/Program Files (x86)/SoftKinetic/DepthSenseSDK" + "C:/Program Files/SoftKinetic/DepthSenseSDK" + "/opt/softkinetic/DepthSenseSDK" + DOC "DepthSense SDK directory") + +if(DSSDK_DIR) + + # Include directories + set(DSSDK_INCLUDE_DIRS ${DSSDK_DIR}/include) + mark_as_advanced(DSSDK_INCLUDE_DIRS) + + # Libraries + if(MSVC) + set(DSSDK_LIBRARIES_NAMES DepthSense) + else() + set(DSSDK_LIBRARIES_NAMES DepthSense DepthSensePlugins turbojpeg) + endif() + foreach(LIB ${DSSDK_LIBRARIES_NAMES}) + find_library(DSSDK_LIBRARY_${LIB} + NAMES "${LIB}" + PATHS "${DSSDK_DIR}/lib/" NO_DEFAULT_PATH) + list(APPEND DSSDK_LIBRARIES ${DSSDK_LIBRARY_${LIB}}) + mark_as_advanced(DSSDK_LIBRARY_${LIB}) + endforeach() + + # Version + set(DSSDK_VERSION 0) + file(STRINGS "${DSSDK_INCLUDE_DIRS}/DepthSenseVersion.hxx" _dsversion_H_CONTENTS REGEX "#define DEPTHSENSE_FILE_VERSION_STRING.*") + set(_DSSDK_VERSION_REGEX "([0-9]+\\.[0-9]+\\.[0-9]+\\.[0-9]+)") + if("${_dsversion_H_CONTENTS}" MATCHES ".*#define DEPTHSENSE_FILE_VERSION_STRING .*${_DSSDK_VERSION_REGEX}.*") + set(DSSDK_VERSION "${CMAKE_MATCH_1}") + endif() + unset(_dsversion_H_CONTENTS) + +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(DSSDK + FOUND_VAR DSSDK_FOUND + REQUIRED_VARS DSSDK_LIBRARIES DSSDK_INCLUDE_DIRS + VERSION_VAR DSSDK_VERSION +) diff --git a/deps_install/share/pcl-1.10/Modules/FindEigen.cmake b/deps_install/share/pcl-1.10/Modules/FindEigen.cmake new file mode 100755 index 0000000..e1fb6ab --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindEigen.cmake @@ -0,0 +1,41 @@ +############################################################################### +# Find Eigen3 +# +# This sets the following variables: +# EIGEN_FOUND - True if Eigen was found. +# EIGEN_INCLUDE_DIRS - Directories containing the Eigen include files. +# EIGEN_DEFINITIONS - Compiler flags for Eigen. +# EIGEN_VERSION - Package version + +find_package(PkgConfig QUIET) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen3" "$ENV{PROGRAMW6432}/Eigen3" + PATH_SUFFIXES eigen3 include/eigen3 include) + +if(EIGEN_INCLUDE_DIR) + file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen_world_version_match "${_eigen_version_header}") + set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen_major_version_match "${_eigen_version_header}") + set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_version_match "${_eigen_version_header}") + set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") + set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) +endif() + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS}, version: ${EIGEN_VERSION})") +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindEnsenso.cmake b/deps_install/share/pcl-1.10/Modules/FindEnsenso.cmake new file mode 100755 index 0000000..98b5601 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindEnsenso.cmake @@ -0,0 +1,48 @@ +############################################################################### +# Find Ensenso SDK (IDS-Imaging) +# +# find_package(Ensenso) +# +# Variables defined by this module: +# +# ENSENSO_FOUND True if Ensenso SDK was found +# ENSENSO_INCLUDE_DIRS The location(s) of Ensenso SDK headers +# ENSENSO_LIBRARIES Libraries needed to use Ensenso SDK + +find_path(ENSENSO_INCLUDE_DIR nxLib.h + HINTS "${ENSENSO_ABI_HINT}" + "/opt/ensenso/development/c" + "$ENV{PROGRAMFILES}/Ensenso/development/c" + "$ENV{PROGRAMW6432}/Ensenso/development/c" + PATH_SUFFIXES include/) + +find_library(ENSENSO_LIBRARY QUIET + NAMES NxLib64 NxLib32 nxLib64 nxLib32 + HINTS "${ENSENSO_ABI_HINT}" + "$ENV{PROGRAMFILES}/Ensenso/development/c" + "$ENV{PROGRAMW6432}/Ensenso/development/c" + PATH_SUFFIXES lib/) + +if(ENSENSO_INCLUDE_DIR AND ENSENSO_LIBRARY) + + # Include directories + set(ENSENSO_INCLUDE_DIRS ${ENSENSO_INCLUDE_DIR}) + unset(ENSENSO_INCLUDE_DIR) + mark_as_advanced(ENSENSO_INCLUDE_DIRS) + + # Libraries + set(ENSENSO_LIBRARIES ${ENSENSO_LIBRARY}) + unset(ENSENSO_LIBRARY) + mark_as_advanced(ENSENSO_LIBRARIES) + +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(ENSENSO + FOUND_VAR ENSENSO_FOUND + REQUIRED_VARS ENSENSO_LIBRARIES ENSENSO_INCLUDE_DIRS +) + +if(ENSENSO_FOUND) + message(STATUS "Ensenso found (include: ${ENSENSO_INCLUDE_DIRS}, lib: ${ENSENSO_LIBRARIES})") +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindFLANN.cmake b/deps_install/share/pcl-1.10/Modules/FindFLANN.cmake new file mode 100755 index 0000000..782ee01 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindFLANN.cmake @@ -0,0 +1,156 @@ +#.rst: +# FindFLANN +# -------- +# +# Try to find FLANN library and headers. This module supports both old released versions +# of FLANN ≤ 1.9.1 and newer development versions that ship with a modern config file. +# +# IMPORTED Targets +# ^^^^^^^^^^^^^^^^ +# +# This module defines the :prop_tgt:`IMPORTED` targets: +# +# ``FLANN::FLANN`` +# Defined if the system has FLANN. +# +# Result Variables +# ^^^^^^^^^^^^^^^^ +# +# This module sets the following variables: +# +# :: +# +# FLANN_FOUND True in case FLANN is found, otherwise false +# FLANN_ROOT Path to the root of found FLANN installation +# +# Example usage +# ^^^^^^^^^^^^^ +# +# :: +# +# find_package(FLANN REQUIRED) +# +# add_executable(foo foo.cc) +# target_link_libraries(foo FLANN::FLANN) +# + +# Early return if FLANN target is already defined. This makes it safe to run +# this script multiple times. +if(TARGET FLANN::FLANN) + return() +endif() + +# First try to locate FLANN using modern config +find_package(flann NO_MODULE ${FLANN_FIND_VERSION} QUIET) +if(flann_FOUND) + unset(flann_FOUND) + set(FLANN_FOUND ON) + # Create interface library that effectively becomes an alias for the appropriate (static/dynamic) imported FLANN target + add_library(FLANN::FLANN INTERFACE IMPORTED) + if(FLANN_USE_STATIC) + set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp_s) + else() + set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES flann::flann_cpp) + endif() + # Determine FLANN installation root based on the path to the processed Config file + get_filename_component(_config_dir "${flann_CONFIG}" DIRECTORY) + get_filename_component(FLANN_ROOT "${_config_dir}/../../.." ABSOLUTE) + unset(_config_dir) + return() +endif() + +# Second try to locate FLANN using pkgconfig +find_package(PkgConfig QUIET) +if(FLANN_FIND_VERSION) + pkg_check_modules(PC_FLANN flann>=${FLANN_FIND_VERSION}) +else() + pkg_check_modules(PC_FLANN flann) +endif() + +find_path(FLANN_INCLUDE_DIR + NAMES + flann/flann.hpp + HINTS + ${PC_FLANN_INCLUDE_DIRS} + ${FLANN_ROOT} + $ENV{FLANN_ROOT} + PATHS + $ENV{PROGRAMFILES}/Flann + $ENV{PROGRAMW6432}/Flann + PATH_SUFFIXES + include +) + +if(FLANN_USE_STATIC) + set(FLANN_RELEASE_NAME flann_cpp_s) + set(FLANN_DEBUG_NAME flann_cpp_s-gd) + set(FLANN_LIBRARY_TYPE STATIC) +else() + set(FLANN_RELEASE_NAME flann_cpp) + set(FLANN_DEBUG_NAME flann_cpp-gd) + set(FLANN_LIBRARY_TYPE SHARED) +endif() + +find_library(FLANN_LIBRARY + NAMES + ${FLANN_RELEASE_NAME} + HINTS + ${PC_FLANN_LIBRARY_DIRS} + ${FLANN_ROOT} + $ENV{FLANN_ROOT} + PATHS + $ENV{PROGRAMFILES}/Flann + $ENV{PROGRAMW6432}/Flann + PATH_SUFFIXES + lib +) + +find_library(FLANN_LIBRARY_DEBUG + NAMES + ${FLANN_DEBUG_NAME} + HINTS + ${PC_FLANN_LIBRARY_DIRS} + ${FLANN_ROOT} + $ENV{FLANN_ROOT} + PATHS + $ENV{PROGRAMFILES}/Flann + $ENV{PROGRAMW6432}/Flann + PATH_SUFFIXES + lib +) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args( + FLANN DEFAULT_MSG + FLANN_LIBRARY FLANN_INCLUDE_DIR +) + +if(FLANN_FOUND) + add_library(FLANN::FLANN ${FLANN_LIBRARY_TYPE} IMPORTED) + set_target_properties(FLANN::FLANN PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${FLANN_INCLUDE_DIR}") + set_target_properties(FLANN::FLANN PROPERTIES INTERFACE_COMPILE_DEFINITIONS "${PC_FLANN_CFLAGS_OTHER}") + set_property(TARGET FLANN::FLANN APPEND PROPERTY IMPORTED_CONFIGURATIONS "RELEASE") + set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX") + if(WIN32 AND NOT FLANN_USE_STATIC) + set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_IMPLIB_RELEASE "${FLANN_LIBRARY}") + else() + set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LOCATION_RELEASE "${FLANN_LIBRARY}") + endif() + if(FLANN_LIBRARY_DEBUG) + set_property(TARGET FLANN::FLANN APPEND PROPERTY IMPORTED_CONFIGURATIONS "DEBUG") + set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX") + if(WIN32 AND NOT FLANN_USE_STATIC) + set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_IMPLIB_DEBUG "${FLANN_LIBRARY_DEBUG}") + else() + set_target_properties(FLANN::FLANN PROPERTIES IMPORTED_LOCATION_DEBUG "${FLANN_LIBRARY_DEBUG}") + endif() + endif() + # Pkgconfig may specify additional link libraries besides from FLANN itself + # in PC_FLANN_LIBRARIES, add them to the target link interface. + foreach(_library ${PC_FLANN_LIBRARIES}) + if(NOT _library MATCHES "flann") + set_property(TARGET FLANN::FLANN APPEND PROPERTY INTERFACE_LINK_LIBRARIES "${_library}") + endif() + endforeach() + get_filename_component(FLANN_ROOT "${FLANN_INCLUDE_DIR}" PATH) +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindOpenNI.cmake b/deps_install/share/pcl-1.10/Modules/FindOpenNI.cmake new file mode 100755 index 0000000..09a6081 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindOpenNI.cmake @@ -0,0 +1,96 @@ +############################################################################### +# Find OpenNI +# +# find_package(OpenNI) +# +# Variables defined by this module: +# +# OPENNI_FOUND True if OpenNI was found +# OPENNI_INCLUDE_DIRS The location(s) of OpenNI headers +# OPENNI_LIBRARIES Libraries needed to use OpenNI +# OPENNI_DEFINITIONS Compiler flags for OpenNI + +find_package(PkgConfig QUIET) + +# Find LibUSB +if(NOT WIN32) + pkg_check_modules(PC_USB_10 libusb-1.0) + find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h + HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES libusb-1.0) + + find_library(USB_10_LIBRARY + NAMES usb-1.0 + HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES lib) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR) + + if(NOT USB_10_FOUND) + message(STATUS "OpenNI disabled because libusb-1.0 not found.") + return() + else() + include_directories(SYSTEM ${USB_10_INCLUDE_DIR}) + endif() +endif() + +pkg_check_modules(PC_OPENNI QUIET libopenni) + +set(OPENNI_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) + +set(OPENNI_SUFFIX) +if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + set(OPENNI_SUFFIX 64) +endif() + +# Add a hint so that it can find it without the pkg-config +find_path(OPENNI_INCLUDE_DIR XnStatus.h + HINTS ${PC_OPENNI_INCLUDEDIR} + ${PC_OPENNI_INCLUDE_DIRS} + /usr/include/openni + /usr/include/ni + /opt/local/include/ni + "${OPENNI_ROOT}" + "$ENV{OPENNI_ROOT}" + PATHS "$ENV{OPEN_NI_INSTALL_PATH${OPENNI_SUFFIX}}/Include" + PATH_SUFFIXES openni include Include) + +# Add a hint so that it can find it without the pkg-config +find_library(OPENNI_LIBRARY + NAMES OpenNI${OPENNI_SUFFIX} + HINTS ${PC_OPENNI_LIBDIR} + ${PC_OPENNI_LIBRARY_DIRS} + /usr/lib + "${OPENNI_ROOT}" + "$ENV{OPENNI_ROOT}" + PATHS "$ENV{OPEN_NI_LIB${OPENNI_SUFFIX}}" + PATH_SUFFIXES lib Lib Lib64) + +if(OPENNI_INCLUDE_DIR AND OPENNI_LIBRARY) + + # Include directories + set(OPENNI_INCLUDE_DIRS ${OPENNI_INCLUDE_DIR}) + unset(OPENNI_INCLUDE_DIR) + mark_as_advanced(OPENNI_INCLUDE_DIRS) + + # Libraries + if(CMAKE_SYSTEM_NAME STREQUAL "Darwin") + set(OPENNI_LIBRARIES ${OPENNI_LIBRARY} ${LIBUSB_1_LIBRARIES}) + else() + set(OPENNI_LIBRARIES ${OPENNI_LIBRARY}) + endif() + unset(OPENNI_LIBRARY) + mark_as_advanced(OPENNI_LIBRARIES) + +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OpenNI + FOUND_VAR OPENNI_FOUND + REQUIRED_VARS OPENNI_LIBRARIES OPENNI_INCLUDE_DIRS +) + +if(OPENNI_FOUND) + message(STATUS "OpenNI found (include: ${OPENNI_INCLUDE_DIRS}, lib: ${OPENNI_LIBRARIES})") +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindOpenNI2.cmake b/deps_install/share/pcl-1.10/Modules/FindOpenNI2.cmake new file mode 100755 index 0000000..1026331 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindOpenNI2.cmake @@ -0,0 +1,89 @@ +############################################################################### +# Find OpenNI2 +# +# find_package(OpenNI2) +# +# Variables defined by this module: +# +# OPENNI2_FOUND True if OpenNI2 was found +# OPENNI2_INCLUDE_DIRS The location(s) of OpenNI2 headers +# OPENNI2_LIBRARIES Libraries needed to use OpenNI2 +# OPENNI2_DEFINITIONS Compiler flags for OpenNI2 + +find_package(PkgConfig QUIET) + +# Find LibUSB +if(NOT WIN32) + pkg_check_modules(PC_USB_10 libusb-1.0) + find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h + HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES libusb-1.0) + + find_library(USB_10_LIBRARY + NAMES usb-1.0 + HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES lib) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR) + + if(NOT USB_10_FOUND) + message(STATUS "OpenNI 2 disabled because libusb-1.0 not found.") + return() + else() + include_directories(SYSTEM ${USB_10_INCLUDE_DIR}) + endif() +endif() + +pkg_check_modules(PC_OPENNI2 QUIET libopenni2) + +set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) + +set(OPENNI2_SUFFIX) +if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + set(OPENNI2_SUFFIX 64) +endif() + +find_path(OPENNI2_INCLUDE_DIR OpenNI.h + PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" # Win64 needs '64' suffix + "/usr/include/openni2" # common path for deb packages + PATH_SUFFIXES include/openni2 +) + +find_library(OPENNI2_LIBRARY + NAMES OpenNI2 # No suffix needed on Win64 + libOpenNI2 # Linux + PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" # Windows default path, Win64 needs '64' suffix + "$ENV{OPENNI2_REDIST}" # Linux install does not use a separate 'lib' directory +) + +if(OPENNI2_INCLUDE_DIR AND OPENNI2_LIBRARY) + + # Include directories + set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR}) + unset(OPENNI2_INCLUDE_DIR) + mark_as_advanced(OPENNI2_INCLUDE_DIRS) + + # Libraries + if(CMAKE_SYSTEM_NAME STREQUAL "Darwin") + set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} ${LIBUSB_1_LIBRARIES}) + else() + set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY}) + endif() + unset(OPENNI2_LIBRARY) + mark_as_advanced(OPENNI2_LIBRARIES) + + set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}}) + mark_as_advanced(OPENNI2_REDIST_DIR) + +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OpenNI2 + FOUND_VAR OPENNI2_FOUND + REQUIRED_VARS OPENNI2_LIBRARIES OPENNI2_INCLUDE_DIRS +) + +if(OPENNI2_FOUND) + message(STATUS "OpenNI2 found (include: ${OPENNI2_INCLUDE_DIRS}, lib: ${OPENNI2_LIBRARIES})") +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindPcap.cmake b/deps_install/share/pcl-1.10/Modules/FindPcap.cmake new file mode 100755 index 0000000..0b4ec3f --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindPcap.cmake @@ -0,0 +1,109 @@ +################################################################### +# +# Copyright (c) 2006 Frederic Heem, +# Copyright (c) 2012 Keven Ring +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# +# * Neither the name of the Telsey nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +################################################################### +# - Find pcap +# Find the PCAP includes and library +# http://www.tcpdump.org/ +# +# The environment variable PCAPDIR allows to specficy where to find +# libpcap in non standard location. +# +# 2012/01/02 - KEVEN RING +# Modified to find include/libs for WinPCAP +# +# PCAP_INCLUDE_DIRS - where to find pcap.h, etc. +# PCAP_LIBRARIES - List of libraries when using pcap. +# PCAP_FOUND - True if pcap found. + +if(WIN32) + set(_PCAP_LIBRARY_NAME wpcap) +else() + set(_PCAP_LIBRARY_NAME pcap) +endif() + +if(EXISTS $ENV{PCAPDIR}) +find_path(PCAP_INCLUDE_DIR +NAMES +pcap/pcap.h +pcap.h +PATHS +$ENV{PCAPDIR} +PATHS +$ENV{PCAPDIR}/include +NO_DEFAULT_PATH +) + +find_library(PCAP_LIBRARY +NAMES +${_PCAP_LIBRARY_NAME} +PATHS +$ENV{PCAPDIR} +PATHS +$ENV{PCAPDIR}/lib +NO_DEFAULT_PATH +) + +else() +find_path(PCAP_INCLUDE_DIR +NAMES +pcap/pcap.h +pcap.h +) + +find_library(PCAP_LIBRARY +NAMES +${_PCAP_LIBRARY_NAME} +) + +endif() + +set(PCAP_INCLUDE_DIRS ${PCAP_INCLUDE_DIR}) +set(PCAP_LIBRARIES ${PCAP_LIBRARY}) + +#Functions +include(CheckFunctionExists) +set(CMAKE_REQUIRED_INCLUDES ${PCAP_INCLUDE_DIRS}) +set(CMAKE_REQUIRED_LIBRARIES ${PCAP_LIBRARIES}) + +#Is pcap found ? +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(PCAP DEFAULT_MSG + PCAP_LIBRARIES PCAP_INCLUDE_DIRS) + +mark_as_advanced( +PCAP_LIBRARIES +PCAP_INCLUDE_DIRS +) diff --git a/deps_install/share/pcl-1.10/Modules/FindQhull.cmake b/deps_install/share/pcl-1.10/Modules/FindQhull.cmake new file mode 100755 index 0000000..7f3cf90 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindQhull.cmake @@ -0,0 +1,94 @@ +############################################################################### +# Find QHULL +# +# This sets the following variables: +# QHULL_FOUND - True if QHULL was found. +# QHULL_INCLUDE_DIRS - Directories containing the QHULL include files. +# QHULL_LIBRARIES - Libraries needed to use QHULL. +# QHULL_DEFINITIONS - Compiler flags for QHULL. +# If QHULL_USE_STATIC is specified then look for static libraries ONLY else +# look for shared ones + +set(QHULL_MAJOR_VERSION 6) + +if(QHULL_USE_STATIC) + set(QHULL_RELEASE_NAME qhullstatic) + set(QHULL_DEBUG_NAME qhullstatic_d) +else() + set(QHULL_RELEASE_NAME qhull_p qhull${QHULL_MAJOR_VERSION} qhull) + set(QHULL_DEBUG_NAME qhull_p_d qhull${QHULL_MAJOR_VERSION}_d qhull_d${QHULL_MAJOR_VERSION} qhull_d) +endif() + +find_file(QHULL_HEADER + NAMES libqhull/libqhull.h qhull.h + HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" "${QHULL_INCLUDE_DIR}" + PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" + PATH_SUFFIXES qhull src/libqhull libqhull include) + +set(QHULL_HEADER "${QHULL_HEADER}" CACHE INTERNAL "QHull header" FORCE ) + +if(QHULL_HEADER) + get_filename_component(qhull_header ${QHULL_HEADER} NAME_WE) + if("${qhull_header}" STREQUAL "qhull") + set(HAVE_QHULL_2011 OFF) + get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) + elseif("${qhull_header}" STREQUAL "libqhull") + set(HAVE_QHULL_2011 ON) + get_filename_component(QHULL_INCLUDE_DIR ${QHULL_HEADER} PATH) + get_filename_component(QHULL_INCLUDE_DIR ${QHULL_INCLUDE_DIR} PATH) + endif() +else() + set(QHULL_INCLUDE_DIR "QHULL_INCLUDE_DIR-NOTFOUND") +endif() + +find_library(QHULL_LIBRARY + NAMES ${QHULL_RELEASE_NAME} + HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" + PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" + PATH_SUFFIXES project build bin lib) + +get_filename_component(QHULL_LIBRARY_NAME "${QHULL_LIBRARY}" NAME) + +find_library(QHULL_LIBRARY_DEBUG + NAMES ${QHULL_DEBUG_NAME} ${QHULL_RELEASE_NAME} + HINTS "${QHULL_ROOT}" "$ENV{QHULL_ROOT}" + PATHS "$ENV{PROGRAMFILES}/QHull" "$ENV{PROGRAMW6432}/QHull" + PATH_SUFFIXES project build bin lib debug/lib) + +if(NOT QHULL_LIBRARY_DEBUG) + set(QHULL_LIBRARY_DEBUG ${QHULL_LIBRARY}) +endif() + +get_filename_component(QHULL_LIBRARY_DEBUG_NAME "${QHULL_LIBRARY_DEBUG}" NAME) + +if(QHULL_INCLUDE_DIR AND QHULL_LIBRARY) + + # Include directories + set(QHULL_INCLUDE_DIRS ${QHULL_INCLUDE_DIR}) + unset(QHULL_INCLUDE_DIR) + mark_as_advanced(QHULL_INCLUDE_DIRS) + + # Libraries + set(QHULL_LIBRARIES optimized ${QHULL_LIBRARY} debug ${QHULL_LIBRARY_DEBUG}) + unset(QHULL_LIBRARY) + unset(QHULL_LIBRARY_DEBUG) + mark_as_advanced(QHULL_LIBRARIES) + +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Qhull + FOUND_VAR QHULL_FOUND + REQUIRED_VARS QHULL_LIBRARIES QHULL_INCLUDE_DIRS +) + +if(QHULL_FOUND) + set(HAVE_QHULL ON) + if(NOT QHULL_USE_STATIC) + add_definitions("-Dqh_QHpointer") + if(MSVC) + add_definitions("-Dqh_QHpointer_dllimport") + endif() + endif() + message(STATUS "QHULL found (include: ${QHULL_INCLUDE_DIRS}, lib: ${QHULL_LIBRARIES})") +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindRSSDK.cmake b/deps_install/share/pcl-1.10/Modules/FindRSSDK.cmake new file mode 100755 index 0000000..de0cf2c --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindRSSDK.cmake @@ -0,0 +1,71 @@ +############################################################################### +# Find Intel RealSense SDK +# +# find_package(RSSDK) +# +# Variables defined by this module: +# +# RSSDK_FOUND True if RealSense SDK was found +# RSSDK_VERSION The version of RealSense SDK +# RSSDK_INCLUDE_DIRS The location(s) of RealSense SDK headers +# RSSDK_LIBRARIES Libraries needed to use RealSense SDK + +find_path(RSSDK_DIR include/pxcversion.h + PATHS "$ENV{RSSDK_DIR}" + "$ENV{PROGRAMFILES}/Intel/RSSDK" + "$ENV{PROGRAMW6432}/Intel/RSSDK" + "C:/Program Files (x86)/Intel/RSSDK" + "C:/Program Files/Intel/RSSDK" + DOC "RealSense SDK directory") + +if(RSSDK_DIR) + + # Include directories + set(RSSDK_INCLUDE_DIRS ${RSSDK_DIR}/include) + mark_as_advanced(RSSDK_INCLUDE_DIRS) + + # Libraries + set(RSSDK_RELEASE_NAME libpxc.lib) + set(RSSDK_DEBUG_NAME libpxc_d.lib) + set(RSSDK_SUFFIX Win32) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(RSSDK_SUFFIX x64) + endif() + find_library(RSSDK_LIBRARY + NAMES ${RSSDK_RELEASE_NAME} + PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH + PATH_SUFFIXES ${RSSDK_SUFFIX}) + find_library(RSSDK_LIBRARY_DEBUG + NAMES ${RSSDK_DEBUG_NAME} ${RSSDK_RELEASE_NAME} + PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH + PATH_SUFFIXES ${RSSDK_SUFFIX}) + if(NOT RSSDK_LIBRARY_DEBUG) + set(RSSDK_LIBRARY_DEBUG ${RSSDK_LIBRARY}) + endif() + set(RSSDK_LIBRARIES optimized ${RSSDK_LIBRARY} debug ${RSSDK_LIBRARY_DEBUG}) + mark_as_advanced(RSSDK_LIBRARY RSSDK_LIBRARY_DEBUG) + + # Version + set(RSSDK_VERSION 0) + file(STRINGS "${RSSDK_INCLUDE_DIRS}/pxcversion.h" _pxcversion_H_CONTENTS REGEX "#define PXC_VERSION_.*") + set(_RSSDK_VERSION_REGEX "([0-9]+)") + foreach(v MAJOR MINOR BUILD REVISION) + if("${_pxcversion_H_CONTENTS}" MATCHES ".*#define PXC_VERSION_${v} *${_RSSDK_VERSION_REGEX}.*") + set(RSSDK_VERSION_${v} "${CMAKE_MATCH_1}") + endif() + endforeach() + unset(_pxcversion_H_CONTENTS) + set(RSSDK_VERSION "${RSSDK_VERSION_MAJOR}.${RSSDK_VERSION_MINOR}.${RSSDK_VERSION_BUILD}.${RSSDK_VERSION_REVISION}") + +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(RSSDK + FOUND_VAR RSSDK_FOUND + REQUIRED_VARS RSSDK_LIBRARIES RSSDK_INCLUDE_DIRS + VERSION_VAR RSSDK_VERSION +) + +if(MSVC) + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /NODEFAULTLIB:LIBCMTD") +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindRSSDK2.cmake b/deps_install/share/pcl-1.10/Modules/FindRSSDK2.cmake new file mode 100755 index 0000000..54254bd --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindRSSDK2.cmake @@ -0,0 +1,22 @@ +############################################################################### +# Find Intel RealSense SDK 2.0 (librealsense) +# +# find_package(RSSDK2) +# +# Variables defined by this module: +# +# RSSDK2_FOUND True if RealSense SDK 2.0 was found +# RSSDK2_INCLUDE_DIRS The location(s) of RealSense SDK 2.0 headers +# RSSDK2_LIBRARIES Libraries needed to use RealSense SDK 2.0 + +find_package(realsense2 QUIET) + +set(RSSDK2_FOUND ${realsense2_FOUND}) +set(RSSDK2_INCLUDE_DIRS ${realsense2_INCLUDE_DIR}) +set(RSSDK2_LIBRARIES ${realsense2_LIBRARY}) + +if(RSSDK2_FOUND) + message(STATUS "RealSense SDK 2 found (include: ${RSSDK2_INCLUDE_DIRS}, lib: ${RSSDK2_LIBRARIES}, version: ${realsense2_VERSION})") +else () + message(STATUS "Could NOT find RSSDK2") +endif() diff --git a/deps_install/share/pcl-1.10/Modules/FindSphinx.cmake b/deps_install/share/pcl-1.10/Modules/FindSphinx.cmake new file mode 100755 index 0000000..61f5521 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FindSphinx.cmake @@ -0,0 +1,26 @@ +############################################################################### +# Find Sphinx +# +# This sets the following variables: +# SPHINX_FOUND - True if Sphinx was found. +# SPHINX_EXECUTABLE - Sphinx-build executable + +find_package(PkgConfig QUIET) +pkg_check_modules(PC_SPHINX sphinx-build) + +find_package(PythonInterp) + +if(PYTHONINTERP_FOUND) + get_filename_component(PYTHON_DIR "${PYTHON_EXECUTABLE}" PATH) +endif() + +find_program(SPHINX_EXECUTABLE NAMES sphinx-build + HINTS ${PC_SPHINX_EXECUTABLE} $ENV{SPHINX_DIR} ${PYTHON_DIR}/Scripts + PATH_SUFFIXES bin + DOC "Sphinx documentation generator" +) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Sphinx DEFAULT_MSG SPHINX_EXECUTABLE) + +mark_as_advanced(SPHINX_EXECUTABLE) diff --git a/deps_install/share/pcl-1.10/Modules/FinddavidSDK.cmake b/deps_install/share/pcl-1.10/Modules/FinddavidSDK.cmake new file mode 100755 index 0000000..8840646 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/FinddavidSDK.cmake @@ -0,0 +1,35 @@ +############################################################################### +# - Try to find davidSDK (David Vision Systems) +# Once done this will define +# DAVIDSDK_FOUND - System has davidSDK +# DAVIDSDK_INCLUDE_DIRS - The davidSDK include directories +# DAVIDSDK_LIBRARIES - The libraries needed to use davidSDK +# DAVIDSDK_DEFINITIONS - Compiler switches required for using davidSDK +# ----------------------- + +find_path(DAVIDSDK_INCLUDE_DIR david.h + HINTS ${DAVIDSDK_ABI_HINT} + /usr/local/include/davidSDK + "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK" + PATH_SUFFIXES include/) + +find_library(DAVIDSDK_LIBRARY QUIET NAMES davidSDK + HINTS ${DAVIDSDK_ABI_HINT} + "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK" + PATH_SUFFIXES lib/) + +set(DAVIDSDK_LIBRARIES ${DAVIDSDK_LIBRARY}) +set(DAVIDSDK_INCLUDE_DIRS ${DAVIDSDK_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +# handle the QUIETLY and REQUIRED arguments and set DAVIDSDK_FOUND to TRUE +# if all listed variables are TRUE +find_package_handle_standard_args(davidSDK DEFAULT_MSG + DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR) + +mark_as_advanced(DAVIDSDK_INCLUDE_DIR DAVIDSDK_LIBRARY) + +if(DAVIDSDK_FOUND) + message(STATUS "davidSDK found") +endif() + diff --git a/deps_install/share/pcl-1.10/Modules/Findlibusb-1.0.cmake b/deps_install/share/pcl-1.10/Modules/Findlibusb-1.0.cmake new file mode 100755 index 0000000..6d7ac0c --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/Findlibusb-1.0.cmake @@ -0,0 +1,70 @@ +# - Try to find libusb-1.0 +# Once done this will define +# +# LIBUSB_1_FOUND - system has libusb +# LIBUSB_1_INCLUDE_DIRS - the libusb include directory +# LIBUSB_1_LIBRARIES - Link these to use libusb +# LIBUSB_1_DEFINITIONS - Compiler switches required for using libusb +# +# Adapted from cmake-modules Google Code project +# +# Copyright (c) 2006 Andreas Schneider +# +# (Changes for libusb) Copyright (c) 2008 Kyle Machulis +# +# Point Cloud Library (PCL) - www.pointclouds.org +# Copyright (c) 2012, Willow Garage, Inc. (use of FindPackageHandleStandardArgs) +# +# Redistribution and use is allowed according to the terms of the New BSD license. +# +# CMake-Modules Project New BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the CMake-Modules Project nor the names of its +# contributors may be used to endorse or promote products derived from this +# software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# + +if(LIBUSB_1_LIBRARIES AND LIBUSB_1_INCLUDE_DIRS) + # in cache already + set(LIBUSB_FOUND TRUE) + set(LIBUSB_1_FOUND TRUE) +else() + find_path(LIBUSB_1_INCLUDE_DIR + NAMES libusb-1.0/libusb.h + PATHS /usr/include /usr/local/include /opt/local/include /sw/include + PATH_SUFFIXES libusb-1.0) + + find_library(LIBUSB_1_LIBRARY + NAMES usb-1.0 + PATHS /usr/lib /usr/local/lib /opt/local/lib /sw/lib) + + set(LIBUSB_1_INCLUDE_DIRS ${LIBUSB_1_INCLUDE_DIR}) + set(LIBUSB_1_LIBRARIES ${LIBUSB_1_LIBRARY}) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(LIBUSB_1 DEFAULT_MSG LIBUSB_1_LIBRARY LIBUSB_1_INCLUDE_DIR) + + # show the LIBUSB_1_INCLUDE_DIRS and LIBUSB_1_LIBRARIES variables only in the advanced view + mark_as_advanced(LIBUSB_1_INCLUDE_DIRS LIBUSB_1_LIBRARIES) +endif() diff --git a/deps_install/share/pcl-1.10/Modules/UseCompilerCache.cmake b/deps_install/share/pcl-1.10/Modules/UseCompilerCache.cmake new file mode 100755 index 0000000..0c21d29 --- /dev/null +++ b/deps_install/share/pcl-1.10/Modules/UseCompilerCache.cmake @@ -0,0 +1,134 @@ +# .rst: +# UseCompilerCache +# -------- +# +# This module provides a function to setup a compiler cache tool (default: ``ccache``) +# Main function of interest is ``UseCompilerCache`` +# +# Needs CMake 3.4 at least +# Inspired from: +# * https://crascit.com/2016/04/09/using-ccache-with-cmake/ +# * https://stackoverflow.com/a/36515503/ +# * https://gitlab.kitware.com/henryiii/cmake/blob/cache/Modules/UseCompilerCache.cmake + +# .rst +# pcl_ccache_compat_file_gen +# -- Generates a wrapper file which launches the compiler commands using ccache. +# This allows support for XCode and CCache < 3.3 +function(pcl_ccache_compat_file_gen FILE_NAME CCACHE_PROGRAM COMPILER) + message(STATUS "${FILE_NAME} for ${CCACHE_PROGRAM} with ${COMPILER}") + file(WRITE "${CMAKE_BINARY_DIR}/${FILE_NAME}" "" + "#! /usr/bin/env sh\n" + "\n" + "# Xcode generator doesn't include the compiler as the\n" + "# first argument, Ninja and Makefiles do. Handle both cases.\n" + "if [ \"$1\" = \"${COMPILER}\" ] ; then\n" + " shift\n" + "fi\n" + "\n" + "export CCACHE_CPP2=true\n" + "exec \"${CCACHE_PROGRAM}\" \"${COMPILER}\" \"$@\"\n") +endfunction() + +# .rst +# UseCompilerCache([PROGRAM ] [QUIET] [REQUIRED]) +# -- Add the compiler cache tool (default to look for ccache on the path) +# to your build through CMAKE__COMPILER_LAUNCHER variables. Also +# supports XCode. Uses a wrapper for XCode and CCache < 3.3. +# Sets the COMPILER_CACHE_VERSION variable. +function(UseCompilerCache) + set(options QUIET REQUIRED) + set(oneValueArgs CCACHE) + set(multiValueArgs) + + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(NOT ARGS_CCACHE) + set(ARGS_CCACHE ccache) + endif() + + find_program(CCACHE_PROGRAM ${ARGS_CCACHE}) + + # Quit if not found + if(NOT CCACHE_PROGRAM) + if(REQUIRED) + message(FATAL_ERROR "Failed to find ${CCACHE_PROGRAM} (REQUIRED)") + endif() + return() + endif() + + if(CMAKE_GENERATOR MATCHES "Visual") + message(FATAL_ERROR "MSVC isn't compatible with current solutions. Please rename compiler cache to cl.exe and prepend its location in env PATH variable") + return() + endif() + + # Get version number + execute_process(COMMAND "${CCACHE_PROGRAM}" --version OUTPUT_VARIABLE output) + string(REPLACE "\n" ";" output "${output}") + foreach(line ${output}) + string(TOLOWER ${line} line) + string(REGEX REPLACE "^ccache version ([\\.0-9]+)$" "\\1" version "${line}") + if(version) + set(COMPILER_CACHE_VERSION ${version} PARENT_SCOPE) + break() + endif() + endforeach() + + if(NOT QUIET) + message(STATUS "Using Compiler Cache (${CCACHE_PROGRAM}) v${version} in the C/C++ toolchain") + endif() + + set(xcode_compat FALSE) + if(CMAKE_GENERATOR STREQUAL Xcode) + set(xcode_compat TRUE) + endif() + set(ccache_compat FALSE) + if((ARGS_CCACHE STREQUAL ccache) AND (version VERSION_LESS 3.3.0)) + set(ccache_compat TRUE) + endif() + + # Indirect wrapper is needed for CCache < 3.3 or XCode + if(NOT (${xcode_compat} OR ${ccache_compat})) + # Support Unix Makefiles and Ninja + message(STATUS "Compiler cache via cmake launcher prefix") + set(CMAKE_C_COMPILER_LAUNCHER "${CCACHE_PROGRAM}" PARENT_SCOPE) + set(CMAKE_CXX_COMPILER_LAUNCHER "${CCACHE_PROGRAM}" PARENT_SCOPE) + set(CMAKE_CUDA_COMPILER_LAUNCHER "${CCACHE_PROGRAM}" PARENT_SCOPE) + return() + endif() + + message(STATUS "Generating launch helpers for compiler cache") + + pcl_ccache_compat_file_gen("launch-c" ${CCACHE_PROGRAM} ${CMAKE_C_COMPILER}) + pcl_ccache_compat_file_gen("launch-cxx" ${CCACHE_PROGRAM} ${CMAKE_CXX_COMPILER}) + execute_process(COMMAND chmod a+rx + "${CMAKE_BINARY_DIR}/launch-c" + "${CMAKE_BINARY_DIR}/launch-cxx") + + # Cuda support only added in CMake 3.10 + set(cuda_supported FALSE) + if (NOT (CMAKE_VERSION VERSION_LESS 3.10) AND CMAKE_CUDA_COMPILER) + set(cuda_supported TRUE) + endif() + if(${cuda_supported}) + pcl_ccache_compat_file_gen("launch-cuda" ${CCACHE_PROGRAM} ${CMAKE_CUDA_COMPILER}) + execute_process(COMMAND chmod a+rx + "${CMAKE_BINARY_DIR}/launch-cuda") + endif() + + if(${xcode_compat}) + # Set Xcode project attributes to route compilation and linking properly + message(STATUS "Compiler cache via launch files to support XCode") + set(CMAKE_XCODE_ATTRIBUTE_CC "${CMAKE_BINARY_DIR}/launch-c" PARENT_SCOPE) + set(CMAKE_XCODE_ATTRIBUTE_CXX "${CMAKE_BINARY_DIR}/launch-cxx" PARENT_SCOPE) + set(CMAKE_XCODE_ATTRIBUTE_LD "${CMAKE_BINARY_DIR}/launch-c" PARENT_SCOPE) + set(CMAKE_XCODE_ATTRIBUTE_LDPLUSPLUS "${CMAKE_BINARY_DIR}/launch-cxx" PARENT_SCOPE) + else() + message(STATUS "Compiler cache via launch files to support Unix Makefiles and Ninja") + set(CMAKE_C_COMPILER_LAUNCHER "${CMAKE_BINARY_DIR}/launch-c" PARENT_SCOPE) + set(CMAKE_CXX_COMPILER_LAUNCHER "${CMAKE_BINARY_DIR}/launch-cxx" PARENT_SCOPE) + if (${cuda_supported}) + set(CMAKE_CUDA_COMPILER_LAUNCHER "${CMAKE_BINARY_DIR}/launch-cuda" PARENT_SCOPE) + endif() + endif() +endfunction() diff --git a/deps_install/share/pcl-1.10/PCLConfig.cmake b/deps_install/share/pcl-1.10/PCLConfig.cmake new file mode 100755 index 0000000..6fd1dd8 --- /dev/null +++ b/deps_install/share/pcl-1.10/PCLConfig.cmake @@ -0,0 +1,725 @@ +# ------------------------------------------------------------------------------------ +# Helper to use PCL from outside project +# +# target_link_libraries(my_fabulous_target PCL_XXX_LIBRARIES) where XXX is the +# upper cased xxx from : +# +# - common +# - kdtree +# - octree +# - search +# - sample_consensus +# - filters +# - 2d +# - geometry +# - io +# - features +# - ml +# - segmentation +# - surface +# - registration +# - keypoints +# - tracking +# - recognition +# - stereo +# +# PCL_INCLUDE_DIRS is filled with PCL and available 3rdparty headers +# PCL_LIBRARY_DIRS is filled with PCL components libraries install directory and +# 3rdparty libraries paths +# +# www.pointclouds.org +#------------------------------------------------------------------------------------ + +# Set default policy behavior similar to minimum requirement version +cmake_policy(VERSION 3.5) + +# explicitly set policies we already support in newer cmake versions +if(POLICY CMP0074) + # TODO: update *_ROOT variables to be PCL_*_ROOT or equivalent. + # CMP0074 directly affects how Find* modules work and *_ROOT variables. Since + # this is a config file that will be consumed by parent projects with (likely) + # NEW behavior, we need to push a policy stack. + cmake_policy(SET CMP0074 NEW) +endif() + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/Modules") + +### ---[ some useful macros +macro(pcl_report_not_found _reason) + unset(PCL_FOUND) + unset(PCL_LIBRARIES) + unset(PCL_COMPONENTS) + unset(PCL_INCLUDE_DIRS) + unset(PCL_LIBRARY_DIRS) + unset(PCL_DEFINITIONS) + if(PCL_FIND_REQUIRED) + message(FATAL_ERROR ${_reason}) + elseif(NOT PCL_FIND_QUIETLY) + message(WARNING ${_reason}) + endif() + return() +endmacro() + +macro(pcl_message) + if(NOT PCL_FIND_QUIETLY) + message(${ARGN}) + endif() +endmacro() + +# Remove duplicate libraries +macro(pcl_remove_duplicate_libraries _unfiltered_libraries _filtered_libraries) + set(${_filtered_libraries}) + set(_debug_libraries) + set(_optimized_libraries) + set(_other_libraries) + set(_waiting_for_debug 0) + set(_waiting_for_optimized 0) + set(_library_position -1) + foreach(library ${${_unfiltered_libraries}}) + if("${library}" STREQUAL "debug") + set(_waiting_for_debug 1) + elseif("${library}" STREQUAL "optimized") + set(_waiting_for_optimized 1) + elseif(_waiting_for_debug) + list(FIND _debug_libraries "${library}" library_position) + if(library_position EQUAL -1) + list(APPEND ${_filtered_libraries} debug ${library}) + list(APPEND _debug_libraries ${library}) + endif() + set(_waiting_for_debug 0) + elseif(_waiting_for_optimized) + list(FIND _optimized_libraries "${library}" library_position) + if(library_position EQUAL -1) + list(APPEND ${_filtered_libraries} optimized ${library}) + list(APPEND _optimized_libraries ${library}) + endif() + set(_waiting_for_optimized 0) + else() + list(FIND _other_libraries "${library}" library_position) + if(library_position EQUAL -1) + list(APPEND ${_filtered_libraries} ${library}) + list(APPEND _other_libraries ${library}) + endif() + endif() + endforeach() +endmacro() + +### ---[ 3rd party libraries +macro(find_boost) + if(PCL_ALL_IN_ONE_INSTALLER) + set(BOOST_ROOT "${PCL_ROOT}/3rdParty/Boost") + elseif(NOT BOOST_INCLUDEDIR) + set(BOOST_INCLUDEDIR "/usr/include") + endif() + # use static Boost in Windows + if(WIN32) + set(Boost_USE_STATIC_LIBS ) + set(Boost_USE_STATIC ) + set(Boost_USE_MULTITHREAD ) + endif() + set(Boost_ADDITIONAL_VERSIONS + "1.74.0" "1.74" + "1.71.0" "1.71" "1.70.0" "1.70" + "1.69.0" "1.69" "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65" + "1.64.0" "1.64" "1.63.0" "1.63" "1.62.0" "1.62" "1.61.0" "1.61" "1.60.0" "1.60" + "1.59.0" "1.59" "1.58.0" "1.58" "1.57.0" "1.57" "1.56.0" "1.56" "1.55.0" "1.55") + # Disable the config mode of find_package(Boost) + set(Boost_NO_BOOST_CMAKE ON) + find_package(Boost 1.55.0 ${QUIET_} COMPONENTS system filesystem date_time iostreams serialization) + + set(BOOST_FOUND ${Boost_FOUND}) + set(BOOST_INCLUDE_DIRS "${Boost_INCLUDE_DIR}") + set(BOOST_LIBRARY_DIRS "${Boost_LIBRARY_DIRS}") + set(BOOST_LIBRARIES ${Boost_LIBRARIES}) + if(WIN32 AND NOT MINGW) + set(BOOST_DEFINITIONS ${BOOST_DEFINITIONS} -DBOOST_ALL_NO_LIB) + endif() +endmacro() + +#remove this as soon as eigen is shipped with FindEigen.cmake +macro(find_eigen) + if(PCL_ALL_IN_ONE_INSTALLER) + set(EIGEN_ROOT "${PCL_ROOT}/3rdParty/Eigen") + elseif(NOT EIGEN_ROOT) + get_filename_component(EIGEN_ROOT "/usr/include/eigen3" ABSOLUTE) + endif() + find_package(Eigen 3.1) + set(EIGEN_DEFINITIONS ${EIGEN_DEFINITIONS}) +endmacro() + +#remove this as soon as qhull is shipped with FindQhull.cmake +macro(find_qhull) + if(PCL_ALL_IN_ONE_INSTALLER) + set(QHULL_ROOT "${PCL_ROOT}/3rdParty/Qhull") + elseif(NOT QHULL_ROOT) + get_filename_component(QHULL_ROOT "/usr/include" PATH) + endif() + + set(QHULL_USE_STATIC ) + find_package(Qhull) +endmacro() + +#remove this as soon as libopenni is shipped with FindOpenni.cmake +macro(find_openni) + if(PCL_FIND_QUIETLY) + set(OpenNI_FIND_QUIETLY TRUE) + endif() + + if(NOT OPENNI_ROOT AND ("TRUE" STREQUAL "TRUE")) + set(OPENNI_INCLUDE_DIRS_HINT "/usr/include/ni") + get_filename_component(OPENNI_LIBRARY_HINT "/usr/lib/libOpenNI.so" PATH) + endif() + + find_package(OpenNI) +endmacro() + +#remove this as soon as libopenni2 is shipped with FindOpenni2.cmake +macro(find_openni2) + if(PCL_FIND_QUIETLY) + set(OpenNI2_FIND_QUIETLY TRUE) + endif() + + if(NOT OPENNI2_ROOT AND ("TRUE" STREQUAL "TRUE")) + set(OPENNI2_INCLUDE_DIRS_HINT "/usr/include/openni2") + get_filename_component(OPENNI2_LIBRARY_HINT "/usr/lib/x86_64-linux-gnu/libOpenNI2.so" PATH) + endif() + + find_package(OpenNI2) +endmacro() + +#remove this as soon as the Ensenso SDK is shipped with FindEnsenso.cmake +macro(find_ensenso) + if(PCL_FIND_QUIETLY) + set(ensenso_FIND_QUIETLY TRUE) + endif() + + if(NOT ENSENSO_ROOT AND ("" STREQUAL "TRUE")) + get_filename_component(ENSENSO_ABI_HINT "ENSENSO_INCLUDE_DIR-NOTFOUND" PATH) + endif() + + find_package(Ensenso) +endmacro() + +#remove this as soon as the davidSDK is shipped with FinddavidSDK.cmake +macro(find_davidSDK) + if(PCL_FIND_QUIETLY) + set(DAVIDSDK_FIND_QUIETLY TRUE) + endif() + + if(NOT davidSDK_ROOT AND ("" STREQUAL "TRUE")) + get_filename_component(DAVIDSDK_ABI_HINT DAVIDSDK_INCLUDE_DIR-NOTFOUND PATH) + endif() + + find_package(davidSDK) +endmacro() + +macro(find_dssdk) + if(PCL_FIND_QUIETLY) + set(DSSDK_FIND_QUIETLY TRUE) + endif() + if(NOT DSSDK_DIR AND ("" STREQUAL "TRUE")) + get_filename_component(DSSDK_DIR_HINT "" PATH) + endif() + + find_package(DSSDK) +endmacro() + +macro(find_rssdk) + if(PCL_FIND_QUIETLY) + set(RSSDK_FIND_QUIETLY TRUE) + endif() + if(NOT RSSDK_DIR AND ("" STREQUAL "TRUE")) + get_filename_component(RSSDK_DIR_HINT "" PATH) + endif() + + find_package(RSSDK) +endmacro() + +macro(find_rssdk2) + if(PCL_ALL_IN_ONE_INSTALLER) + set(realsense2_DIR "${PCL_ROOT}/3rdParty/librealsense2/lib/cmake/realsense2" CACHE PATH "The directory containing realsense2Config.cmake") + elseif(NOT realsense2_DIR) + get_filename_component(realsense2_DIR "" PATH) + set(realsense2_DIR "${realsense2_DIR}/lib/cmake/realsense2" CACHE PATH "The directory containing realsense2Config.cmake") + endif() + find_package(RSSDK2) +endmacro() + +#remove this as soon as flann is shipped with FindFlann.cmake +macro(find_flann) + if(PCL_ALL_IN_ONE_INSTALLER) + set(FLANN_ROOT "${PCL_ROOT}/3rdParty/Flann") + elseif(NOT FLANN_ROOT) + set(FLANN_ROOT "/usr") + endif() + + set(FLANN_USE_STATIC ) + find_package(FLANN) +endmacro() + +macro(find_VTK) + if(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID) + if(EXISTS "${PCL_ROOT}/3rdParty/VTK/lib/cmake") + set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/cmake/vtk-9.1" CACHE PATH "The directory containing VTKConfig.cmake") + else() + set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/vtk-9.1" CACHE PATH "The directory containing VTKConfig.cmake") + endif() + elseif(NOT VTK_DIR AND NOT ANDROID) + set(VTK_DIR "/usr/lib/x86_64-linux-gnu/cmake/vtk-9.1" CACHE PATH "The directory containing VTKConfig.cmake") + endif() + if(NOT ANDROID) + find_package(VTK ${QUIET_} COMPONENTS ${PCL_VTK_COMPONENTS}) + endif() +endmacro() + +macro(find_libusb) + if(NOT WIN32) + find_path(LIBUSB_1_INCLUDE_DIR + NAMES libusb-1.0/libusb.h + PATHS /usr/include /usr/local/include /opt/local/include /sw/include + PATH_SUFFIXES libusb-1.0) + + find_library(LIBUSB_1_LIBRARY + NAMES usb-1.0 + PATHS /usr/lib /usr/local/lib /opt/local/lib /sw/lib) + find_package_handle_standard_args(libusb-1.0 LIBUSB_1_LIBRARY LIBUSB_1_INCLUDE_DIR) + endif() +endmacro() + +macro(find_glew) + find_package(GLEW) +endmacro() + +# Finds each component external libraries if any +# The functioning is as following +# try to find _lib +# |--> _lib found ==> include the headers, +# | link to its library directories or include _lib_USE_FILE +# `--> _lib not found +# |--> _lib is optional ==> disable it (thanks to the guardians) +# | and warn +# `--> _lib is required +# |--> component is required explicitly ==> error +# `--> component is induced ==> warn and remove it +# from the list + +macro(find_external_library _component _lib _is_optional) + if("${_lib}" STREQUAL "boost") + find_boost() + elseif("${_lib}" STREQUAL "eigen") + find_eigen() + elseif("${_lib}" STREQUAL "flann") + find_flann() + elseif("${_lib}" STREQUAL "qhull") + find_qhull() + elseif("${_lib}" STREQUAL "openni") + find_openni() + elseif("${_lib}" STREQUAL "openni2") + find_openni2() + elseif("${_lib}" STREQUAL "ensenso") + find_ensenso() + elseif("${_lib}" STREQUAL "davidSDK") + find_davidSDK() + elseif("${_lib}" STREQUAL "dssdk") + find_dssdk() + elseif("${_lib}" STREQUAL "rssdk") + find_rssdk() + elseif("${_lib}" STREQUAL "rssdk2") + find_rssdk2() + elseif("${_lib}" STREQUAL "vtk") + find_VTK() + elseif("${_lib}" STREQUAL "libusb-1.0") + find_libusb() + elseif("${_lib}" STREQUAL "glew") + find_glew() + elseif("${_lib}" STREQUAL "opengl") + find_package(OpenGL) + endif() + + string(TOUPPER "${_component}" COMPONENT) + string(TOUPPER "${_lib}" LIB) + string(REGEX REPLACE "[.-]" "_" LIB ${LIB}) + if(${LIB}_FOUND) + list(APPEND PCL_${COMPONENT}_INCLUDE_DIRS ${${LIB}_INCLUDE_DIRS}) + if(${LIB}_USE_FILE) + include(${${LIB}_USE_FILE}) + else() + list(APPEND PCL_${COMPONENT}_LIBRARY_DIRS "${${LIB}_LIBRARY_DIRS}") + endif() + if(${LIB}_LIBRARIES) + list(APPEND PCL_${COMPONENT}_LIBRARIES "${${LIB}_LIBRARIES}") + endif() + if(${LIB}_DEFINITIONS AND NOT ${LIB} STREQUAL "VTK") + list(APPEND PCL_${COMPONENT}_DEFINITIONS ${${LIB}_DEFINITIONS}) + endif() + else() + if("${_is_optional}" STREQUAL "OPTIONAL") + list(APPEND PCL_${COMPONENT}_DEFINITIONS "-DDISABLE_${LIB}") + pcl_message("** WARNING ** ${_component} features related to ${_lib} will be disabled") + elseif("${_is_optional}" STREQUAL "REQUIRED") + if((NOT PCL_FIND_ALL) OR (PCL_FIND_ALL EQUAL 1)) + pcl_report_not_found("${_component} is required but ${_lib} was not found") + elseif(PCL_FIND_ALL EQUAL 0) + # raise error and remove _component from PCL_TO_FIND_COMPONENTS + string(TOUPPER "${_component}" COMPONENT) + pcl_message("** WARNING ** ${_component} will be disabled cause ${_lib} was not found") + list(REMOVE_ITEM PCL_TO_FIND_COMPONENTS ${_component}) + endif() + endif() + endif() +endmacro() + +macro(pcl_check_external_dependency _component) +endmacro() + +#flatten dependencies recursivity is great \o/ +macro(compute_dependencies TO_FIND_COMPONENTS) + foreach(component ${${TO_FIND_COMPONENTS}}) + set(pcl_component pcl_${component}) + if(${pcl_component}_int_dep AND (NOT PCL_FIND_ALL)) + foreach(dependency ${${pcl_component}_int_dep}) + list(FIND ${TO_FIND_COMPONENTS} ${component} pos) + list(FIND ${TO_FIND_COMPONENTS} ${dependency} found) + if(found EQUAL -1) + set(pcl_dependency pcl_${dependency}) + if(${pcl_dependency}_int_dep) + list(INSERT ${TO_FIND_COMPONENTS} ${pos} ${dependency}) + if(pcl_${dependency}_ext_dep) + list(APPEND pcl_${component}_ext_dep ${pcl_${dependency}_ext_dep}) + endif() + if(pcl_${dependency}_opt_dep) + list(APPEND pcl_${component}_opt_dep ${pcl_${dependency}_opt_dep}) + endif() + compute_dependencies(${TO_FIND_COMPONENTS}) + else() + list(INSERT ${TO_FIND_COMPONENTS} 0 ${dependency}) + endif() + endif() + endforeach() + endif() + endforeach() +endmacro() + +### ---[ Find PCL + +if(PCL_FIND_QUIETLY) + set(QUIET_ QUIET) +else() + set(QUIET_) +endif() + +find_package(PkgConfig QUIET) + +file(TO_CMAKE_PATH "${PCL_DIR}" PCL_DIR) +if(WIN32 AND NOT MINGW) +# PCLConfig.cmake is installed to PCL_ROOT/cmake + get_filename_component(PCL_ROOT "${PCL_DIR}" PATH) +else() +# PCLConfig.cmake is installed to PCL_ROOT/share/pcl-x.y + get_filename_component(PCL_ROOT "${CMAKE_CURRENT_LIST_DIR}/../.." ABSOLUTE) +endif() + +# check whether PCLConfig.cmake is found into a PCL installation or in a build tree +if(EXISTS "${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}/pcl/pcl_config.h") + # Found a PCL installation + # pcl_message("Found a PCL installation") + set(PCL_CONF_INCLUDE_DIR "${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}") + set(PCL_LIBRARY_DIRS "${PCL_ROOT}/lib") + if(EXISTS "${PCL_ROOT}/3rdParty") + set(PCL_ALL_IN_ONE_INSTALLER ON) + endif() +elseif(EXISTS "${PCL_ROOT}/include/pcl/pcl_config.h") + # Found a non-standard (likely ANDROID) PCL installation + # pcl_message("Found a PCL installation") + set(PCL_CONF_INCLUDE_DIR "${PCL_ROOT}/include") + set(PCL_LIBRARY_DIRS "${PCL_ROOT}/lib") + if(EXISTS "${PCL_ROOT}/3rdParty") + set(PCL_ALL_IN_ONE_INSTALLER ON) + endif() +elseif(EXISTS "${PCL_DIR}/include/pcl/pcl_config.h") + # Found PCLConfig.cmake in a build tree of PCL + # pcl_message("PCL found into a build tree.") + set(PCL_CONF_INCLUDE_DIR "${PCL_DIR}/include") # for pcl_config.h + set(PCL_LIBRARY_DIRS "${PCL_DIR}/lib") + set(PCL_SOURCES_TREE "/home/solomon/pcl-pcl-1.10.1") +else() + pcl_report_not_found("PCL can not be found on this machine") +endif() + +set(PCL_INCLUDE_DIRS "${PCL_CONF_INCLUDE_DIR}") + +#set a suffix for debug libraries +set(PCL_DEBUG_SUFFIX "") +set(PCL_RELEASE_SUFFIX "") + +#set SSE flags used compiling PCL +list(APPEND PCL_DEFINITIONS ) +list(APPEND PCL_COMPILE_OPTIONS -march=native;-msse4.2;-mfpmath=sse) + +set(pcl_all_components common kdtree octree search sample_consensus filters 2d geometry io features ml segmentation surface registration keypoints tracking recognition stereo) +list(LENGTH pcl_all_components PCL_NB_COMPONENTS) + +#list each component dependencies IN PCL +set(pcl_kdtree_int_dep common ) +set(pcl_octree_int_dep common ) +set(pcl_search_int_dep common kdtree octree ) +set(pcl_sample_consensus_int_dep common search ) +set(pcl_filters_int_dep common sample_consensus search kdtree octree ) +set(pcl_2d_int_dep common filters ) +set(pcl_geometry_int_dep common ) +set(pcl_io_int_dep common octree ) +set(pcl_features_int_dep common search kdtree octree filters 2d ) +set(pcl_ml_int_dep common ) +set(pcl_segmentation_int_dep common geometry search sample_consensus kdtree octree features filters ml ) +set(pcl_surface_int_dep common search kdtree octree ) +set(pcl_registration_int_dep common octree kdtree search sample_consensus features filters ) +set(pcl_keypoints_int_dep common search kdtree octree features filters ) +set(pcl_tracking_int_dep common search kdtree filters octree ) +set(pcl_recognition_int_dep common io search kdtree octree features filters registration sample_consensus ml ) +set(pcl_stereo_int_dep common io ) + + +#list each component external dependencies (ext means mandatory and opt means optional) +set(pcl_common_ext_dep eigen boost ) +set(pcl_kdtree_ext_dep flann ) +set(pcl_search_ext_dep flann ) + + +set(pcl_2d_opt_dep vtk ) +set(pcl_io_opt_dep openni openni2 pcap png vtk libusb-1.0 ) +set(pcl_surface_opt_dep qhull ) + + +# VTK components required by PCL +set(PCL_VTK_COMPONENTS "vtkChartsCore;vtkCommonCore;vtkCommonDataModel;vtkCommonExecutionModel;vtkFiltersCore;vtkFiltersExtraction;vtkFiltersModeling;vtkImagingCore;vtkImagingSources;vtkInteractionStyle;vtkInteractionWidgets;vtkIOCore;vtkIOGeometry;vtkIOImage;vtkIOLegacy;vtkIOPLY;vtkRenderingAnnotation;vtkRenderingLOD;vtkViewsContext2D;vtkRenderingContextOpenGL") + +set(pcl_header_only_components 2d cuda_common geometry gpu_tracking modeler in_hand_scanner point_cloud_editor cloud_composer) + +include(FindPackageHandleStandardArgs) + +#check if user provided a list of components +#if no components at all or full list is given set PCL_FIND_ALL +if(PCL_FIND_COMPONENTS) + list(LENGTH PCL_FIND_COMPONENTS PCL_FIND_COMPONENTS_LENGTH) + if(PCL_FIND_COMPONENTS_LENGTH EQUAL PCL_NB_COMPONENTS) + set(PCL_TO_FIND_COMPONENTS ${pcl_all_components}) + set(PCL_FIND_ALL 1) + else() + set(PCL_TO_FIND_COMPONENTS ${PCL_FIND_COMPONENTS}) + endif() +else() + set(PCL_TO_FIND_COMPONENTS ${pcl_all_components}) + set(PCL_FIND_ALL 1) +endif() + +compute_dependencies(PCL_TO_FIND_COMPONENTS) + +# We do not need to find components that have been found already, e.g. during previous invocation +# of find_package(PCL). Filter them out. +foreach(component ${PCL_TO_FIND_COMPONENTS}) + string(TOUPPER "${component}" COMPONENT) + if(NOT PCL_${COMPONENT}_FOUND) + list(APPEND _PCL_TO_FIND_COMPONENTS ${component}) + endif() +endforeach() +set(PCL_TO_FIND_COMPONENTS ${_PCL_TO_FIND_COMPONENTS}) +unset(_PCL_TO_FIND_COMPONENTS) + +if(NOT PCL_TO_FIND_COMPONENTS) + return() +endif() + +# compute external dependencies per component +foreach(component ${PCL_TO_FIND_COMPONENTS}) + foreach(opt ${pcl_${component}_opt_dep}) + find_external_library(${component} ${opt} OPTIONAL) + endforeach() + foreach(ext ${pcl_${component}_ext_dep}) + find_external_library(${component} ${ext} REQUIRED) + endforeach() +endforeach() + +foreach(component ${PCL_TO_FIND_COMPONENTS}) + set(pcl_component pcl_${component}) + string(TOUPPER "${component}" COMPONENT) + + pcl_message(STATUS "looking for PCL_${COMPONENT}") + + string(REGEX REPLACE "^cuda_(.*)$" "\\1" cuda_component "${component}") + string(REGEX REPLACE "^gpu_(.*)$" "\\1" gpu_component "${component}") + + find_path(PCL_${COMPONENT}_INCLUDE_DIR + NAMES pcl/${component} + pcl/apps/${component} + pcl/cuda/${cuda_component} pcl/cuda/${component} + pcl/gpu/${gpu_component} pcl/gpu/${component} + HINTS ${PCL_INCLUDE_DIRS} + "${PCL_SOURCES_TREE}" + PATH_SUFFIXES + ${component}/include + apps/${component}/include + cuda/${cuda_component}/include + gpu/${gpu_component}/include + DOC "path to ${component} headers" + NO_DEFAULT_PATH) + mark_as_advanced(PCL_${COMPONENT}_INCLUDE_DIR) + + if(PCL_${COMPONENT}_INCLUDE_DIR) + list(APPEND PCL_${COMPONENT}_INCLUDE_DIRS "${PCL_${COMPONENT}_INCLUDE_DIR}") + else() + #pcl_message("No include directory found for pcl_${component}.") + endif() + + # Skip find_library for header only modules + list(FIND pcl_header_only_components ${component} _is_header_only) + if(_is_header_only EQUAL -1) + find_library(PCL_${COMPONENT}_LIBRARY ${pcl_component}${PCL_RELEASE_SUFFIX} + HINTS ${PCL_LIBRARY_DIRS} + DOC "path to ${pcl_component} library" + NO_DEFAULT_PATH) + get_filename_component(${component}_library_path + ${PCL_${COMPONENT}_LIBRARY} + PATH) + mark_as_advanced(PCL_${COMPONENT}_LIBRARY) + + find_library(PCL_${COMPONENT}_LIBRARY_DEBUG ${pcl_component}${PCL_DEBUG_SUFFIX} + HINTS ${PCL_LIBRARY_DIRS} + DOC "path to ${pcl_component} library debug" + NO_DEFAULT_PATH) + mark_as_advanced(PCL_${COMPONENT}_LIBRARY_DEBUG) + + if(PCL_${COMPONENT}_LIBRARY_DEBUG) + get_filename_component(${component}_library_path_debug + ${PCL_${COMPONENT}_LIBRARY_DEBUG} + PATH) + endif() + + # Restrict this to Windows users + if(NOT PCL_${COMPONENT}_LIBRARY AND WIN32) + # might be debug only + set(PCL_${COMPONENT}_LIBRARY ${PCL_${COMPONENT}_LIBRARY_DEBUG}) + endif() + + find_package_handle_standard_args(PCL_${COMPONENT} DEFAULT_MSG + PCL_${COMPONENT}_LIBRARY PCL_${COMPONENT}_INCLUDE_DIR) + else() + find_package_handle_standard_args(PCL_${COMPONENT} DEFAULT_MSG + PCL_${COMPONENT}_INCLUDE_DIR) + endif() + + if(PCL_${COMPONENT}_FOUND) + if(NOT "${PCL_${COMPONENT}_INCLUDE_DIRS}" STREQUAL "") + set(_filtered "") + foreach(_inc ${PCL_${COMPONENT}_INCLUDE_DIRS}) + if(EXISTS ${_inc}) + list(APPEND _filtered "${_inc}") + endif() + endforeach() + list(REMOVE_DUPLICATES _filtered) + set(PCL_${COMPONENT}_INCLUDE_DIRS ${_filtered}) + list(APPEND PCL_INCLUDE_DIRS ${_filtered}) + endif() + mark_as_advanced(PCL_${COMPONENT}_INCLUDE_DIRS) + if(_is_header_only EQUAL -1) + list(APPEND PCL_DEFINITIONS ${PCL_${COMPONENT}_DEFINITIONS}) + list(APPEND PCL_LIBRARY_DIRS ${component_library_path}) + if(PCL_${COMPONENT}_LIBRARY_DEBUG) + list(APPEND PCL_LIBRARY_DIRS ${component_library_path_debug}) + endif() + list(APPEND PCL_COMPONENTS ${pcl_component}) + mark_as_advanced(PCL_${COMPONENT}_LIBRARY PCL_${COMPONENT}_LIBRARY_DEBUG) + endif() + # Append internal dependencies + foreach(int_dep ${pcl_${component}_int_dep}) + string(TOUPPER "${int_dep}" INT_DEP) + if(PCL_${INT_DEP}_FOUND) + list(APPEND PCL_${COMPONENT}_INCLUDE_DIRS ${PCL_${INT_DEP}_INCLUDE_DIRS}) + if(PCL_${INT_DEP}_LIBRARIES) + list(APPEND PCL_${COMPONENT}_LINK_LIBRARIES "${PCL_${INT_DEP}_LIBRARIES}") + endif() + endif() + endforeach() + if(_is_header_only EQUAL -1) + add_library(${pcl_component} SHARED IMPORTED) + if(PCL_${COMPONENT}_LIBRARY_DEBUG) + set_target_properties(${pcl_component} + PROPERTIES + IMPORTED_CONFIGURATIONS "RELEASE;DEBUG" + IMPORTED_LOCATION_RELEASE "${PCL_${COMPONENT}_LIBRARY}" + IMPORTED_LOCATION_DEBUG "${PCL_${COMPONENT}_LIBRARY_DEBUG}" + IMPORTED_IMPLIB_RELEASE "${PCL_${COMPONENT}_LIBRARY}" + IMPORTED_IMPLIB_DEBUG "${PCL_${COMPONENT}_LIBRARY_DEBUG}" + ) + else() + set_target_properties(${pcl_component} + PROPERTIES + IMPORTED_LOCATION "${PCL_${COMPONENT}_LIBRARY}" + IMPORTED_IMPLIB "${PCL_${COMPONENT}_LIBRARY}" + ) + endif() + else() # header-only + add_library(${pcl_component} INTERFACE IMPORTED) + endif() + + foreach(def ${PCL_DEFINITIONS}) + string(REPLACE " " ";" def2 ${def}) + string(REGEX REPLACE "^-D" "" def3 "${def2}") + list(APPEND definitions ${def3}) + endforeach() + if(CMAKE_VERSION VERSION_LESS 3.3) + set_target_properties(${pcl_component} + PROPERTIES + INTERFACE_COMPILE_DEFINITIONS "${definitions}" + INTERFACE_COMPILE_OPTIONS "${PCL_COMPILE_OPTIONS}" + INTERFACE_COMPILE_FEATURES "cxx_std_14" + INTERFACE_INCLUDE_DIRECTORIES "${PCL_${COMPONENT}_INCLUDE_DIRS};${PCL_CONF_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "${PCL_${COMPONENT}_LINK_LIBRARIES}" + ) + elseif(CMAKE_VERSION VERSION_LESS 3.11) + set_target_properties(${pcl_component} + PROPERTIES + INTERFACE_COMPILE_DEFINITIONS "${definitions}" + INTERFACE_COMPILE_OPTIONS "$<$:${PCL_COMPILE_OPTIONS}>" + INTERFACE_COMPILE_FEATURES "cxx_std_14" + INTERFACE_INCLUDE_DIRECTORIES "${PCL_${COMPONENT}_INCLUDE_DIRS};${PCL_CONF_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "${PCL_${COMPONENT}_LINK_LIBRARIES}" + ) + else() + set_target_properties(${pcl_component} + PROPERTIES + INTERFACE_COMPILE_DEFINITIONS "${definitions}" + INTERFACE_COMPILE_OPTIONS "$<$:${PCL_COMPILE_OPTIONS}>" + INTERFACE_COMPILE_FEATURES "cxx_std_14" + INTERFACE_INCLUDE_DIRECTORIES "${PCL_${COMPONENT}_INCLUDE_DIRS};${PCL_CONF_INCLUDE_DIR}" + ) + # If possible, we use target_link_libraries to avoid problems with link-type keywords, + # see https://github.com/PointCloudLibrary/pcl/issues/2989 + # target_link_libraries on imported libraries is supported only since CMake 3.11 + target_link_libraries(${pcl_component} INTERFACE ${PCL_${COMPONENT}_LINK_LIBRARIES}) + endif() + set(PCL_${COMPONENT}_LIBRARIES ${pcl_component}) + endif() +endforeach() + +if(NOT "${PCL_INCLUDE_DIRS}" STREQUAL "") + list(REMOVE_DUPLICATES PCL_INCLUDE_DIRS) +endif() + +if(NOT "${PCL_LIBRARY_DIRS}" STREQUAL "") + list(REMOVE_DUPLICATES PCL_LIBRARY_DIRS) +endif() + +if(NOT "${PCL_DEFINITIONS}" STREQUAL "") + list(REMOVE_DUPLICATES PCL_DEFINITIONS) +endif() + +pcl_remove_duplicate_libraries(PCL_COMPONENTS PCL_LIBRARIES) + +# Add 3rd party libraries, as user code might include our .HPP implementations +list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${OPENNI2_LIBRARIES} ${ENSENSO_LIBRARIES} ${davidSDK_LIBRARIES} ${DSSDK_LIBRARIES} ${RSSDK_LIBRARIES} ${RSSDK2_LIBRARIES} ${VTK_LIBRARIES}) +if (TARGET FLANN::FLANN) + list(APPEND PCL_LIBRARIES FLANN::FLANN) +endif() + +find_package_handle_standard_args(PCL DEFAULT_MSG PCL_LIBRARIES PCL_INCLUDE_DIRS) +mark_as_advanced(PCL_LIBRARIES PCL_INCLUDE_DIRS PCL_LIBRARY_DIRS) diff --git a/deps_install/share/pcl-1.10/PCLConfigVersion.cmake b/deps_install/share/pcl-1.10/PCLConfigVersion.cmake new file mode 100755 index 0000000..a54548d --- /dev/null +++ b/deps_install/share/pcl-1.10/PCLConfigVersion.cmake @@ -0,0 +1,13 @@ +# Check whether the requested PACKAGE_FIND_VERSION is compatible + +set(PACKAGE_VERSION 1.10.1) + +# Check whether the requested PACKAGE_FIND_VERSION is compatible +if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") + set(PACKAGE_VERSION_COMPATIBLE FALSE) +else() + set(PACKAGE_VERSION_COMPATIBLE TRUE) + if("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") + set(PACKAGE_VERSION_EXACT TRUE) + endif() +endif()